From ebbe4d2235452fd77575bc084bb519987e566ea9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Nov 2013 22:43:07 +0100 Subject: initial wip version of launchdetector --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + src/lib/launchdetection/CatapultLaunchMethod.cpp | 85 +++++++++++++++++++ src/lib/launchdetection/CatapultLaunchMethod.h | 71 ++++++++++++++++ src/lib/launchdetection/LaunchDetector.cpp | 94 ++++++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 72 +++++++++++++++++ src/lib/launchdetection/LaunchMethod.h | 54 +++++++++++++ src/lib/launchdetection/launchdetection_params.c | 67 +++++++++++++++ src/lib/launchdetection/module.mk | 40 +++++++++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 61 ++++++++++---- 10 files changed, 529 insertions(+), 17 deletions(-) create mode 100644 src/lib/launchdetection/CatapultLaunchMethod.cpp create mode 100644 src/lib/launchdetection/CatapultLaunchMethod.h create mode 100644 src/lib/launchdetection/LaunchDetector.cpp create mode 100644 src/lib/launchdetection/LaunchDetector.h create mode 100644 src/lib/launchdetection/LaunchMethod.h create mode 100644 src/lib/launchdetection/launchdetection_params.c create mode 100644 src/lib/launchdetection/module.mk diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 306827086..b3ce02f01 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -116,6 +116,7 @@ MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +MODULES += lib/launchdetection # # Demo apps diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 761fb8d9d..17fca68b9 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -113,6 +113,7 @@ MODULES += lib/ecl MODULES += lib/external_lgpl MODULES += lib/geo MODULES += lib/conversion +MODULES += lib/launchdetection # # Demo apps diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp new file mode 100644 index 000000000..0a95b46f6 --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -0,0 +1,85 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation4 and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CatapultLaunchMethod.cpp + * Catpult Launch detection + * + * Authors and acknowledgements in header. + */ + +#include "CatapultLaunchMethod.h" + +CatapultLaunchMethod::CatapultLaunchMethod() : + last_timestamp(0), + integrator(0.0f), + launchDetected(false), + threshold_accel(NULL, "LAUN_CAT_A", false), + threshold_time(NULL, "LAUN_CAT_T", false) +{ + +} + +CatapultLaunchMethod::~CatapultLaunchMethod() { + +} + +void CatapultLaunchMethod::update(float accel_x) +{ + float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f; + last_timestamp = hrt_absolute_time(); + + if (accel_x > threshold_accel.get()) { + integrator += accel_x * dt; + if (integrator > threshold_accel.get() * threshold_time.get()) { + launchDetected = true; + } + + } else { + /* reset integrator */ + integrator = 0.0f; + launchDetected = false; + } + +} + +bool CatapultLaunchMethod::getLaunchDetected() +{ + return launchDetected; +} + +void CatapultLaunchMethod::updateParams() +{ + threshold_accel.update(); + threshold_time.update(); +} diff --git a/src/lib/launchdetection/CatapultLaunchMethod.h b/src/lib/launchdetection/CatapultLaunchMethod.h new file mode 100644 index 000000000..e943f11e9 --- /dev/null +++ b/src/lib/launchdetection/CatapultLaunchMethod.h @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation4 and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file CatapultLaunchMethod.h + * Catpult Launch detection + * + * @author Thomas Gubler + */ + +#ifndef CATAPULTLAUNCHMETHOD_H_ +#define CATAPULTLAUNCHMETHOD_H_ + +#include "LaunchMethod.h" + +#include +#include + +class CatapultLaunchMethod : public LaunchMethod +{ +public: + CatapultLaunchMethod(); + ~CatapultLaunchMethod(); + + void update(float accel_x); + bool getLaunchDetected(); + void updateParams(); + +private: + hrt_abstime last_timestamp; +// float threshold_accel_raw; +// float threshold_time; + float integrator; + bool launchDetected; + + control::BlockParamFloat threshold_accel; + control::BlockParamFloat threshold_time; + +}; + +#endif /* CATAPULTLAUNCHMETHOD_H_ */ diff --git a/src/lib/launchdetection/LaunchDetector.cpp b/src/lib/launchdetection/LaunchDetector.cpp new file mode 100644 index 000000000..2545e0a7e --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.cpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file launchDetection.cpp + * Auto Detection for different launch methods (e.g. catapult) + * + * Authors and acknowledgements in header. + */ + +#include "LaunchDetector.h" +#include "CatapultLaunchMethod.h" +#include + +LaunchDetector::LaunchDetector() : + launchdetection_on(NULL, "LAUN_ALL_ON", false) +{ + /* init all detectors */ + launchMethods[0] = new CatapultLaunchMethod(); + + + /* update all parameters of all detectors */ + updateParams(); +} + +LaunchDetector::~LaunchDetector() +{ + +} + +void LaunchDetector::update(float accel_x) +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + launchMethods[i]->update(accel_x); + } + } +} + +bool LaunchDetector::getLaunchDetected() +{ + if (launchdetection_on.get() == 1) { + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + if(launchMethods[i]->getLaunchDetected()) { + return true; + } + } + } + + return false; +} + +void LaunchDetector::updateParams() { + + warnx(" LaunchDetector::updateParams()"); + //launchdetection_on.update(); + + for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) { + //launchMethods[i]->updateParams(); + warnx("updating component %d", i); + } + + warnx(" LaunchDetector::updateParams() ended"); +} diff --git a/src/lib/launchdetection/LaunchDetector.h b/src/lib/launchdetection/LaunchDetector.h new file mode 100644 index 000000000..981891143 --- /dev/null +++ b/src/lib/launchdetection/LaunchDetector.h @@ -0,0 +1,72 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation4 and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LaunchDetector.h + * Auto Detection for different launch methods (e.g. catapult) + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHDETECTOR_H +#define LAUNCHDETECTOR_H + +#include +#include + +#include "LaunchMethod.h" + +#include + +class __EXPORT LaunchDetector +{ +public: + LaunchDetector(); + ~LaunchDetector(); + + void update(float accel_x); + bool getLaunchDetected(); + void updateParams(); + bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); }; + +// virtual bool getLaunchDetected(); +protected: +private: + LaunchMethod* launchMethods[1]; + control::BlockParamInt launchdetection_on; + + +}; + + +#endif // LAUNCHDETECTOR_H diff --git a/src/lib/launchdetection/LaunchMethod.h b/src/lib/launchdetection/LaunchMethod.h new file mode 100644 index 000000000..bfb5f8cb4 --- /dev/null +++ b/src/lib/launchdetection/LaunchMethod.h @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation4 and/or other materials provided with the + * distribution. + * 3. Neither the name ECL nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file LaunchMethod.h + * Base class for different launch methods + * + * @author Thomas Gubler + */ + +#ifndef LAUNCHMETHOD_H_ +#define LAUNCHMETHOD_H_ + +class LaunchMethod +{ +public: + virtual void update(float accel_x) = 0; + virtual bool getLaunchDetected() = 0; + virtual void updateParams() = 0; +protected: +private: +}; + +#endif /* LAUNCHMETHOD_H_ */ diff --git a/src/lib/launchdetection/launchdetection_params.c b/src/lib/launchdetection/launchdetection_params.c new file mode 100644 index 000000000..536749c88 --- /dev/null +++ b/src/lib/launchdetection/launchdetection_params.c @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * AS IS AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file fw_pos_control_l1_params.c + * + * Parameters defined by the L1 position control task + * + * @author Lorenz Meier + */ + +#include + +#include + +/* + * Launch detection parameters, accessible via MAVLink + * + */ + +/* Catapult Launch detection */ + +// @DisplayName Switch to enable launchdetection +// @Description if set to 1 launchdetection is enabled +// @Range 0 or 1 +PARAM_DEFINE_INT32(LAUN_ALL_ON, 0); + +// @DisplayName Catapult Accelerometer Threshold +// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection +// @Range > 0 +PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f); + +// @DisplayName Catapult Time Threshold +// @Description LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection +// @Range > 0, in seconds +PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f); diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk new file mode 100644 index 000000000..13648b74c --- /dev/null +++ b/src/lib/launchdetection/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Launchdetection Library +# + +SRCS = LaunchDetector.cpp \ + CatapultLaunchMethod.cpp \ + launchdetection_params.c diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index a9648b207..99428ea50 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -86,6 +86,7 @@ #include #include +#include /** * L1 control app start / stop handling function @@ -164,6 +165,9 @@ private: /* heading hold */ float target_bearing; + /* Launch detection */ + LaunchDetector launchDetector; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -338,7 +342,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn(false), + launchDetector() { _nav_capabilities.turn_distance = 0.0f; @@ -464,6 +469,8 @@ FixedwingPositionControl::parameters_update() return 1; } + launchDetector.updateParams(); + return OK; } @@ -818,30 +825,50 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { + /* Perform launch detection */ + bool do_fly_takeoff = false; + warnx("Launch detection running"); + if (launchDetector.launchDetectionEnabled()) { + launchDetector.update(_accel.x); + if (launchDetector.getLaunchDetected()) { + do_fly_takeoff = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + do_fly_takeoff = true; + } + + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (do_fly_takeoff) { - /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ + if (altitude_error > 10.0f) { - /* limit roll motion to ensure enough lift */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); + /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _airspeed.indicated_airspeed_m_s, eas2tas, + true, math::max(math::radians(global_triplet.current.param1), math::radians(10.0f)), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); - } else { + /* limit roll motion to ensure enough lift */ + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } else { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, math::radians(_parameters.pitch_limit_min), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + } + } else { + throttle_max = 0.0f; } } -- cgit v1.2.3 From 52960e06c601cacab90a196803e479dce539cd2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 17:15:38 +0100 Subject: add fw autoland documentation --- Documentation/fw_landing.png | Bin 0 -> 17371 bytes src/modules/fw_pos_control_l1/landingslope.h | 16 ++++++++-------- 2 files changed, 8 insertions(+), 8 deletions(-) create mode 100644 Documentation/fw_landing.png diff --git a/Documentation/fw_landing.png b/Documentation/fw_landing.png new file mode 100644 index 000000000..c1165f16a Binary files /dev/null and b/Documentation/fw_landing.png differ diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 8ff431509..1a149fc7c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -46,16 +46,16 @@ class Landingslope { private: - //xxx: documentation of landing pending - float _landing_slope_angle_rad; - float _flare_relative_alt; + /* see Documentation/fw_landing.png for a plot of the landing slope */ + float _landing_slope_angle_rad; /**< phi in the plot */ + float _flare_relative_alt; /**< h_flare,rel in the plot */ float _motor_lim_horizontal_distance; - float _H1_virt; - float _H0; - float _d1; + float _H1_virt; /**< H1 in the plot */ + float _H0; /**< h_flare,rel + H1 in the plot */ + float _d1; /**< d1 in the plot */ float _flare_constant; - float _flare_length; - float _horizontal_slope_displacement; + float _flare_length; /**< d1 + delta d in the plot */ + float _horizontal_slope_displacement; /**< delta d in the plot */ void calculateSlopeValues(); -- cgit v1.2.3 From 9cc1fc1cb59255c1390f058f03918b0d105c2d26 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 19:08:09 +0100 Subject: fixed launchdetection logic, catapult tested in HIL --- src/lib/launchdetection/CatapultLaunchMethod.cpp | 5 ++ .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 67 ++++++++++++++-------- 2 files changed, 48 insertions(+), 24 deletions(-) diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index 0a95b46f6..d5c759b17 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -39,6 +39,7 @@ */ #include "CatapultLaunchMethod.h" +#include CatapultLaunchMethod::CatapultLaunchMethod() : last_timestamp(0), @@ -61,11 +62,15 @@ void CatapultLaunchMethod::update(float accel_x) if (accel_x > threshold_accel.get()) { integrator += accel_x * dt; +// warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); if (integrator > threshold_accel.get() * threshold_time.get()) { launchDetected = true; } } else { +// warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", +// (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); /* reset integrator */ integrator = 0.0f; launchDetected = false; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 26f6768cc..5056bcdc1 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -132,7 +133,7 @@ private: int _control_mode_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ - int _accel_sub; /**< body frame accelerations */ + int _sensor_combined_sub; /**< for body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -145,7 +146,7 @@ private: struct vehicle_control_mode_s _control_mode; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct mission_item_triplet_s _mission_item_triplet; /**< triplet of mission items */ - struct accel_report _accel; /**< body frame accelerations */ + struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -171,6 +172,10 @@ private: bool land_motor_lim; bool land_onslope; + /* takeoff/launch states */ + bool launch_detected; + bool launch_detection_message_sent; + /* Landingslope object */ Landingslope landingslope; @@ -311,7 +316,7 @@ private: /** * Check for accel updates. */ - void vehicle_accel_poll(); + void vehicle_sensor_combined_poll(); /** * Check for set triplet updates. @@ -389,7 +394,9 @@ FixedwingPositionControl::FixedwingPositionControl() : land_onslope(false), flare_curve_alt_last(0.0f), _mavlink_fd(-1), - launchDetector() + launchDetector(), + launch_detected(false), + launch_detection_message_sent(false) { /* safely initialize structs */ vehicle_attitude_s _att = {0}; @@ -400,7 +407,7 @@ FixedwingPositionControl::FixedwingPositionControl() : vehicle_control_mode_s _control_mode = {0}; vehicle_global_position_s _global_pos = {0}; mission_item_triplet_s _mission_item_triplet = {0}; - accel_report _accel = {0}; + sensor_combined_s _sensor_combined = {0}; @@ -631,14 +638,14 @@ FixedwingPositionControl::vehicle_attitude_poll() } void -FixedwingPositionControl::vehicle_accel_poll() +FixedwingPositionControl::vehicle_sensor_combined_poll() { /* check if there is a new position */ - bool accel_updated; - orb_check(_accel_sub, &accel_updated); + bool sensors_updated; + orb_check(_sensor_combined_sub, &sensors_updated); - if (accel_updated) { - orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); + if (sensors_updated) { + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); } } @@ -756,7 +763,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float baro_altitude = _global_pos.alt; /* filter speed and altitude for controller */ - math::Vector3 accel_body(_accel.x, _accel.y, _accel.z); + math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); math::Vector3 accel_earth = _R_nb.transpose() * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); @@ -973,24 +980,30 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* Perform launch detection */ - bool do_fly_takeoff = false; - warnx("Launch detection running"); - if (launchDetector.launchDetectionEnabled()) { - launchDetector.update(_accel.x); - if (launchDetector.getLaunchDetected()) { - do_fly_takeoff = true; - warnx("Launch detected. Taking off!"); +// warnx("Launch detection running"); + if(!launch_detected) { //do not do further checks once a launch was detected + if (launchDetector.launchDetectionEnabled()) { +// warnx("Launch detection enabled"); + if(!launch_detection_message_sent) { + mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + launch_detection_message_sent = true; + } + launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); + if (launchDetector.getLaunchDetected()) { + launch_detected = true; + warnx("Launch detected. Taking off!"); + } + } else { + /* no takeoff detection --> fly */ + launch_detected = true; } - } else { - /* no takeoff detection --> fly */ - do_fly_takeoff = true; } _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - if (do_fly_takeoff) { + if (launch_detected) { /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ if (altitude_error > 15.0f) { @@ -1037,6 +1050,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio land_onslope = false; } + /* reset takeoff/launch state */ + if (mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF) { + launch_detected = false; + launch_detection_message_sent = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; @@ -1151,7 +1170,7 @@ FixedwingPositionControl::task_main() _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); _mission_item_triplet_sub = orb_subscribe(ORB_ID(mission_item_triplet)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -1233,7 +1252,7 @@ FixedwingPositionControl::task_main() vehicle_attitude_poll(); vehicle_setpoint_poll(); - vehicle_accel_poll(); + vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); // vehicle_baro_poll(); -- cgit v1.2.3 From 080ecf56caac571f905f657875cec9fa8e7e17d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 22:21:53 +0100 Subject: launchdetection: add mavlink text output --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 5056bcdc1..455fe3674 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -991,7 +991,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio launchDetector.update(_sensor_combined.accelerometer_m_s2[0]); if (launchDetector.getLaunchDetected()) { launch_detected = true; - warnx("Launch detected. Taking off!"); + mavlink_log_info(_mavlink_fd, "#audio: Takeoff"); } } else { /* no takeoff detection --> fly */ -- cgit v1.2.3 From ec8bc6c0208a5eb9ad3decf7bec196c5bf113dd4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 16:33:50 +0100 Subject: fw pos ctrl: remove a wrong transpose --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 455fe3674..bbb205b2f 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -764,7 +764,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* filter speed and altitude for controller */ math::Vector3 accel_body(_sensor_combined.accelerometer_m_s2[0], _sensor_combined.accelerometer_m_s2[1], _sensor_combined.accelerometer_m_s2[2]); - math::Vector3 accel_earth = _R_nb.transpose() * accel_body; + math::Vector3 accel_earth = _R_nb * accel_body; _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _mission_item_triplet.current.altitude - _global_pos.alt; -- cgit v1.2.3 From 4191ae33c264459f0a85d9c03b8cb4893c6ee33e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 21:54:33 +0100 Subject: navigator/mission feasibility: prepare for pre-mission fence checking --- .../navigator/mission_feasibility_checker.cpp | 21 +++++++++++---------- src/modules/navigator/mission_feasibility_checker.h | 10 +++++----- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 25b2636bb..cc079dee1 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -61,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Init if not done yet */ init(); @@ -74,39 +75,39 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nItems); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); else - return checkMissionFeasibleFixedwing(dm_current, nItems); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { - return checkGeofence(dm_current, nItems); + return checkGeofence(dm_current, nMissionItems, fence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nItems) && checkGeofence(dm_current, nItems)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) { //xxx: check geofence return true; } -bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nItems) +bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint * if landing waypoint is found: the previous waypoint is checked to be at a feasible distance and altitude given the landing slope */ - for (size_t i = 0; i < nItems; i++) { + for (size_t i = 0; i < nMissionItems; i++) { static struct mission_item_s missionitem; const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7d1cc2f8a..ef235ead4 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,15 +57,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nItems); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nItems); - bool checkFixedWingLanding(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nItems); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); public: MissionFeasibilityChecker(); @@ -74,7 +74,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nItems); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefcc..c88f237ad 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -456,7 +456,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); -- cgit v1.2.3 From dca6d97a5288766e3e0da05dc5fdc98108fa7892 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 14:18:02 +0100 Subject: create geofence class and start moving fence functionality to this class --- src/lib/geo/geo.c | 24 ----------- src/lib/geo/geo.h | 11 ----- src/modules/navigator/geofence.cpp | 73 ++++++++++++++++++++++++++++++++ src/modules/navigator/geofence.h | 64 ++++++++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator_main.cpp | 2 + 6 files changed, 141 insertions(+), 36 deletions(-) create mode 100644 src/modules/navigator/geofence.cpp create mode 100644 src/modules/navigator/geofence.h diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index f64bfb41a..08fe2b696 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -503,27 +503,3 @@ __EXPORT float _wrap_360(float bearing) return bearing; } - -__EXPORT bool inside_geofence(const struct vehicle_global_position_s *vehicle, const struct fence_s *fence) -{ - - /* Adaptation of algorithm originally presented as - * PNPOLY - Point Inclusion in Polygon Test - * W. Randolph Franklin (WRF) */ - - unsigned int i, j, vertices = fence->count; - bool c = false; - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - - // skip vertex 0 (return point) - for (i = 0, j = vertices - 1; i < vertices; j = i++) - if (((fence->vertices[i].lon) >= lon != (fence->vertices[j].lon >= lon)) && - (lat <= (fence->vertices[j].lat - fence->vertices[i].lat) * (lon - fence->vertices[i].lon) / - (fence->vertices[j].lon - fence->vertices[i].lon) + fence->vertices[i].lat)) - c = !c; - return c; -} - - - diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 5f92e14cf..5f4bce698 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -143,15 +143,4 @@ __EXPORT float _wrap_360(float bearing); __EXPORT float _wrap_pi(float bearing); __EXPORT float _wrap_2pi(float bearing); -/** - * Return whether craft is inside geofence. - * - * Calculate whether point is inside arbitrary polygon - * @param craft pointer craft coordinates - * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected - * @return true: craft is inside fence, false:craft is outside fence - */ -__EXPORT bool inside_geofence(const struct vehicle_global_position_s *craft, const struct fence_s *fence); - - __END_DECLS diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp new file mode 100644 index 000000000..bc5ef44f2 --- /dev/null +++ b/src/modules/navigator/geofence.cpp @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr + * @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file geofence.cpp + * Provides functions for handling the geofence + */ +#include "geofence.h" + +#include + +Geofence::Geofence() +{ + +} + +Geofence::~Geofence() +{ + +} + + +bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +{ + + /* Adaptation of algorithm originally presented as + * PNPOLY - Point Inclusion in Polygon Test + * W. Randolph Franklin (WRF) */ + + unsigned int i, j, vertices = _fence.count; + bool c = false; + double lat = vehicle->lat / 1e7d; + double lon = vehicle->lon / 1e7d; + + // skip vertex 0 (return point) + for (i = 0, j = vertices - 1; i < vertices; j = i++) + if (((_fence.vertices[i].lon) >= lon != (_fence.vertices[j].lon >= lon)) && + (lat <= (_fence.vertices[j].lat - _fence.vertices[i].lat) * (lon - _fence.vertices[i].lon) / + (_fence.vertices[j].lon - _fence.vertices[i].lon) + _fence.vertices[i].lat)) + c = !c; + return c; +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h new file mode 100644 index 000000000..6d6e6e87c --- /dev/null +++ b/src/modules/navigator/geofence.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Jean Cyr + * @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +/** + * @file geofence.h + * Provides functions for handling the geofence + */ + +#ifndef GEOFENCE_H_ +#define GEOFENCE_H_ + +#include + +class Geofence { +private: + struct fence_s _fence; +public: + Geofence(); + ~Geofence(); + + /** + * Return whether craft is inside geofence. + * + * Calculate whether point is inside arbitrary polygon + * @param craft pointer craft coordinates + * @param fence pointer to array of coordinates, one per vertex. First and last vertex are assumed connected + * @return true: craft is inside fence, false:craft is outside fence + */ + bool inside(const struct vehicle_global_position_s *craft); +}; + + +#endif /* GEOFENCE_H_ */ diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 6be4e87a0..b7900e84e 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -40,6 +40,7 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ navigator_mission.cpp \ - mission_feasibility_checker.cpp + mission_feasibility_checker.cpp \ + geofence.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c88f237ad..a9547355f 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -78,6 +78,7 @@ #include "navigator_mission.h" #include "mission_feasibility_checker.h" +#include "geofence.h" /* oddly, ERROR is not defined for c++ */ @@ -157,6 +158,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ + Geofence geofence; struct fence_s _fence; /**< storage for fence vertices */ bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ -- cgit v1.2.3 From 429a11a21d25e34ca711b2c0debb2ac3e84c45ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 15:01:08 +0100 Subject: navigator/geofence: move more functions to geofence class (WIP) --- src/modules/navigator/geofence.cpp | 97 +++++++++++++++- src/modules/navigator/geofence.h | 16 +++ .../navigator/mission_feasibility_checker.cpp | 16 +-- .../navigator/mission_feasibility_checker.h | 9 +- src/modules/navigator/navigator_main.cpp | 127 +++------------------ 5 files changed, 138 insertions(+), 127 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc5ef44f2..4a0528b16 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -39,10 +39,14 @@ #include "geofence.h" #include +#include +#include +#include +#include -Geofence::Geofence() +Geofence::Geofence() : _fence_pub(-1) { - + memset(&_fence, 0, sizeof(_fence)); } Geofence::~Geofence() @@ -71,3 +75,92 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) c = !c; return c; } + +bool +Geofence::load(unsigned vertices) +{ + struct fence_s temp_fence; + + unsigned i; + for (i = 0; i < vertices; i++) { + if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { + break; + } + } + + temp_fence.count = i; + + if (valid()) + memcpy(&_fence, &temp_fence, sizeof(_fence)); + else + warnx("Invalid fence file, ignored!"); + + return _fence.count != 0; +} + +bool +Geofence::valid() +{ + // NULL fence is valid + if (_fence.count == 0) { + return true; + } + + // Otherwise + if ((_fence.count < 4) || (_fence.count > GEOFENCE_MAX_VERTICES)) { + warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); + return false; + } + + return true; +} + +void +Geofence::addPoint(int argc, char *argv[]) +{ + int ix, last; + double lon, lat; + struct fence_vertex_s vertex; + char *end; + + if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { + dm_clear(DM_KEY_FENCE_POINTS); + publishFence(0); + return; + } + + if (argc < 3) + errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); + + ix = atoi(argv[0]); + if (ix >= DM_KEY_FENCE_POINTS_MAX) + errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); + + lat = strtod(argv[1], &end); + lon = strtod(argv[2], &end); + + last = 0; + if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) + last = 1; + + vertex.lat = (float)lat; + vertex.lon = (float)lon; + + if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { + if (last) + publishFence((unsigned)ix + 1); + return; + } + + errx(1, "can't store fence point"); +} + +void +Geofence::publishFence(unsigned vertices) +{ + if (_fence_pub == -1) + _fence_pub = orb_advertise(ORB_ID(fence), &vertices); + else + orb_publish(ORB_ID(fence), _fence_pub, &vertices); +} + diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 6d6e6e87c..8f3a07b02 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -45,6 +45,7 @@ class Geofence { private: struct fence_s _fence; + orb_advert_t _fence_pub; /**< publish fence topic */ public: Geofence(); ~Geofence(); @@ -58,6 +59,21 @@ public: * @return true: craft is inside fence, false:craft is outside fence */ bool inside(const struct vehicle_global_position_s *craft); + + + /** + * Load fence parameters. + */ + bool load(unsigned vertices); + + bool valid(); + + /** + * Specify fence vertex position. + */ + void addPoint(int argc, char *argv[]); + + void publishFence(unsigned vertices); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index cc079dee1..aba2dffff 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* Init if not done yet */ init(); @@ -75,27 +75,27 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, fence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, fence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { - return checkGeofence(dm_current, nMissionItems, fence); + return checkGeofence(dm_current, nMissionItems, geofence); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, fence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); } -bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence) +bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) { //xxx: check geofence return true; diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index ef235ead4..7a0b2a296 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -43,6 +43,7 @@ #include #include #include +#include "geofence.h" class MissionFeasibilityChecker @@ -57,15 +58,15 @@ private: void init(); /* Checks for all airframes */ - bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); public: MissionFeasibilityChecker(); @@ -74,7 +75,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, const struct fence_s &fence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index a9547355f..354fa733b 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -120,14 +120,9 @@ public: void status(); /** - * Load fence parameters. + * Add point to geofence */ - bool load_fence(unsigned vertices); - - /** - * Specify fence vertex position. - */ - void fence_point(int argc, char *argv[]); + void add_fence_point(int argc, char *argv[]); private: @@ -145,7 +140,6 @@ private: int _capabilities_sub; /**< notification of vehicle capabilities updates */ orb_advert_t _triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _fence_pub; /**< publish fence topic */ orb_advert_t _mission_result_pub; /**< publish mission result topic */ orb_advert_t _control_mode_pub; /**< publish vehicle control mode topic */ @@ -158,8 +152,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - Geofence geofence; - struct fence_s _fence; /**< storage for fence vertices */ + Geofence _geofence; bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ @@ -252,12 +245,8 @@ private: */ void task_main() __attribute__((noreturn)); - void publish_fence(unsigned vertices); - void publish_safepoints(unsigned points); - bool fence_valid(const struct fence_s &fence); - /** * Functions that are triggered when a new state is entered. */ @@ -347,7 +336,6 @@ Navigator::Navigator() : /* publications */ _triplet_pub(-1), - _fence_pub(-1), _mission_result_pub(-1), _control_mode_pub(-1), @@ -365,8 +353,6 @@ Navigator::Navigator() : _time_first_inside_orbit(0), _set_nav_state_timestamp(0) { - memset(&_fence, 0, sizeof(_fence)); - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); @@ -458,7 +444,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _fence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); @@ -511,7 +497,7 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = load_fence(GEOFENCE_MAX_VERTICES); + _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); /* * do subscriptions @@ -782,9 +768,9 @@ Navigator::status() } if (_fence_valid) { warnx("Geofence is valid"); - warnx("Vertex longitude latitude"); - for (unsigned i = 0; i < _fence.count; i++) - warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); +// warnx("Vertex longitude latitude"); +// for (unsigned i = 0; i < _fence.count; i++) +// warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { warnx("Geofence not set"); } @@ -808,96 +794,6 @@ Navigator::status() } } -void -Navigator::publish_fence(unsigned vertices) -{ - if (_fence_pub == -1) - _fence_pub = orb_advertise(ORB_ID(fence), &vertices); - else - orb_publish(ORB_ID(fence), _fence_pub, &vertices); -} - -bool -Navigator::fence_valid(const struct fence_s &fence) -{ - // NULL fence is valid - if (fence.count == 0) { - return true; - } - - // Otherwise - if ((fence.count < 4) || (fence.count > GEOFENCE_MAX_VERTICES)) { - warnx("Fence must have at least 3 sides and not more than %d", GEOFENCE_MAX_VERTICES - 1); - return false; - } - - return true; -} - -bool -Navigator::load_fence(unsigned vertices) -{ - struct fence_s temp_fence; - - unsigned i; - for (i = 0; i < vertices; i++) { - if (dm_read(DM_KEY_FENCE_POINTS, i, temp_fence.vertices + i, sizeof(struct fence_vertex_s)) != sizeof(struct fence_vertex_s)) { - break; - } - } - - temp_fence.count = i; - - if (fence_valid(temp_fence)) - memcpy(&_fence, &temp_fence, sizeof(_fence)); - else - warnx("Invalid fence file, ignored!"); - - return _fence.count != 0; -} - -void -Navigator::fence_point(int argc, char *argv[]) -{ - int ix, last; - double lon, lat; - struct fence_vertex_s vertex; - char *end; - - if ((argc == 1) && (strcmp("-clear", argv[0]) == 0)) { - dm_clear(DM_KEY_FENCE_POINTS); - publish_fence(0); - return; - } - - if (argc < 3) - errx(1, "Specify: -clear | sequence latitude longitude [-publish]"); - - ix = atoi(argv[0]); - if (ix >= DM_KEY_FENCE_POINTS_MAX) - errx(1, "Sequence must be less than %d", DM_KEY_FENCE_POINTS_MAX); - - lat = strtod(argv[1], &end); - lon = strtod(argv[2], &end); - - last = 0; - if ((argc > 3) && (strcmp(argv[3], "-publish") == 0)) - last = 1; - - vertex.lat = (float)lat; - vertex.lon = (float)lon; - - if (dm_write(DM_KEY_FENCE_POINTS, ix, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) == sizeof(vertex)) { - if (last) - publish_fence((unsigned)ix + 1); - return; - } - - errx(1, "can't store fence point"); -} - - - StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { { /* STATE_NONE */ @@ -1351,6 +1247,11 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const } } +void Navigator::add_fence_point(int argc, char *argv[]) +{ + _geofence.addPoint(argc, argv); +} + static void usage() { @@ -1395,7 +1296,7 @@ int navigator_main(int argc, char *argv[]) navigator::g_navigator->status(); } else if (!strcmp(argv[1], "fence")) { - navigator::g_navigator->fence_point(argc - 2, argv + 2); + navigator::g_navigator->add_fence_point(argc - 2, argv + 2); } else { usage(); -- cgit v1.2.3 From a48264d5d44018412b9443b245e6974d3f54b20d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 13:37:49 +0100 Subject: navigator: load geofence from textfile --- src/modules/navigator/geofence.cpp | 101 ++++++++++++++++++++++++++++++- src/modules/navigator/geofence.h | 13 +++- src/modules/navigator/navigator_main.cpp | 31 +++++++++- 3 files changed, 140 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a0528b16..199ccb41b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -43,8 +43,21 @@ #include #include #include +#include +#include +#include +#include -Geofence::Geofence() : _fence_pub(-1) + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +Geofence::Geofence() : _fence_pub(-1), + _altitude_min(0), + _altitude_max(0) { memset(&_fence, 0, sizeof(_fence)); } @@ -77,7 +90,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) } bool -Geofence::load(unsigned vertices) +Geofence::loadFromDm(unsigned vertices) { struct fence_s temp_fence; @@ -164,3 +177,87 @@ Geofence::publishFence(unsigned vertices) orb_publish(ORB_ID(fence), _fence_pub, &vertices); } +int +Geofence::loadFromFile(const char *filename) +{ + FILE *fp; + char line[120]; + int pointCounter = 0; + bool gotVertical = false; + const char commentChar = '#'; + + /* Make sure no data is left in the datamanager */ + clearDm(); + + /* open the mixer definition file */ + fp = fopen(GEOFENCE_FILENAME, "r"); + if (fp == NULL) { + return ERROR; + } + + /* create geofence points from valid lines and store in DM */ + for (;;) { + + /* get a line, bail on error/EOF */ + if (fgets(line, sizeof(line), fp) == NULL) + break; + + /* Trim leading whitespace */ + size_t textStart = 0; + while((textStart < sizeof(line)/sizeof(char)) && isspace(line[textStart])) textStart++; + + /* if the line starts with #, skip */ + if (line[textStart] == commentChar) + continue; + + if (gotVertical) { + /* Parse the line as a geofence point */ + struct fence_vertex_s vertex; + + if (sscanf(line, "%f %f", &(vertex.lat), &(vertex.lon)) != 2) + return ERROR; + + + if (dm_write(DM_KEY_FENCE_POINTS, pointCounter, DM_PERSIST_POWER_ON_RESET, &vertex, sizeof(vertex)) != sizeof(vertex)) + return ERROR; + + warnx("Geofence: point: %d, lat %.5f: lon: %.5f", pointCounter, (double)vertex.lat, (double)vertex.lon); + + pointCounter++; + } else { + /* Parse the line as the vertical limits */ + if (sscanf(line, "%f %f", &_altitude_min, &_altitude_max) != 2) + return ERROR; + + + warnx("Geofence: alt min: %.4f, alt_max: %.4f", (double)_altitude_min, (double)_altitude_max); + gotVertical = true; + } + + + } + + fclose(fp); + + /* Re-Load imported geofence from DM */ + if(gotVertical && pointCounter > 0) + { + bool fence_valid = loadFromDm(GEOFENCE_MAX_VERTICES); + if (fence_valid) { + warnx("Geofence: imported and loaded successfully"); + return OK; + } else { + warnx("Geofence: datamanager read error"); + return ERROR; + } + } else { + warnx("Geofence: import error"); + } + + return ERROR; +} + +int Geofence::clearDm() +{ + dm_clear(DM_KEY_FENCE_POINTS); +} diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 8f3a07b02..8a1d06e71 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,10 +42,15 @@ #include +#define GEOFENCE_FILENAME "/fs/microsd/etc/geofence.txt" + class Geofence { private: - struct fence_s _fence; + struct fence_s _fence; orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; public: Geofence(); ~Geofence(); @@ -64,7 +69,9 @@ public: /** * Load fence parameters. */ - bool load(unsigned vertices); + bool loadFromDm(unsigned vertices); + + int clearDm(); bool valid(); @@ -74,6 +81,8 @@ public: void addPoint(int argc, char *argv[]); void publishFence(unsigned vertices); + + int loadFromFile(const char *filename); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 354fa733b..a226aac7c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -75,6 +75,8 @@ #include #include #include +#include +#include #include "navigator_mission.h" #include "mission_feasibility_checker.h" @@ -124,6 +126,11 @@ public: */ void add_fence_point(int argc, char *argv[]); + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -497,7 +504,22 @@ Navigator::task_main() mavlink_log_info(_mavlink_fd, "[navigator] started"); - _fence_valid = _geofence.load(GEOFENCE_MAX_VERTICES); + _fence_valid = _geofence.loadFromDm(GEOFENCE_MAX_VERTICES); + + /* Try to load the geofence: + * if /fs/microsd/etc/geofence.txt load from this file + * else clear geofence data in datamanager + */ + struct stat buffer; + if( stat (GEOFENCE_FILENAME, &buffer) == 0 ) { + warnx("Try to load geofence.txt"); + _geofence.loadFromFile(GEOFENCE_FILENAME); + } else { + if (_geofence.clearDm() > 0 ) + warnx("Geofence cleared"); + else + warnx("Could not clear geofence"); + } /* * do subscriptions @@ -1252,6 +1274,11 @@ void Navigator::add_fence_point(int argc, char *argv[]) _geofence.addPoint(argc, argv); } +void Navigator::load_fence_from_file(const char *filename) +{ + _geofence.loadFromFile(filename); +} + static void usage() { @@ -1297,6 +1324,8 @@ int navigator_main(int argc, char *argv[]) } else if (!strcmp(argv[1], "fence")) { navigator::g_navigator->add_fence_point(argc - 2, argv + 2); + } else if (!strcmp(argv[1], "fencefile")) { + navigator::g_navigator->load_fence_from_file(GEOFENCE_FILENAME); } else { usage(); -- cgit v1.2.3