From ebbe4d2235452fd77575bc084bb519987e566ea9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Nov 2013 22:43:07 +0100 Subject: [PATCH 1/9] initial wip version of launchdetector --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 1 + .../launchdetection/CatapultLaunchMethod.cpp | 85 +++++++++++++++++ .../launchdetection/CatapultLaunchMethod.h | 71 ++++++++++++++ src/lib/launchdetection/LaunchDetector.cpp | 94 +++++++++++++++++++ src/lib/launchdetection/LaunchDetector.h | 72 ++++++++++++++ src/lib/launchdetection/LaunchMethod.h | 54 +++++++++++ .../launchdetection/launchdetection_params.c | 67 +++++++++++++ src/lib/launchdetection/module.mk | 40 ++++++++ .../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 3068270865..b3ce02f017 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 761fb8d9d6..17fca68b99 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 0000000000..0a95b46f60 --- /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 0000000000..e943f11e9d --- /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 0000000000..2545e0a7e2 --- /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 0000000000..9818911437 --- /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 0000000000..bfb5f8cb4c --- /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 0000000000..536749c88c --- /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 0000000000..13648b74ca --- /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 a9648b207d..99428ea50a 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)); + /* 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)); + + } 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 { - - _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)); + throttle_max = 0.0f; } } From 52960e06c601cacab90a196803e479dce539cd2b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 29 Dec 2013 17:15:38 +0100 Subject: [PATCH 2/9] 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 0000000000000000000000000000000000000000..c1165f16a6e6501ce200f611803d3582070189c5 GIT binary patch literal 17371 zcmd43Rd5_l(5@+2wwPrxGc&U-)`&;U7Be$j7Be$5Gcz+YGcz-7%=e$Odv12G_o5@Z zBc{50YO*Td%zSb}5(L=@%PruF@uN-8WZ;3Yy58 z{?o|*5@f#(5Ps_y0ZP<#YT}c!sY-3aj*ZQfLrlw*r7Dy?Xq}0vYWgwEWt0Yh-)lq~ zkDi8Jj8daTm_`g^-yF0Zm`x__v(i&qj?^dbj^lx_#Qw-HsVOxO$U@M1p0V)$$U+h< z$U@M>Bfp@DzyEhl^xx?py>l>~z_c{np(sL5XDlITJzy%kg_wkdL~B)w;mG#$%^p>O z^eeO3ELq+V(G9L(Wk#=Km}@i3f_|r%}<+ght}?+TZR=f>Fq2O3&nqx_x~-v9601 z&FSq|2O#(E8$NEN(5}IMgMhGeax&`v3%=grv9!DcP5d{OTq<|&WMA)FR4lnv=k>#c z)5xFUXkrmDG5hmnW=kt8RyyKy9%t$xQZy2uu#gaDcN;7>!wVENw7cFV&ZP|#`9@3i zk1r@hJc*%5{5kEa0VWfgRFCOAJ%iPTn-PZx7a{1IQXwHBmiDS`vla$ytu|)_O-)U( z#Y*k8!XgE8sH3luHNo)1GyenI9Po)$U)iL%7+7MF%fo|%gu|fQ{gF6Ut)*gw;m*#^ z^UaPl%_eI-1B1T(kvJF_n4RSsgJVY?8r912r!AkThzOa~3n1seajmWhB;g>1eMxqV zx5->DQYeG;P-M4E$8{!rZ0)k~%A6Sww2ts1x#O8Q%6s_qk-#-wAfp?W#TKDlKy5%c zRt~b8oV6UJ$yDY+hRf#8Y=M-Th6an{vAn>?y`n*o(ND%WczbP#+cB#5Rf}?5)=UGx zD7&F^_=G}kX2(THR5XVxTvcd2wYeg>BBQay^)8<**$keXIQKowSjyPnp{tMREldZP z|1`e3d&~By*mEJeEW7@kSNkJ#Z{Hq_adC5lAA2mc7bk5nFb*q@5DoZfx}n_-z^rn! z=M6=^&U@RCV1Bt9f(ESQi-uwN`};FlE;E&vmrF=UOm|_Rp$P{e;!|lh(veFg<{Jz} zYIV3Pf3+qhVG{bhGe_WZ7D~jBh=_^Z9%cFL?(GSe01Jv0@*vApDp&^14vP6w;K6k2 zsJd_U_PAJgxnKsSfFEfZ_pgrpQ3MB5<%}XajSfe$eEj?{u&^W?9Esn4z#i)=0+|1F zgMGf9F@g?QbejHZy}nt?F2fO2@An&V0zTJYCBTB~o0}Y$tIhYjDZz5JntwUMLH38^ zDd_?gcnKzPzhT8Sc_%$%SrtcGNjFzWI__AQLiPl>3G2S z0f&)qI+Oc;F#gV=1LxEf?R~qThL{ zZ?nV0Y!#A5rM&p->tHO2+x41gadGi=sMct#*={dT4`R&dR)z(P>W|J}8;_`i*=7T! zVlRTL23a=z`pB`Qf@C5Qow(UF)4_1;bY7Rp>T0ITjkcl7jkYW^D%p*@VqgJ)&0;Y; zIT`DNl!GHNJ3AXsFep8~D)x6Mfc%_`%lXII>(r6-%a;OuHvT8tv92ctS@HgnyzjmGPFYZ!xt_@VLk_IJ5=yyM+MsNCI?K`x}_3(Z*?iGEj;YzhGRMY zOw_UP(m`Yp5RwvrKZBhso~;(Jr`JTD7XrI?lA=_q3~PwgB9)XQ-HsA0Ox4HBreg0L zffYWN!asZ_j-9o!c*VqLa#y@fiS6+vAq$0Le{;Zu!0Ju&_z;M!#Z(hV%ZaO)bc0pb zkJljkGx1157Lsm7YNT$opLYafd51IGG*IjaMkKo*`^?*W47WN3cKsHECJz4%fMlfN z;Yh8~9ICz0q6&EQjL^1J?R*7__XUcueP3!NgeE_Z`U^Mkt{jWE($DKojCkb{Aeq9MAb{TJM?dKm7gew|&(WfuDga z)Q%+yS=Bwe9;<*LZf8F%Nwe4r=y}R$R(D5l&_*{H?7mnVjFbm$slvh~F#~B4LPA2b z@a`r6GHk5B6$$i$T@S2dn>xJ%p;_`4sYt3>?;&}lp0_algE+iG#J5F%|AM{VpWFf7 zuJ7;JmYYSqy}hNAnRXA?NQO3i0qA<0~iemvu(sD!06U^}j$Vl|gQKLfF}X z9vq|Loq2d`77GtQKD!$9TS6V(aLi<^8W;#6X-{WRulu zmF@ieoJyq}i`{(w&s?F5bP7vA0U2>`)c>L}^U0hs3(P_n=E;rncGF&oW8onNubPWs)gkNqC)SGRI!R`(x3|4B5 zq_YI@X=!OUJ?|#Yzw+9ljs8hfpYtnt#E)rVF4!wbH8_r$E!39)+0CgQ&Z+?9l(@V+ z#I+2ZK9`^GcXG-#`l3!3tLz2wz$8Xp6--7QC2(-?Qn@Svvqh`L@)Y-vhoyy+xgupC zFcu&oA>Z}&*#_EHfPj*XHkY#A0JyR|@hF;t_|*oBq}sYVMF91~{s@|K36PxKrS2P9jw|94QRcRS4ttu(n z!f<|?~j z+y4zn`EkYW8s(C{wc~d|$g~TT>C|F_xMSVG=GO>nXjq(H&kZ2^p|`4YFSRaTs!}n^ zx#cPR>lYXb3QE28Mru{pCr{r)*5wTv9^Ux=NF2A%C*R1(2%r0X@M?pFV4b$3BO5x6 z(y*g$0U)7w{Co@NxiHYLH-?i=e`u2`lhYQaC*je=XhzY^Q&5g@ z&lntVVvgS56}+>v(?2lK{^i!lkq4j9^ZIuxF84>`DBB-bEq>CeAv-uYSXx>JhlaWzhVeh#JwLnd z?(I=)x3U=<8-Is_s&hJ9bc~v-))O9|m`GtYK@15ADc9}F6b?c(;+Dn5#bwZLy|<@K znIOZ)3JFO0(RYS)G#{<_es@f$)mbI!%R|fMAhvS`$L}GO#(|7sd@d0%{E^A$)*D3V zEs?@vRNc^Uuvno9T65MLt>799G9B&@_h&_)0oF!F1LrGsCz~C-+9>)jg7{K<0xcMrf9WY1GaO_^U1)Utu zBUFj%w09kNp^jg&dU9#C7c1jG9D`SA{5CDF$wL=PX33-Q`}}w_Ffi~73k&mJ$>4PX zDIxcV(|YrT&^DJV9A=?CR!-KTt-h?_9F+e40eRM93Mo0df?wbRK3{N0lRdmwf3vcj zvw9~pSkQ}*cL%W9vG328q9)Qfi_|%sPL(`8Jwf(Iui0pLI$t6d%HTq+nNKI!@q*Un z{FZ?2Gu~|xgIQ#qnfnVIiwSHpbKw)8<$Efd??f6oH8gSU!eEUL(RN|jfW z#YIC+tzhwahM6#T7Y?4LGN?z;tROk9*QcAt*Or%U_XZJ2#pBW@s$mcpYa=sod^Pq^{pk9DR4nYS z&}6d4yFY=b>M-vA8z31Q+cS1^dz&klB><1bL^KqcVqYO7Bm@luGsBq0=Vm{+bbTN` z&JjL5EOBstUhZ>wd|XTz$H&8?!Og=XP$$Cx&ATKi4N!9EJ1k|H8!L|KAzj!Qa+zz- zOfOfG2nJfMQYq*B1L}Ge%zwOz!Kx&ay{vcOF)}i;`+UCR@f(XR(bQQapOuF2IGy(A ziiUxy)tjPZ@Vao=ixl>(kEavcz!U46ey_Qy0Zq@;kk8^5?DR>tIx93{U<7Uhb5yC4 zZwOigsvq);Xs$GSGD98(gs!eG4!e2(N}WlGS`7&t2Hl_c=Nrql#)#8}vO0$&*}*76 zn{i|r*%T6^=bE{waxWA5I>gD`litGgW;>Dzp3BQ>`vMM@(}y$p4xVs47}ZKFYExo( zqP!C-VIiUFx;lf``;+C>RhB>9V7q&J*xdGH=rl^%LIJShIP9U%&#qt?3|hfOMJB0U zX(=fu;ZrYmEwdVhDhEs-EkeJ{gnD154^WLsm2d z-XU@d^3p3rucA?D#yl}goEzx{h0rq91RPeel34PD+dIY6@f18nrDXs0_m(kBp3&%N zWFtMjoWUrb5pq;3TvyY zqs>Y5v<{6A;VU_l=RS+v(w~!4f3vH5dfu>h0Y{U97`y!8FuQwxm&u;aiUv+$Fv=Fq zVejAM0_DWbtgS!C9pjCSjiodBu%)D=U@_>1YmLT|x$H;^q>>69PiCc4*+Q~>KDoz^ zmJ6gXPZ!F9T3Wa}+#hzAYYf0FSLRsf~+g(+4RbJNeQaaH8;~OMY7# zg_$tp7*G91qgW`N$_ z+Lz4Fvp?@26ci+0w%mn4y~b=PN)Kr>?$fy~p=fA#Fp7p`&z5dl*2)OX;e_8%R0MJ} z@>C4yIR)WGQn&|WNt84+G!v}{V@Xii5-}uv?)MU3s}E;Ob$H@0w+Ah;r8hFv35Rah z6eC1gGphIq{1b8gl%OS$YY--(GWB`iR6RY&&*KQ)`K5Cfpwv>Eh?fr)J1m34ZXUnm z7=Mz$=IQ12a$Yyp>~x0FWWBK)Cnu02mn8t!+}etYg3@!dHxv>UMoL6PG|`xnptagy zAub~F-D0VdoS8XhrOu>Fo`so067of-` ztGC2Zv82#o&g4jyE|}Hl3m1bZB{c?LlqX$2 zR;pSx*&l*-^Y}P8GlL>0H^sJA)YKJ!R3MdPyilqfPAHf;TOj4I)B7Wp%`A4p#D=J9 z-O}oNNgn}=VQ_7Iy%Mj>C7ZrTAw4s-f|t*;hw|fCt@-j*pVIhV@{zT}MJkZcr(Ee| z7SQpK2z+}y_?6vTEPZj-+t(*duhl%gtcNA~-=<@FOYUbjSC}(*Qmot6StHD>4zlS| z-rzZ5xwz=`UlZA>bbO6W8>M+a4@S-Wc0@&gk0q`mV@BZgoG!3H#xg~=^WYoFGQSvB z+iVG>XJk-oHCNXNGrJYQChfK%Zq>}VkUAhu6pYBpKzCteo^XhUnnsKtP4>K9^{MLq zUMRhOShBs+LHxn+-OYt(G+qDR_f$vh6ueI0^_Y}^&$YW;y)GP2c{8>=_J#VLQegE3 zWh1@y#_Ue6hQTnSzzT`LU%jrL8{Pe3)}2DREO7!n%#spfAtjQJ+5aJIwatYQ0|R3` znc2^#^Cb|4R1AwZpRY&SWU!kDf_u*=Fn^!S9CE?%_wzsLvPj^371CIuS)VB;Eo20) zg`%VN76aNPbc|+-;|P&}Kqe|VfXQg|=cdOMKiFsj?a)e{3AIjpb4=d2CZagupii8j zF%Rp%)gKB&jk1YNUhi0m3=04_K##}O@2gGuVtEcoI(S(CWZ|9>0FojD{_$c>JWIgq z-cN+z_4;qE(U|?yS9?+-YPKgPy^1qI}o3|fC~b_ci#p7uxLT3l~(6(X&m zM`%)h?7%&5O@7@tPc*%h516P|@_eI5IbSa!4}gIBv;C{0W2XdHT$F))q}y@7%{+H{ zT0WK2rqZX==}d)>k8hUj+h6rMyzO&?m`S1-JGk>hzY z?_ziz&i4V23KclxnFuj48o0t$I`UNn@Aq-=Zp8k~$(HO2Kp<*?RMO$~PM=Z<@W*uF zFrVi$6E274pXF*jH8r(r#}lBy*So`DBz{;{7NLNEfaO*v{$HoFMZ3F0nb^4h!UnuiS)=n^B=@+@BD~`>tS3) zF8_=!h+M2T^eru^3hs+I!$CuD?+%2WMEc{Pq6&e2bps4;US3w0ORZE6EA#5TE@}3d zfS$f%{FkC;lZ`8x4j;}TeS+y78GL4O=8!H~09br1( zWNDdQT}>}wtyWFXpl?_>tXAVs&fqW^3_ssr`~w4V_FWlS;Gaqqi{S0YRgG#1t8Nd* z9PUk5OG8A%Fqn@d-wlLe&ea%*|H6gqP{yrzz zpb&vu;9Dv1N1D*U~skB!HC}0(ZSE_a{0af zwBT~1ZLY~iHx!*Vu&$0(9N4jvmzQ_4+CXbGmRJBtp!p67**VWV7k3obQ=(X8>*4~H zZN5;7$8GOFnn1gEbTkx+&#Ttq&TegOZMjk#Fq%N?@OZvrB8GiFlLvJ)m38#AErU-B<2cGM=ukE{D}>ewHBxHFbz`naWI=D&zJ_ zj96c9udS`^_TfYtoqDbRRp(n|AObEbF78;y7o)d;%_2Jz5>h0gpschsJio_dzGjp4 zt6RKo|J=}WvmI$?XXkpGOO$K|53}Vm6Ig6q+{tnc>E-36h`4y2!4P8S%OP%~^~P)O zkn|EQLWBvKI*i9gtCRUmu4t)lm-h9)@j>5?8BYe>-lR_r7cpwauwjh4B z)v3(h)^@t<@Zg{zE6BCy-;YvY0l=(L#%6PGe$B96)_ies5oDgPcc&X|F0E%5s|`{s zD=Q~iYiny|_`J@?O?NjpGhG-cC^@j`H2Lws({5jgss4`vOx^r9{G^$N(Q}-`l~`Zd zqP@imO?U!6uC++Jqe*-nkBbJ&H6NWFmC1-{?;Q9NPXEX;-rKFM&#?>fCS9|v!^7#4 zxJ8JQ?cc5!>{g|d#uN241(b5|Xl<^{;NajVvIZ~r-pHE9>zC@8yEQ%nbLRiz~c$#?aF=HwDWc=FUZZG@`n{I;6F7>Rx zo%Buu;Qeq@S*GMmtym9vw9}{8^Q~A7WiZ^g&IAFuoe9RK>w|-sm{_aBJ;CX0ai-2> z3VvzA)IN1S^e1(1Ul7v6l;V^c1`f_>lCJkisd8Df#}flHGxJYI9X?ImNLrP0iG$IE zkdTlW!ON|#DVwej!u^YhIH6&3Y4RM9B7@NTNv$m2#~(OnCQ1ouh1Xvp9+;-K(gR*7y8wU_4(fe> z(J<3HSF1qL*q7n=cr+Y}#Gj%^=hH?toyq;tV7`#6)nXq{uZi(jAx}&p`xm&SrRDKb z6=8?R(_++86dAC^4OKI^=+yr#E)H@%HYMAN0Ia`786W>BmV+U(p^hcmQ+uR=6^%{T_-xkALD{N7-gLs>& z-lu19$V_1W^_}06?$o33k@;;4D?Vfug+_<_gToeB04#d0cAGOJ%H>!br_pC@>uIre zgR`_5cL0m%=>Bj`UtdFhwI}y{MVT_`!IGHC%X$i;AEh6#05Ah`tw_c3By4O-f`2q+ zV>pdI9T_hPz=^@>_*>#fbmFj;T;vr=+>5IpFKAnj~tu8^GUJb;Jx_|2R<0!It z=k%@(r^{3s6B)EqPkz_cM&^v9vH0-1?U8+^y}Kn}M`t_#W~;V{&Kv6TRL3QScV1HZaH5r)~v*!8!pED@{K`3G$o9)f5vo!fz7 zbaL|WRo9oSl2W`k33P_+(6Le3J>Y+{05f?AxMnm7w5o^0F{H0G1B!8mM8I_)`>emw zV2SB!LVs1k{LT<3YfAIxcXzj&w(9$)Z7LB;DMhTWM@O=))*64(C_!Ul zVvYkR__@qx$@)ZJ;z3UC`C1QX1^=&4Rg;X>=fq@|=BNO@ZyxkwEQjsyVv6%`)z_oRDmYp+>)+XS0 z7XboMRa8{?JfEXgDl|ZpghqpT!4LWc5t6WYU^!RAKjyW;>CqhYx%MLLj#H|JwWggV zUA+@vaPYJ{$;0W7%*<#ALiU)cc6)Y(0zg8sa+yjb*|)#AoYvbXCn0HRxDRJbMZ!Ud zuOdYKVbdp$&#lk)gJ19kunD<~>l)t);K`V|XO?roSGQgLpe*>Kd;4^idZEcChVgSfpqm0iZwmuGoy~#b^?+tLd zb_Iijh}7CevStesV~M|adNs^?mQ>Sr6@!x=Mms`)6vf`|keTP_8mZWlkh{uM1!~we zvCNu7+u8$ZW}9=>)Tz8}PgDgmJ1vgXDZExy~z4m z{kJ2Ra%!%g#AvCqe~xj5YnbYooYk-a(*D|Q^=%nTl>w&<4c4U_t!5i40j`-z)~piL zDD9^_Li=JWRq8iJWsEBqO=%A%ONuT!+K-jd6>v#UGF<92C5;^9hNxrU+8x;ye*NlZz4$ICweclbB*B8b zLjEX8mv#9P|6>RvS!7{mZg=<37=~I4Du>lS_lNq@dB%sX1q{SOZ*Y|vv3~oSpWPQ`m5dQIG%)-Dp&XU4o0zgdGz38;r(4<0My?6 zAGCQ*k%MEKVT@j#`C1Bd?-*Xs@>0-csunQ41ERF_jwd7?vA1ZFkV5VMpl*(hkimeO!|^Hme0vo}84-5VCQ8>CZw(EREgK2v5{yIcKtJ!YaZRR66I{cFGZauV7GWVBlSGNP0y2nAewsv-UFSiGQVe-&uIddnjYbRE+ zZ&`)p~Y!w)32~v=1%oVTa2?Ii2-l z>UL-Om`6+=jP(r>TAL zkYNX}pPyIMg)j5{D|@P9DcZnmKko>~2S!Rr`*(OW^jCX#aO|2F$!h3d-+wPr=jcq5 zg=94zm#B*?8u5e_%9o=aSQT7iF(0GCQMDXbt?0~cS9UW9N-&6@Jv5Y%gim5HTadh1 zvzYTtq%k^&zAH+)l=+E^JLU(4#AUw^z2*HzuUe&ZaMks7*ml*qcRZ7~f?AI3<||v2 zCpE#4PT-z8N=~ErwzfLHEJ!MM&oc^}A=@_gvKO^8P0VHYEtPW#a;{7ZJCzNjln-v( z7+i*d>@h_}lxZARv#Lk~gM+h`+Pp%-!gc)J_Yxz&l2aX;rkF`wzpGyA>dCHgvsJCH zFB4Y$4w>#^R`~SpDP~^PCv>ZLnrHh(iJOX|A6cr2-ARYOrW`5P}OK! zaK89+snzVcvK({Bs1AVqdYSUMzP=_RBKj*CN1@Q=?e+S2k+x9-6GOY+YzMNY_FHd9 z9(g)B45$r=2 z=#HVxGP~Rq3ojy@%5H&i)$yd$c&7vwe}ZL?Kz`Qrz(Tziz&k+$uWJ z@%azH)mqWCB4UznsT?1i+)5SZ=ga4rl?B(C3MTu4*qoDu2E>c<-m4#=Bq zEc}iO$by6dK4su1(u8Mn2P2tWh7(`l(=#%-olfUze>~QbVF#bj*BVI;M&OE{EmpuF zA`WhDW~Q=Ru-I(!O=j|^9U70HXUFUJtssQ2v!F)75{#R&L$v7we`hKKdPd?jfXlU_ z7A~%P2=vU)&wtCxID{;`<5yfv5(_N=ABS?XvWLerc}m4Z52++qEv`2tCX?woPG^f? z>D&&1jaF;lkg>5-NQG;T9eFm(ZzwA8G?<;Pl-4^P3@r0%!p*q=rIEG2islAIOSN$a zBjyI>RvVL@w26>smo>mVLO-0 z^&I_y(AOjCuH4Rk5UH5&fm^VXvZg9;199655*;Wc^enRLo%AGc$Sj#r!f6Sbf`Nrh&UB^(_q zP=>5lN=WFN)7c_98QFI!DJhk5wclT#AJ{yORG_M`coczHh!pj}paZU(&7DWP!Y6`p zed_OlUG%K3oM9#VT9tx&ueL>Dvn>Hz`&j_IfWUOM-sWk+{iqe><>jS+P>`UDx@?Rg zQSIDHoe2yqY;SMxxBn?#NhC37+d4V1JDc*!x{l9xD8lgkBvTe&*&ac&I-RmwtN(8I zTF>#O2}pw68wqOe^{%Q1S%8~TnX!fgN4q10 z{mbn^iDnbS*-}-J^+xM!#!p(6@Dx@P904G)NFrCf=Af*<#2evk+(&QwzmTszEjPcoEDbkR?^om0nwd2SOVNXm{!KtN9b+-fp_8YcIb zZ#_8_DZ*@YPEsYs>0$+LnMy_PzySG1MkbeCudPC1t=pZkSR|e#5Qqu^g}CpBB;b0u z91HI+aRwYG7qcc9Sy?lIvBZ_24N@6Iq}EjcoK?ifVhIU#kJ4D#Q`zjlMCkBfi-?OW zc6z!o=ytx3BRcZnSD6SyZRF>Z;o;-2bb9fk(D-$9 z81zWn>Z?e{W!0RDFC`_FlwHKhR*dbt+>mdrPyE^5UcbX?&%@n*?+JPEB{HQ9=Tni>Lvf47`QDdQWx=WBwt5^oXe0-}}{gkZ1>1@#?(Q37R zWV;9KKNd$u(An8pt0PsA5*%gM2)&+axZfC!`~H#`QHMWxtck(0rFvjl@4&*?c>54C zPpl^p!S)QJT(f9m4I~$DOEd7FUwfOj%pD{sDUY-~QywW^AJ=SBcwHDU8Fi%RtdVhX zrS=9R>d1*QN;n4KV&Ie;E5~06&R#y&ubypghZ@ppX>bQ-|)xV-5m4f{fn9IvQF$KT`L&t=1Zi-k)!z(>P>wmkf=J zL`#4LwdC2b=rpzmqv#)Zw5pZ)yd}Y82*f(;q6|Q=7RibDa@{KaTFpX)VVBEw@OsUi zxwl(-_1gR%2!OOS$S2_QI?H!@y7BSx<%6q36Hf=8Znmk?a{C9G-*Gj!m?=v~;0ou7 zX^=XJ>9lgjbaZ$Xc6F1IlGb@+FcNWbsckhlcK-$UeBKE%(@8QU@<&F-g@ZyeH=wy# zEBe@4RFQ(GjyhfV>Of)8JY;2}kc1S1zA4Wqv)b;6->F*Vygj{Z(KB$U>!7O%wo&{*8!%Argn(-1hN&#S%e!pZ{X5 zsmN@u(AC}jzhU2h{II*;{_Xws@kCdVrMp$64uC|bRUxCG2%OB|-P_v}E|SftZfcr3 zoEFik_xbWpWi!)1XA%)W!N3qZTdcUbzu&K$5*WL6e!1~_?d=E=5)x|m`4X^NtvH)gM!QJ?HfFR~AupNP0Fu}FJlpAP(Q=~|-Nxohx@lzW zO7TR4CfxY$Z~{bWEb01WoGn$|4H5cOTP!gc1d-qRqEX5sV_*z+`$B*h>vjnQz@n3o zk?p5A-HpB77nty%?J8Stb!J2fekKckK0`=JNo~3vWq7J0(T1DMZ!Lzfw+gn6^rySexWY(D@9UT&wQH954`zs|SrR0C0#n7lI)qzlS#hRL$ z(;h!4^Tl%Y(Vm{)+MS*&!!e=lrt8girz3HcQ*^p5_64&ijx=<1($onrZ)}XC=WU7r zND@sEQBj2g0Q$!VRS*(kq4`4TPkN2`I7&Gd{r+IKI~8a>sw4*OTo91pYU9{N32C<+49Bjw z*%H_Y)CVIZB>ea9AD7*(f4iJw%%%v*nshRg@9scYTzYzVP!N=_uP>;8AA&}iJ9lEb z+0Gq?L7xjGPsvlo5_5E9OQ2PyASNcBdvN5z2NiBkW(%-8A?O&|gOr*Oje1kxxHvS2!*R*F^~J@+kvK|L)9LJVcm-%;`wTuo z4h4OE5D5bL0_=B9L))<=WZVnIijFq}}Qm zg3D<=U2itWHuiM65!TYeO+Y{Z21-z^)S1*-uGCtFX+slBD_2TEUhBu@Uv0D{k%)%M zH(776jXj;O2zR(YP|0V5fr*Cx)awsMQ7TuP$*zGV7SV~Ojr;xQ4^d@hB|H|>-!_*k z=@AHj^hF5*(Z@ox`EmrJLK`fWP%$t9fuY&iMDHITbsy>e$Rthw zlZhjfIy-EI+@H_Q%{^YOA*Gbdl&<5e6N^4F`ycC#g)}4<=`;W$3@=q`&wcq^tTr?^ zH)FkHwQzmG5c@0aw;EpeKoQEfIa_IIYp-;8aI%_AvJHEqp`t>=!|%Vp+jpu-wuthzLQ)=%y&K#~=#@r&EIZru;!$I`aR~NA>^5(?9u!)#eK{<_o0( zEiK&QQ3Nt~tMSx|#VQpzAWZ#iv0@Nf4x0EDAI<=8eqbP=!D5NU^?I9aOfE}cVl;u4 z+v}B0wMu7TZ7t2=Xj0FZ;s+M7|FEpsOJapW9^^uqs*mx@>%-a2!^6PrEUJfxN2y8$ zKus+fh2*DD@H@Hc$A090P?hZ0`s8F}nQ9ddGYO~(thOR`cxWi-Y^h4H?mdjLGuvP& zQhhT@wuseGO$eHJHpYtF&uXpF?sk8~gdm0d-~TNny1qWsK=r}4O4%ZhLlk7>=*w1c zaPZpBa>lc<*B9%L=xF3`-@e^PjUUGYY3A5#748lvUQWxZiZz=Uve}8A&R629tC_qY zZUf*jr%z@RK{`?lUK5&F8fd(gA!f#^*5*u)POGxt?F&(Fvy};orndL@MV>ASWAl4ZbAnKcQD0`U z@ZtUQTN0>Ub}8=fe_oINoZbk^{6D*&bUpaW#6-j%hyaEs_GhA=NM#?Zb{`{)18tCf zvF0TI+su;xuq?N`v9g=bXKOT=OQ&&6EbkbGbae1HSS$^0Y~UjyA%R`4xA;XxAuW`t z7Acph{O1xBKFfT*K9`r5w|94QbraRwTkM2#?|9(ZxwzE6G^}}p zXjLnx+ua;M22)qZT4yo^DSgCa__M1-%dfea>vX9qDlH9HR#q0ItNxz6f9Qs?5Ql_> zfSjPHm{{rav+Kd>X{l_MfYo_*=Gf+@&PJyf??i2l!BDB$Tp@Kqd@7Gq>Fc&1QaFl6 z>*M)KUL*m3X?jM+!x&2_CkcsAOKU6WCg|AMC3tvvNGIl|fti_P6al|lmv=`^-ze&X67NSK0`Boz6C;?{+oRD&tJ5Gft#)Q+ zrkbW^k$R#+dy5lj%D^)_5TkjgaGZWa& z%?%kjxt^7k6^qYTk{)nm`*fj9sa#FrYh8(=Yv#n!)x!e>%y~cWK!I7WH~U6JAb@dj zaI81m^(7MvsEXaBuKJ7k`uaAvwt^^BFBHM1EMX<3pF2A{^HsWAWL3g|cLW?ZFzF*4 zGD^z5!^6Och=|V7v7cX`>PEnkZY6Q1jsLU2B4TQ4n!2PoNXUsk6~@_=ITlSU92_3L zzg%Omyt;b(`F3T}Y;DaQ1cgK}-C&_=JdqlZkf5BpB)!AK=X(9OY0EoJz0O#n-c%v3 z71G$)7?0OE9JCVoJsztqmMYn*(t5YPM3a?RT-|TeKw5ufuqC0Qf*#ChG=8U|X_JJG z4jva5_e0utsZu-7a;0_<{-%W`^huP0jHR*GXzWz58v+V(9&^v(jGK&%4D=4K_b0+f zlNsQ1t%%e&PnR2-gk2@t5Px?E!sa@?JRnY@H7GbU~lY;$kXLDx$@n zJcGxH#aS)I{r(uGrl#gU$5)}*w7a`2R3w`L0+7MNFz9D})a=W`cQ58k&}dc4K@{BZ zu*77RVAU0K!kSi_v&`35gT>OR2hPEugZzIjf9dJzSlssc-Yq3sLE*cc2^O|zi}YUT zi+e*+D~(pTmzS3yL!%b6>hy+5fwQ$jq$d{QP`gPVgh%tUb%;v&G)l zRz5N^(){jlLM;4US1zmX{pC(MOTa5WEG(>4yRFgvZh~{+;cV&Q?5NQJ8UaC>CquTS zwUvgGlk?#4u<)OspUfdoKIT`UOq%o|PCA#JP)l1It*WZ(!R~Hupf?w}-z9j#~k{Ihe zUv7^olNOdg-rqs+4cJxZ8-vMY`iSj@Xc$I83n?e%%gYN$(;ZHvJqfL`+ic>|($Yra zamUKUz!=mpIfDrqDg%M2eo#mp#{#6Jr0R{9*qxo7kSn^UJ!3$z;)&tj0605m=fx=P zgg8eYd|j^xbnRA0x10;-i&fFP!wDH{WR4q8APX`#4K;PhZ$HSL^KgI=W{2 z16TzGg?iJOZ>p-QAfC%F=;h_*X4B(prrFK_4-fCYYjY@yP|nnp>hkh(Je56cEQxWh z-c0dyu|jI2)#+s~N-)zywl5GNPb>oG`T5!Q?oj5#o{^C;l7JszZ_ncG?cLnm3{of{ zj7OzPXJV%>=wzV`)9G}cn1LZuPT=J`5)zV@mewp8KO-aKU=*R;$LoWgqod)~X2)yS zK{B&}t({%Z!a@SK!y%aM{xC{=dwa2J6@k@i{R~q1WBt4z((}s;v*WQm4u@s1pC1G< zG4cJy8YU$r#O8|9&Y?eSpsY*p@JdlQrOa%1lt}5{;f|BNDu7Y^r(PW0= z!|B4@P^SB0{m(MhDir|%0UB;@by0EgGE{Ov@j{vEnSr4FelU;cbM@`b&5Vu1)n*4< zRZ+9UQRL^B*Xz9kE~oW;6{qnyRzW-mM^aN$6A)0+;&4=Iz0rDNGT-WO6bTCN5O6t^ znyfc~0O|()0oa8upD&Z$$GbbluFqGC)T7SMP5^bmVwG+ch@3A`EK;(!uPxPVs^oD# zPxjs@+QV@3Q$h>DQ*N<0`VR)&Xmtt+4c-0xcw29CL6Vb``<3>)6a;fwfnr$rr>4Sz zz$h><2qVz{0@!Q`fZ~D2i#4cjUkIrCI=P3-jWoNxL2!)*iBae{c8kS%IY4eEPAaW{ zK&D<_pxyhk-T1@=x67qg;Sm}R4qz&if9Aw-d3AL*S2WCI(azR(d$vF-9EhlKzk7cMuZyj-bD?k$BG~I`I*-#SGCKN??nr!IEEYp(V`B~Q zt<}|3#Ufc06cpd4CeG{xg3%<#tP}2hkopFjoSclORgKhWFb9_}Q=!kz&9!xMnwa(W zH;XkSvY)r8>L~o`YH4ZdOr>fl>dFrLNgaFHS*hIy3iVf-?ee@sd+!_N6taK$9#yhB z97;M1W|eU% zDe9>Pq3x2Z@cs(a1)!EqP^WY2b(YU3HX~z|bd!{{G>Es8&JxHdRW3VC;xwNRMyFK) z1F^WN99F3(-sR=x_jfX2poVw*gHiBm*BkxV*jSd0PP4f}Q0jMo->_1nL3d5k^?fUv)j3VIg?dbt=)rPH6 zA8$_%`@<*{vgyP9A!y*xh=~8!)C>$J)cyVS@#*yVplvxfO@4dUzM1lW_pV(@d3k-a zXG{P6^)>m$g@q+=Z%qX)SY75T-6f_wEAb5jX!E&s`MV=0Pj-4u)w;MMaB+Il-m0%! ze}8>dwzQONSQ)&WDQfZC`v1Sv?c*Z?%e=no-sjjX87X7~SzttDk@7TjKLhsH z{RQprj@?~W`!IL5dH%T`N#hm4%l*t;)OuW3b+(*n2kiuJ;S`R0wQTc=6COIcx|=nP m5_LcgaSlaQ+}*Md^=G>mc+ccNo5sMvz~JfX=d#Wzp$Py>_*o Date: Mon, 30 Dec 2013 19:08:09 +0100 Subject: [PATCH 3/9] fixed launchdetection logic, catapult tested in HIL --- .../launchdetection/CatapultLaunchMethod.cpp | 5 ++ .../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 0a95b46f60..d5c759b17b 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 26f6768ccf..5056bcdc13 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(); From 080ecf56caac571f905f657875cec9fa8e7e17d0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 30 Dec 2013 22:21:53 +0100 Subject: [PATCH 4/9] 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 5056bcdc13..455fe36742 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 */ From ec8bc6c0208a5eb9ad3decf7bec196c5bf113dd4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 16:33:50 +0100 Subject: [PATCH 5/9] 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 455fe36742..bbb205b2f8 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; From 4191ae33c264459f0a85d9c03b8cb4893c6ee33e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 1 Jan 2014 21:54:33 +0100 Subject: [PATCH 6/9] navigator/mission feasibility: prepare for pre-mission fence checking --- .../navigator/mission_feasibility_checker.cpp | 21 ++++++++++--------- .../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 25b2636bb8..cc079dee19 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 7d1cc2f8a9..ef235ead44 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 982ebefccb..c88f237ad0 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); From dca6d97a5288766e3e0da05dc5fdc98108fa7892 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 14:18:02 +0100 Subject: [PATCH 7/9] 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 f64bfb41a4..08fe2b696e 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 5f92e14cf3..5f4bce6986 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 0000000000..bc5ef44f2a --- /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 0000000000..6d6e6e87c2 --- /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 6be4e87a05..b7900e84ed 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 c88f237ad0..a9547355ff 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 */ From 429a11a21d25e34ca711b2c0debb2ac3e84c45ca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 2 Jan 2014 15:01:08 +0100 Subject: [PATCH 8/9] 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 bc5ef44f2a..4a0528b16f 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 6d6e6e87c2..8f3a07b02e 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 cc079dee19..aba2dffff8 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 ef235ead44..7a0b2a2966 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 a9547355ff..354fa733b3 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(); From a48264d5d44018412b9443b245e6974d3f54b20d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 4 Jan 2014 13:37:49 +0100 Subject: [PATCH 9/9] 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 4a0528b16f..199ccb41b0 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 8f3a07b02e..8a1d06e71a 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 354fa733b3..a226aac7c1 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();