From 7baa337d9bcf7077a5e5080e951899cb595f6ff6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 19:25:53 +0200 Subject: flight termination on geofence violation --- src/modules/commander/commander.cpp | 8 ++++++++ src/modules/navigator/navigator_main.cpp | 10 ++++++++++ src/modules/uORB/topics/position_setpoint_triplet.h | 1 + 3 files changed, 19 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 04450a44f..522c6e886 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1244,6 +1244,14 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); + + /* Check for geofence violation */ + if (pos_sp_triplet.geofence_violated) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present */ + armed.force_failsafe = true; + status_changed = true; + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..ba46bd568 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -335,6 +335,11 @@ Navigator::task_main() /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = true; + if (_pos_sp_triplet.geofence_violated != true) { + _pos_sp_triplet_updated = true; + } /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -342,6 +347,11 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { + /* inform other apps via the sp triplet */ + _pos_sp_triplet.geofence_violated = false; + if (_pos_sp_triplet.geofence_violated != false) { + _pos_sp_triplet_updated = true; + } /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 4a1932180..4e8c6c53e 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,6 +97,7 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ + bool geofence_violated; /**< true if the geofence is violated */ }; /** -- cgit v1.2.3 From b5ef8fd2cd54d180b5debe362a4c1f07f64394af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 20:03:37 +0200 Subject: create empty datalinkloss class for OBC Currently the class does the same as the RTL class. It is now possible whichclass is sued in the navigator to handle datalink loss via a parameter --- src/modules/navigator/datalinkloss.cpp | 317 ++++++++++++++++++++++++++++ src/modules/navigator/datalinkloss.h | 95 +++++++++ src/modules/navigator/datalinkloss_params.c | 98 +++++++++ src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 4 + src/modules/navigator/navigator_main.cpp | 13 +- src/modules/navigator/navigator_params.c | 11 + 7 files changed, 539 insertions(+), 3 deletions(-) create mode 100644 src/modules/navigator/datalinkloss.cpp create mode 100644 src/modules/navigator/datalinkloss.h create mode 100644 src/modules/navigator/datalinkloss_params.c diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp new file mode 100644 index 000000000..17e85b284 --- /dev/null +++ b/src/modules/navigator/datalinkloss.cpp @@ -0,0 +1,317 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _rtl_state(RTL_STATE_NONE), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY") +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +DataLinkLoss::~DataLinkLoss() +{ +} + +void +DataLinkLoss::on_inactive() +{ + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } +} + +void +DataLinkLoss::on_activation() +{ + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + + set_rtl_item(); +} + +void +DataLinkLoss::on_active() +{ + if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(); + } +} + +void +DataLinkLoss::set_rtl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_RETURN: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +DataLinkLoss::advance_rtl() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h new file mode 100644 index 000000000..242cfac8d --- /dev/null +++ b/src/modules/navigator/datalinkloss.h @@ -0,0 +1,95 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file datalinkloss.cpp + * Helper class for Data Link Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_DATALINKLOSS_H +#define NAVIGATOR_DATALINKLOSS_H + +#include +#include + +#include +#include +#include +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class DataLinkLoss : public MissionBlock +{ +public: + DataLinkLoss(Navigator *navigator, const char *name); + + ~DataLinkLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /** + * Set the RTL item + */ + void set_rtl_item(); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_LAND, + RTL_STATE_LANDED, + } _rtl_state; + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c new file mode 100644 index 000000000..bfe6ce7e1 --- /dev/null +++ b/src/modules/navigator/datalinkloss_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. 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. + * + ****************************************************************************/ + +/** + * @file rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes + */ + +#include + +#include + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b50198996..a1e42ec38 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -49,7 +49,9 @@ SRCS = navigator_main.cpp \ offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ - geofence_params.c + geofence_params.c \ + datalinkloss.cpp \ + datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b3..d0b2ed841 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -35,6 +35,7 @@ * Helper class to access missions * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #ifndef NAVIGATOR_H @@ -57,6 +58,7 @@ #include "loiter.h" #include "rtl.h" #include "offboard.h" +#include "datalinkloss.h" #include "geofence.h" /** @@ -165,6 +167,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ @@ -173,6 +176,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index ba46bd568..1ce6770c9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -40,6 +40,7 @@ * @author Jean Cyr * @author Julian Oes * @author Anton Babushkin + * @author Thomas Gubler */ #include @@ -125,10 +126,12 @@ Navigator::Navigator() : _loiter(this, "LOI"), _rtl(this, "RTL"), _offboard(this, "OFF"), + _dataLinkLoss(this, "DLL"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), - _param_acceptance_radius(this, "ACC_RAD") + _param_acceptance_radius(this, "ACC_RAD"), + _param_datalinkloss_obc(this, "DLL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -376,7 +379,13 @@ Navigator::task_main() _navigation_mode = &_rtl; break; case NAVIGATION_STATE_AUTO_RTGS: - _navigation_mode = &_rtl; /* TODO: change this to something else */ + /* Use complex data link loss mode only when enabled via param + * otherwise use rtl */ + if (_param_datalinkloss_obc.get() != 0) { + _navigation_mode = &_dataLinkLoss; + } else { + _navigation_mode = &_rtl; /* TODO: change this to something else */ + } break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 084afe340..afaf1c3c3 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -37,6 +37,7 @@ * Parameters for navigator in general * * @author Julian Oes + * @author Thomas Gubler */ #include @@ -64,3 +65,13 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * @group Mission */ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); + +/** + * Set OBC mode for data link loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); -- cgit v1.2.3 From 8739308999b410ac8e2a92cf3e5fa63c5e18f5ba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:40:26 +0200 Subject: WIP, datalinkloss: implementing basic behavior --- src/modules/navigator/datalinkloss.cpp | 228 +++++++--------------------- src/modules/navigator/datalinkloss.h | 33 ++-- src/modules/navigator/datalinkloss_params.c | 91 +++++++---- 3 files changed, 130 insertions(+), 222 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 17e85b284..2bd80165d 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,10 +57,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _dll_state(DLL_STATE_NONE), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT") { /* load initial params */ updateParams(); @@ -77,7 +81,7 @@ DataLinkLoss::on_inactive() { /* reset RTL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { - _rtl_state = RTL_STATE_NONE; + _dll_state = DLL_STATE_NONE; } } @@ -85,40 +89,40 @@ void DataLinkLoss::on_activation() { /* decide where to enter the RTL procedure when we switch into it */ - if (_rtl_state == RTL_STATE_NONE) { - /* for safety reasons don't go into RTL if landed */ - if (_navigator->get_vstatus()->condition_landed) { - _rtl_state = RTL_STATE_LANDED; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - /* if lower than return altitude, climb up first */ - } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - + _param_return_alt.get()) { - _rtl_state = RTL_STATE_CLIMB; - - /* otherwise go straight to return */ - } else { - /* set altitude setpoint to current altitude */ - _rtl_state = RTL_STATE_RETURN; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_global_position()->alt; - } - } - - set_rtl_item(); + //if (_rtl_state == RTL_STATE_NONE) { + //[> for safety reasons don't go into RTL if landed <] + //if (_navigator->get_vstatus()->condition_landed) { + //_rtl_state = RTL_STATE_LANDED; + //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + //[> if lower than return altitude, climb up first <] + //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + //+ _param_return_alt.get()) { + //_rtl_state = RTL_STATE_CLIMB; + + //[> otherwise go straight to return <] + //} else { + //[> set altitude setpoint to current altitude <] + //_rtl_state = RTL_STATE_RETURN; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _navigator->get_global_position()->alt; + //} + //} + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + set_dll_item(); } void DataLinkLoss::on_active() { - if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { - advance_rtl(); - set_rtl_item(); + if (is_mission_item_reached()) { + advance_dll(); + set_dll_item(); } } void -DataLinkLoss::set_rtl_item() +DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); @@ -128,146 +132,43 @@ DataLinkLoss::set_rtl_item() set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); - - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", - (int)(climb_alt - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_RETURN: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - // don't change altitude - - if (pos_sp_triplet->previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint( - pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, - _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint( - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_DESCEND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + switch (_dll_state) { + case DLL_STATE_FLYTOCOMMSHOLDWP: { + _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.altitude = (double)(_param_commsholdalt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = false; - _mission_item.origin = ORIGIN_ONBOARD; - - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", - (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); - break; - } - - case RTL_STATE_LOITER: { - bool autoland = _param_land_delay.get() > -DELAY_SIGMA; - - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = autoland; - _mission_item.origin = ORIGIN_ONBOARD; - - _navigator->set_can_loiter_at_sp(true); - - if (autoland) { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", (double)_mission_item.time_inside); - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); - } - break; - } - - case RTL_STATE_LAND: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _navigator->get_loiter_radius(); - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + _navigator->set_can_loiter_at_sp(true); break; } - - case RTL_STATE_LANDED: { - _mission_item.lat = _navigator->get_home_position()->lat; - _mission_item.lon = _navigator->get_home_position()->lon; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: { + _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.altitude = (double)(_param_airfieldhomealt.get()); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = 0.0f; + _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + _navigator->set_can_loiter_at_sp(true); break; } - default: break; } @@ -282,35 +183,16 @@ DataLinkLoss::set_rtl_item() } void -DataLinkLoss::advance_rtl() +DataLinkLoss::advance_dll() { - switch (_rtl_state) { - case RTL_STATE_CLIMB: - _rtl_state = RTL_STATE_RETURN; + switch (_dll_state) { + case DLL_STATE_NONE: + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; break; - - case RTL_STATE_RETURN: - _rtl_state = RTL_STATE_DESCEND; + case DLL_STATE_FLYTOCOMMSHOLDWP: + //XXX check here if time is over are over + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; break; - - case RTL_STATE_DESCEND: - /* only go to land if autoland is enabled */ - if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { - _rtl_state = RTL_STATE_LOITER; - - } else { - _rtl_state = RTL_STATE_LAND; - } - break; - - case RTL_STATE_LOITER: - _rtl_state = RTL_STATE_LAND; - break; - - case RTL_STATE_LAND: - _rtl_state = RTL_STATE_LANDED; - break; - default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 242cfac8d..101c88a25 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -68,28 +68,27 @@ public: private: /** - * Set the RTL item + * Set the DLL item */ - void set_rtl_item(); + void set_dll_item(); /** - * Move to next RTL item + * Move to next DLL item */ - void advance_rtl(); + void advance_dll(); - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND, - RTL_STATE_LOITER, - RTL_STATE_LAND, - RTL_STATE_LANDED, - } _rtl_state; + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; - control::BlockParamFloat _param_return_alt; - control::BlockParamFloat _param_descend_alt; - control::BlockParamFloat _param_land_delay; + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; }; - #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index bfe6ce7e1..a836fc8ca 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -32,11 +32,11 @@ ****************************************************************************/ /** - * @file rtl_params.c + * @file datalinkloss_params.c * - * Parameters for RTL + * Parameters for DLL * - * @author Julian Oes + * @author Thomas Gubler */ #include @@ -44,55 +44,82 @@ #include /* - * RTL parameters, accessible via MAVLink + * Data Link Loss parameters, accessible via MAVLink */ /** - * Loiter radius after RTL (FW only) + * Comms hold wait time * - * Default value of loiter radius after RTL (fixedwing only). + * The amount of time in seconds the system should wait at the comms hold waypoint * - * @unit meters + * @unit seconds * @min 0.0 - * @group RTL + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); /** - * RTL altitude + * Comms hold Lat * - * Altitude to fly back in RTL in meters + * Latitude of comms hold waypoint * - * @unit meters - * @min 0 - * @max 1 - * @group RTL + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); +/** + * Comms hold Lon + * + * Longitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); /** - * RTL loiter altitude + * Comms hold alt * - * Stay at this altitude above home position after RTL descending. - * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * Altitude of comms hold waypoint * - * @unit meters - * @min 0 - * @max 100 - * @group RTL + * @unit m + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); /** - * RTL delay + * Airfield home Lat * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * Latitude of airfield home waypoint * - * @unit seconds - * @min -1 - * @max - * @group RTL + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); -- cgit v1.2.3 From dcf114aa65273d5d5ce522565fc364fc347ba3fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:53:04 +0200 Subject: data link loss timeout as param --- src/modules/commander/commander.cpp | 8 +++++--- src/modules/commander/commander_params.c | 12 ++++++++++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7d4d677d0..8eba64c1d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -127,7 +127,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -650,6 +649,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); + param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); /* welcome user */ warnx("starting"); @@ -924,6 +924,7 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t arming_ret; int32_t datalink_loss_enabled = false; + int32_t datalink_loss_timeout = 10; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -983,6 +984,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); + param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); } orb_check(sp_man_sub, &updated); @@ -1023,7 +1025,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < DL_TIMEOUT) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { (void)rc_calibration_check(mavlink_fd); } @@ -1448,7 +1450,7 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < DL_TIMEOUT) { + if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ if (telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i regained", i); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 4750f9d5c..25effbd21 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -95,3 +95,15 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @max 1 */ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); + +/** + * Datalink timeout threshold + * + * After this amount of seconds the data link lost mode triggers + * + * @group commander + * @unit second + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); -- cgit v1.2.3 From 86b9e367a6cc5791f83df3223190a470798c00ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 18:23:41 +0200 Subject: introduce data link lost counter --- src/modules/commander/commander.cpp | 1 + src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8eba64c1d..46065fef1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1477,6 +1477,7 @@ int commander_thread_main(int argc, char *argv[]) if (!status.data_link_lost) { mavlink_log_critical(mavlink_fd, "ALL DATA LINKS LOST"); status.data_link_lost = true; + status.data_link_lost_counter++; status_changed = true; } } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b75..eda1dfaec 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,6 +201,7 @@ struct vehicle_status_s { bool rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From a9a8f1435fa798b289aa4e4af9312041abdbcf94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Jul 2014 23:56:32 +0200 Subject: abort comm loss mode if counter above param and return home directly --- src/modules/commander/commander_params.c | 5 ++-- src/modules/navigator/datalinkloss.cpp | 14 ++++++++--- src/modules/navigator/datalinkloss.h | 37 ++++++++++++++++------------- src/modules/navigator/datalinkloss_params.c | 11 +++++++++ 4 files changed, 44 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 25effbd21..980a7a2bb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -94,10 +94,9 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(DL_LOSS_EN, 0); -/** - * Datalink timeout threshold + /** Datalink timeout threshold * * After this amount of seconds the data link lost mode triggers * diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2bd80165d..a98e21139 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,14 +57,16 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _dll_state(DLL_STATE_NONE), + _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), _param_airfieldhomelat(this, "AH_LAT"), _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT") + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "DLL_N"), + _dll_state(DLL_STATE_NONE) { /* load initial params */ updateParams(); @@ -187,7 +189,13 @@ DataLinkLoss::advance_dll() { switch (_dll_state) { case DLL_STATE_NONE: - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* Check the number of data link losses. If above home fly home directly */ + updateSubscriptions(); + if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } break; case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 101c88a25..650cc7bc5 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -43,10 +43,7 @@ #include #include -#include -#include -#include -#include +#include #include "navigator_mode.h" #include "mission_block.h" @@ -67,6 +64,25 @@ public: virtual void on_active(); private: + /* Subscriptions */ + uORB::Subscription _vehicleStatus; + + /* Params */ + control::BlockParamFloat _param_commsholdwaittime; + control::BlockParamInt _param_commsholdlat; // * 1e7 + control::BlockParamInt _param_commsholdlon; // * 1e7 + control::BlockParamFloat _param_commsholdalt; + control::BlockParamInt _param_airfieldhomelat; // * 1e7 + control::BlockParamInt _param_airfieldhomelon; // * 1e7 + control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamInt _param_numberdatalinklosses; + + enum DLLState { + DLL_STATE_NONE = 0, + DLL_STATE_FLYTOCOMMSHOLDWP = 1, + DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + } _dll_state; + /** * Set the DLL item */ @@ -77,18 +93,5 @@ private: */ void advance_dll(); - enum DLLState { - DLL_STATE_NONE = 0, - DLL_STATE_FLYTOCOMMSHOLDWP = 1, - DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, - } _dll_state; - - control::BlockParamFloat _param_commsholdwaittime; - control::BlockParamInt _param_commsholdlat; // * 1e7 - control::BlockParamInt _param_commsholdlon; // * 1e7 - control::BlockParamFloat _param_commsholdalt; - control::BlockParamInt _param_airfieldhomelat; // * 1e7 - control::BlockParamInt _param_airfieldhomelon; // * 1e7 - control::BlockParamFloat _param_airfieldhomealt; }; #endif diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index a836fc8ca..038c80a1a 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -123,3 +123,14 @@ PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); * @group DLL */ PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); + +/** + * Number of allowed Datalink timeouts + * + * After more than this number of data link timeouts the aircraft returns home directly + * + * @group commander + * @min 0 + * @max 1000 + */ +PARAM_DEFINE_INT32(NAV_DLL_N, 2); -- cgit v1.2.3 From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 22:58:19 +0200 Subject: add method to block fallback to mission failsafe navigation modes can use a flag in mission_result to tell the commander to not switch back to mission --- src/modules/commander/commander.cpp | 5 ++-- src/modules/commander/state_machine_helper.cpp | 3 ++- src/modules/commander/state_machine_helper.h | 2 +- src/modules/navigator/datalinkloss.cpp | 2 ++ src/modules/navigator/mission.cpp | 32 ++++++-------------------- src/modules/navigator/mission.h | 8 ------- src/modules/navigator/navigator.h | 13 ++++++++++- src/modules/navigator/navigator_main.cpp | 22 ++++++++++++++++++ src/modules/navigator/navigator_mode.cpp | 3 +++ src/modules/uORB/topics/mission_result.h | 1 + 10 files changed, 53 insertions(+), 38 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c89e0123..cb09a68e3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1461,7 +1461,7 @@ int commander_thread_main(int argc, char *argv[]) for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { /* handle the case where data link was regained */ - if (telemetry_lost[i]) { + if (telemetry_lost[i]) {//XXX also add hysteresis here mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; } @@ -1545,7 +1545,8 @@ int commander_thread_main(int argc, char *argv[]) /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, - mission_result.finished); + mission_result.finished, + mission_result.stay_in_failsafe); // TODO handle mode changes by commands if (main_state_changed) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7b26e3e8c..4e1cfb987 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -435,7 +435,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, /** * Check failsafe and main status and set navigation status for navigator accordingly */ -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, + const bool stay_in_failsafe) { navigation_state_t nav_state_old = status->nav_state; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index bb1b87e71..4285d2977 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -63,7 +63,7 @@ transition_result_t main_state_transition(struct vehicle_status_s *current_state transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished, const bool stay_in_failsafe); int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index a98e21139..52f263aff 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -200,6 +200,8 @@ DataLinkLoss::advance_dll() case DLL_STATE_FLYTOCOMMSHOLDWP: //XXX check here if time is over are over _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); break; default: break; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ba766cd10..7ce0e2f89 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -68,8 +68,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_offboard_mission_index(-1), _need_takeoff(true), _takeoff(false), - _mission_result_pub(-1), - _mission_result({0}), _mission_type(MISSION_TYPE_NONE), _inited(false), _dist_1wp_ok(false) @@ -577,18 +575,18 @@ void Mission::report_mission_item_reached() { if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; + _navigator->get_mission_result()->reached = true; + _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; } - publish_mission_result(); + _navigator->publish_mission_result(); } void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); - _mission_result.seq_current = _current_offboard_mission_index; - publish_mission_result(); + _navigator->get_mission_result()->seq_current = _current_offboard_mission_index; + _navigator->publish_mission_result(); save_offboard_mission_state(); } @@ -596,23 +594,7 @@ Mission::report_current_offboard_mission_item() void Mission::report_mission_finished() { - _mission_result.finished = true; - publish_mission_result(); + _navigator->get_mission_result()->finished = true; + _navigator->publish_mission_result(); } -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; -} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a1155..1b8c8c874 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -128,11 +128,6 @@ private: */ void report_mission_finished(); - /** - * Publish the mission result so commander and mavlink know what is going on - */ - void publish_mission_result(); - control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; @@ -145,9 +140,6 @@ private: bool _need_takeoff; bool _takeoff; - orb_advert_t _mission_result_pub; - struct mission_result_s _mission_result; - enum { MISSION_TYPE_NONE, MISSION_TYPE_ONBOARD, diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index d0b2ed841..363877bb8 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -52,6 +52,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -102,6 +103,11 @@ public: */ void load_fence_from_file(const char *filename); + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + /** * Setters */ @@ -115,7 +121,9 @@ public: struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } struct home_position_s* get_home_position() { return &_home_pos; } - struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } + struct mission_result_s* get_mission_result() { return &_mission_result; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } @@ -143,6 +151,7 @@ private: int _param_update_sub; /**< param update subscription */ orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + orb_advert_t _mission_result_pub; vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -152,6 +161,8 @@ private: navigation_capabilities_s _nav_caps; /**< navigation capabilities */ position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + mission_result_s _mission_result; + bool _mission_item_valid; /**< flags if the current mission item is valid */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1ce6770c9..77124e8f6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -108,6 +108,7 @@ Navigator::Navigator() : _offboard_mission_sub(-1), _param_update_sub(-1), _pos_sp_triplet_pub(-1), + _mission_result_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -115,6 +116,7 @@ Navigator::Navigator() : _mission_item{}, _nav_caps{}, _pos_sp_triplet{}, + _mission_result{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -370,6 +372,9 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: + /* Some failsafe modes prohibit the fallback to mission + * usually this is done after some time to make sure + * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -553,3 +558,20 @@ int navigator_main(int argc, char *argv[]) return 0; } + +void +Navigator::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.reached = false; + _mission_result.finished = false; +} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index f43215665..3c6754c55 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -63,6 +63,9 @@ NavigatorMode::run(bool active) { if (_first_run) { /* first run */ _first_run = false; + /* Reset stay in failsafe flag */ + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); on_activation(); } else { diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index beb797e62..65ddfb4ad 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -57,6 +57,7 @@ struct mission_result_s unsigned seq_current; /**< Sequence of the current mission item */ bool reached; /**< true if mission has been reached */ bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ }; /** -- cgit v1.2.3 From 756ea3019ec3a50623d846c019c6474488273a34 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:00:34 +0200 Subject: datalink loss: fix type error --- src/modules/navigator/datalinkloss.cpp | 24 ++---------------------- 1 file changed, 2 insertions(+), 22 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 52f263aff..91bb5b110 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -90,26 +90,6 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - /* decide where to enter the RTL procedure when we switch into it */ - //if (_rtl_state == RTL_STATE_NONE) { - //[> for safety reasons don't go into RTL if landed <] - //if (_navigator->get_vstatus()->condition_landed) { - //_rtl_state = RTL_STATE_LANDED; - //mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); - - //[> if lower than return altitude, climb up first <] - //} else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt - //+ _param_return_alt.get()) { - //_rtl_state = RTL_STATE_CLIMB; - - //[> otherwise go straight to return <] - //} else { - //[> set altitude setpoint to current altitude <] - //_rtl_state = RTL_STATE_RETURN; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _navigator->get_global_position()->alt; - //} - //} _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; set_dll_item(); } @@ -139,7 +119,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_commsholdalt.get()); + _mission_item.altitude = _param_commsholdalt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; @@ -157,7 +137,7 @@ DataLinkLoss::set_dll_item() _mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; _mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; _mission_item.altitude_is_relative = false; - _mission_item.altitude = (double)(_param_airfieldhomealt.get()); + _mission_item.altitude = _param_airfieldhomealt.get(); _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; -- cgit v1.2.3 From f10d1277a5e795256a247cb6f130cefba12c5ffc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:07:11 +0200 Subject: navigator: fix comment --- src/modules/navigator/navigator.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 363877bb8..fc0d47b38 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -178,7 +178,7 @@ private: Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ - DataLinkLoss _dataLinkLoss; /**< class that handles offboard */ + DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ -- cgit v1.2.3 From a35814d15b1317f73f325e98f0500f5fd1233583 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 08:57:58 +0200 Subject: dl loss: correct timeout, add hysteresis also for regain --- src/modules/commander/commander.cpp | 21 ++++++++++++++++----- src/modules/commander/commander_params.c | 18 +++++++++++++++--- 2 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cb09a68e3..fe1974c88 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -650,6 +650,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); /* welcome user */ warnx("starting"); @@ -841,11 +842,13 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to telemetry status topics */ int telemetry_subs[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; + uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; + telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; } @@ -930,6 +933,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + int32_t datalink_regain_timeout = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -990,6 +994,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); } orb_check(sp_man_sub, &updated); @@ -1030,7 +1035,7 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd && telemetry_last_heartbeat[i] == 0 && telemetry.heartbeat_time > 0 && - hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout) { + hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) { (void)rc_calibration_check(mavlink_fd); } @@ -1459,15 +1464,21 @@ int commander_thread_main(int argc, char *argv[]) /* data links check */ bool have_link = false; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { - if (telemetry_last_heartbeat[i] != 0 && hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout) { - /* handle the case where data link was regained */ - if (telemetry_lost[i]) {//XXX also add hysteresis here + if (telemetry_last_heartbeat[i] != 0 && + hrt_elapsed_time(&telemetry_last_heartbeat[i]) < datalink_loss_timeout * 1e6) { + /* handle the case where data link was regained, + * accept datalink as healthy only after datalink_regain_timeout seconds + * */ + if (telemetry_lost[i] && + hrt_elapsed_time(&telemetry_last_dl_loss[i]) > datalink_regain_timeout * 1e6) { + mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; + have_link = true; } - have_link = true; } else { + telemetry_last_dl_loss[i] = hrt_absolute_time(); if (!telemetry_lost[i]) { mavlink_log_critical(mavlink_fd, "data link %i lost", i); telemetry_lost[i] = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f73ae71f3..3d1e231c6 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -106,13 +106,25 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); */ PARAM_DEFINE_INT32(DL_LOSS_EN, 0); - /** Datalink timeout threshold + /** Datalink loss time threshold * - * After this amount of seconds the data link lost mode triggers + * After this amount of seconds without datalink the data link lost mode triggers * * @group commander * @unit second * @min 0 - * @max 1000 + * @max 30 */ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); + +/** Datalink regain time threshold + * + * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss' + * flag is set back to false + * + * @group commander + * @unit second + * @min 0 + * @max 30 + */ +PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -- cgit v1.2.3 From bffa9e3fa83b84b51b2446291682519433fd3fff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 19:27:40 +0200 Subject: navigator: add skeleton of FW engine failure mode --- src/modules/navigator/datalinkloss.h | 2 +- src/modules/navigator/enginefailure.cpp | 147 +++++++++++++++++++++++++++++++ src/modules/navigator/enginefailure.h | 83 +++++++++++++++++ src/modules/navigator/module.mk | 1 + src/modules/navigator/navigator.h | 3 + src/modules/navigator/navigator_main.cpp | 1 + 6 files changed, 236 insertions(+), 1 deletion(-) create mode 100644 src/modules/navigator/enginefailure.cpp create mode 100644 src/modules/navigator/enginefailure.h diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 650cc7bc5..5a46b5700 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -31,7 +31,7 @@ * ****************************************************************************/ /** - * @file datalinkloss.cpp + * @file datalinkloss.h * Helper class for Data Link Loss Mode acording to the OBC rules * * @author Thomas Gubler diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp new file mode 100644 index 000000000..ea654c5fd --- /dev/null +++ b/src/modules/navigator/enginefailure.cpp @@ -0,0 +1,147 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ + + /* @file enginefailure.cpp + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "navigator.h" +#include "enginefailure.h" + +#define DELAY_SIGMA 0.01f + +EngineFailure::EngineFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _ef_state(EF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +EngineFailure::~EngineFailure() +{ +} + +void +EngineFailure::on_inactive() +{ + _ef_state = EF_STATE_NONE; +} + +void +EngineFailure::on_activation() +{ + _ef_state = EF_STATE_LOITERDOWN; + set_ef_item(); +} + +void +EngineFailure::on_active() +{ + if (is_mission_item_reached()) { + advance_ef(); + set_ef_item(); + } +} + +void +EngineFailure::set_ef_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_ef_state) { + case EF_STATE_LOITERDOWN: { + //XXX create mission item at ground (below?) here + + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +EngineFailure::advance_ef() +{ + switch (_ef_state) { + case EF_STATE_NONE: + _ef_state = EF_STATE_LOITERDOWN; + break; + default: + break; + } +} diff --git a/src/modules/navigator/enginefailure.h b/src/modules/navigator/enginefailure.h new file mode 100644 index 000000000..2c48c2fce --- /dev/null +++ b/src/modules/navigator/enginefailure.h @@ -0,0 +1,83 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file enginefailure.h + * Helper class for a fixedwing engine failure mode + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_ENGINEFAILURE_H +#define NAVIGATOR_ENGINEFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class EngineFailure : public MissionBlock +{ +public: + EngineFailure(Navigator *navigator, const char *name); + + ~EngineFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + enum EFState { + EF_STATE_NONE = 0, + EF_STATE_LOITERDOWN = 1, + } _ef_state; + + /** + * Set the DLL item + */ + void set_ef_item(); + + /** + * Move to next EF item + */ + void advance_ef(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index a1e42ec38..0539087df 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,6 +51,7 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ + enginefailure.cpp \ datalinkloss_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fc0d47b38..fe6639dfe 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -60,6 +60,7 @@ #include "rtl.h" #include "offboard.h" #include "datalinkloss.h" +#include "enginefailure.h" #include "geofence.h" /** @@ -179,6 +180,8 @@ private: RTL _rtl; /**< class that handles RTL */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ + EngineFailure _engineFailure; /**< class that handles the engine failure mode + (FW only!) */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 77124e8f6..21ab691ff 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -129,6 +129,7 @@ Navigator::Navigator() : _rtl(this, "RTL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), + _engineFailure(this, "EF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), -- cgit v1.2.3 From bc4d7952f384fc079cd30327a5b4e32da90bbcb6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:20 +0200 Subject: dl loss: don't set unnecessary value --- src/modules/navigator/datalinkloss.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 91bb5b110..19f335633 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -143,7 +143,6 @@ DataLinkLoss::set_dll_item() _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - _mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; -- cgit v1.2.3 From a08b7a9f359cc0cf9c6d0631bb844b54f50adcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:40 +0200 Subject: enginefailure: set mission item --- src/modules/navigator/enginefailure.cpp | 30 +++++++++++++++--------------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index ea654c5fd..de567f0dc 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -104,21 +104,21 @@ EngineFailure::set_ef_item() case EF_STATE_LOITERDOWN: { //XXX create mission item at ground (below?) here - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + //XXX setting altitude to a very low value, evaluate other options + _mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); break; } default: -- cgit v1.2.3 From 303664d94af4949bcc7f1c3393f82eb242a60c5e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:49:22 +0200 Subject: add landengfail nav state --- src/modules/navigator/navigator_main.cpp | 3 +++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 4 insertions(+) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 21ab691ff..043d883b2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -393,6 +393,9 @@ Navigator::task_main() _navigation_mode = &_rtl; /* TODO: change this to something else */ } break; + case NAVIGATION_STATE_AUTO_LANDENGFAIL: + _navigation_mode = &_engineFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index eda1dfaec..c2926dce8 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -103,6 +103,7 @@ typedef enum { NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ + NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From 1b2f9070ea063255aca4a882e4adda7f89e082e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 22:29:04 +0200 Subject: engine_failure flag Added engine_failure flag to behicle status, alsoset_nav_state in the state machine helper takes this flag into account --- src/modules/commander/state_machine_helper.cpp | 19 ++++++++++++++----- src/modules/uORB/topics/vehicle_status.h | 6 ++++-- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4e1cfb987..bc6803596 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -490,9 +490,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; @@ -532,8 +535,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe if datalink and RC is lost */ - if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + /* go into failsafe on a engine failure or if datalink and RC is lost */ + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { @@ -586,8 +591,12 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_RTL: - /* require global position and home */ - if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { + /* require global position and home, also go into failsafe on an engine failure */ + + if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if ((!status->condition_global_position_valid || + !status->condition_home_position_valid)) { status->failsafe = true; if (status->condition_local_position_valid) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c2926dce8..a47488621 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -201,8 +201,10 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ - bool data_link_lost; /**< datalink to GCS lost */ - uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + bool data_link_lost; /**< datalink to GCS lost */ + uint8_t data_link_lost_counter; /**< counts unique data link lost events */ + + bool engine_failure; /** Set to true if an engine failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From 351598626054e5d853e8768d2f5d04226510c12a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:44:38 +0200 Subject: define gps loss failsafe nav state --- src/modules/uORB/topics/vehicle_status.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index a47488621..e0072b52d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -104,6 +104,7 @@ typedef enum { NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ + NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ NAVIGATION_STATE_ACRO, /**< Acro mode */ NAVIGATION_STATE_LAND, /**< Land mode */ NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ -- cgit v1.2.3 From db5d668439be63e4c8fd7dab49b81c5e162ee095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Jul 2014 00:55:43 +0200 Subject: add simplistic gps failure detection --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 1 + 2 files changed, 17 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fe1974c88..50beb5da8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1135,6 +1135,22 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed); /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 707abb545..0751b57fe 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,7 @@ struct vehicle_status_s { uint8_t data_link_lost_counter; /**< counts unique data link lost events */ bool engine_failure; /** Set to true if an engine failure is detected */ + bool gps_failure; /** Set to true if a gps failure is detected */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From 9ee6ab366d2a7c6f24e2f08021da9cd861663bdc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 17:30:05 +0200 Subject: add flighttermination circuit breaker --- src/modules/systemlib/circuit_breaker.c | 13 +++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 2 files changed, 14 insertions(+) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64317a18a..8a0b51b71 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -95,6 +95,19 @@ PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); */ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); +/** + * Circuit breaker for flight termination + * + * Setting this parameter to 121212 will disable the flight termination action. + * --> The IO driver will not do flight termination if requested by the FMU + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 121212 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index b60584608..54c4fced6 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,6 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 +#define CBRK_FLIGHTTERMINATION_KEY 121212 #include -- cgit v1.2.3 From 0f2b66fa8b3782c965966e9c0b0b6f16b80d7f38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 22:38:48 +0200 Subject: failsafe: enable support for commands --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ src/modules/commander/state_machine_helper.cpp | 11 ++++++++++- src/modules/navigator/navigator_main.cpp | 2 +- src/modules/uORB/topics/vehicle_status.h | 5 ++++- 4 files changed, 40 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c6f56d5e3..b635a22cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -557,6 +557,31 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s armed_local->force_failsafe = false; warnx("disabling failsafe"); } + /* param2 is currently used for other failsafe modes */ + status_local->engine_failure_cmd = false; + status_local->data_link_lost_cmd = false; + status_local->gps_failure_cmd = false; + status_local->rc_signal_lost_cmd = false; + if ((int)cmd->param2 <= 0) { + /* reset all commanded failure modes */ + warnx("revert to normal mode"); + } else if ((int)cmd->param2 == 1) { + /* trigger engine failure mode */ + status_local->engine_failure_cmd = true; + warnx("engine failure mode commanded"); + } else if ((int)cmd->param2 == 2) { + /* trigger data link loss mode */ + status_local->data_link_lost_cmd = true; + warnx("data link loss mode commanded"); + } else if ((int)cmd->param2 == 3) { + /* trigger gps loss mode */ + status_local->gps_failure_cmd = true; + warnx("gps loss mode commanded"); + } else if ((int)cmd->param2 == 4) { + /* trigger rc loss mode */ + status_local->rc_signal_lost_cmd = true; + warnx("rc loss mode commanded"); + } cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index ecfe62e03..157e35ef8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -491,10 +491,19 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_AUTO_MISSION: /* go into failsafe + * - if commanded to do so * - if we have an engine failure * - if either the datalink is enabled and lost as well as RC is lost * - if there is no datalink and the mission is finished */ - if (status->engine_failure) { + if (status->engine_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->data_link_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + //} else if (status->gps_failure_cmd) { + //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->rc_signal_lost_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 043d883b2..4af37fa3e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -384,7 +384,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 301503b82..b465f8407 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -200,13 +200,16 @@ struct vehicle_status_s { bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ + bool rc_signal_lost_cmd; /**< true if RC lost mode is commanded */ bool rc_input_blocked; /**< set if RC input should be ignored */ bool data_link_lost; /**< datalink to GCS lost */ + bool data_link_lost_cmd; /**< datalink to GCS lost mode commanded */ uint8_t data_link_lost_counter; /**< counts unique data link lost events */ - bool engine_failure; /** Set to true if an engine failure is detected */ + bool engine_failure_cmd; /** Set to true if an engine failure mode is commanded */ bool gps_failure; /** Set to true if a gps failure is detected */ + bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ bool offboard_control_signal_found_once; bool offboard_control_signal_lost; -- cgit v1.2.3 From 6fe0482b277cd7120878787171c735e2d424c1c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 00:07:38 +0200 Subject: failsafe: make warnx more clear --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b635a22cb..4f996b526 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -552,10 +552,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd->param1 > 0.5f) { //XXX update state machine? armed_local->force_failsafe = true; - warnx("forcing failsafe"); + warnx("forcing failsafe (termination)"); } else { armed_local->force_failsafe = false; - warnx("disabling failsafe"); + warnx("disabling failsafe (termination)"); } /* param2 is currently used for other failsafe modes */ status_local->engine_failure_cmd = false; -- cgit v1.2.3 From 85d8781bc32fc25b30350aab518a8b9823cce661 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:04 +0200 Subject: datalinkloss: improve logic --- src/modules/navigator/datalinkloss.cpp | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 19f335633..6c5012bfd 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "CH_T"), - _param_commsholdlat(this, "CH_LAT"), - _param_commsholdlon(this, "CH_LON"), - _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), - _param_numberdatalinklosses(this, "DLL_N"), + _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), + _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), + _param_commsholdlon(this, "NAV_DLL_CH_LON", false), + _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), + _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), + _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), + _param_numberdatalinklosses(this, "NAV_DLL_N", false), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -81,7 +81,7 @@ DataLinkLoss::~DataLinkLoss() void DataLinkLoss::on_inactive() { - /* reset RTL state only if setpoint moved */ + /* reset DLL state only if setpoint moved */ if (!_navigator->get_can_loiter_at_sp()) { _dll_state = DLL_STATE_NONE; } @@ -90,7 +90,8 @@ DataLinkLoss::on_inactive() void DataLinkLoss::on_activation() { - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + _dll_state = DLL_STATE_NONE; + advance_dll(); set_dll_item(); } @@ -131,6 +132,10 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); + warnx("mission item: lat %.7f lon %.7f alt %.3f", + _mission_item.lat, + _mission_item.lon, + (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -159,6 +164,10 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + warnx("triplet current: lat %.7f lon %.7f alt %.3f", + pos_sp_triplet->current.lat, + pos_sp_triplet->current.lon, + (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } @@ -166,18 +175,21 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { + warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("too many data link losses, fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { + warnx("fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: - //XXX check here if time is over are over + warnx("fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); -- cgit v1.2.3 From 8bc7d7f43dd350c19993cd12b514a6a0b5366b3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:43 +0200 Subject: navigator: correctly use all navigator modes --- src/modules/navigator/navigator.h | 3 +-- src/modules/navigator/navigator_main.cpp | 2 ++ 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index fe6639dfe..536977972 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -65,9 +65,8 @@ /** * Number of navigation modes that need on_active/on_inactive calls - * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 6 class Navigator : public control::SuperBlock { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4af37fa3e..18bfc2cec 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -141,6 +141,8 @@ Navigator::Navigator() : _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; _navigation_mode_array[3] = &_offboard; + _navigation_mode_array[4] = &_dataLinkLoss; + _navigation_mode_array[5] = &_engineFailure; updateParams(); } -- cgit v1.2.3 From b71778d7e1d03adf8b1366982f0e86753ac072be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:41:48 +0200 Subject: datalinkloss: change default param value --- src/modules/navigator/datalinkloss_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 038c80a1a..77a8763cb 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -67,7 +67,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, 266072120); +PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120); /** * Comms hold Lon @@ -100,7 +100,7 @@ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); * @min 0.0 * @group DLL */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, 265847810); +PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); /** * Airfield home Lon -- cgit v1.2.3 From 8dbe6a6dc0d753a0f092fc2d4be508e48429c008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 14:35:45 +0200 Subject: px4io driver: use flighttermination circuit breaker --- src/drivers/px4io/px4io.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09..afb03789f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1169,7 +1169,8 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN; } - if (armed.force_failsafe) { + /* Do not set failsafe if circuit breaker is enabled */ + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; -- cgit v1.2.3 From 5406ba78a8de82c49aa9ea8e033c82670950259d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:53:03 +0200 Subject: make navigator mode a child of navigator --- src/modules/navigator/datalinkloss.cpp | 19 +++++++++++-------- src/modules/navigator/mission.cpp | 6 +++--- src/modules/navigator/navigator_mode.cpp | 2 +- src/modules/navigator/rtl.cpp | 6 +++--- 4 files changed, 18 insertions(+), 15 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 6c5012bfd..2efca0a94 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -58,14 +58,14 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), - _param_commsholdwaittime(this, "NAV_DLL_CH_T", false), - _param_commsholdlat(this, "NAV_DLL_CH_LAT", false), - _param_commsholdlon(this, "NAV_DLL_CH_LON", false), - _param_commsholdalt(this, "NAV_DLL_CH_ALT", false), - _param_airfieldhomelat(this, "NAV_DLL_AH_LAT", false), - _param_airfieldhomelon(this, "NAV_DLL_AH_LON", false), - _param_airfieldhomealt(this, "NAV_DLL_AH_ALT", false), - _param_numberdatalinklosses(this, "NAV_DLL_N", false), + _param_commsholdwaittime(this, "CH_T"), + _param_commsholdlat(this, "CH_LAT"), + _param_commsholdlon(this, "CH_LON"), + _param_commsholdalt(this, "CH_ALT"), + _param_airfieldhomelat(this, "AH_LAT"), + _param_airfieldhomelon(this, "AH_LON"), + _param_airfieldhomealt(this, "AH_ALT"), + _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -182,14 +182,17 @@ DataLinkLoss::advance_dll() updateSubscriptions(); if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { warnx("too many data link losses, fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { warnx("fly to comms hold"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c76192f66..79865e2ae 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -59,9 +59,9 @@ Mission::Mission(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _param_onboard_enabled(this, "ONBOARD_EN"), - _param_takeoff_alt(this, "TAKEOFF_ALT"), - _param_dist_1wp(this, "DIST_1WP"), + _param_onboard_enabled(this, "MIS_ONBOARD_EN", false), + _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), + _param_dist_1wp(this, "MIS_DIST_1WP", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index 3c6754c55..d91f7ab18 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -43,7 +43,7 @@ #include "navigator.h" NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : - SuperBlock(NULL, name), + SuperBlock(navigator, name), _navigator(navigator), _first_run(true) { diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 142a73409..b6c4b8fdf 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,9 +58,9 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), - _param_return_alt(this, "RETURN_ALT"), - _param_descend_alt(this, "DESCEND_ALT"), - _param_land_delay(this, "LAND_DELAY") + _param_return_alt(this, "RTL_RETURN_ALT", false), + _param_descend_alt(this, "RTL_DESCEND_ALT", false), + _param_land_delay(this, "RTL_LAND_DELAY", false) { /* load initial params */ updateParams(); -- cgit v1.2.3 From 2791a7097686d327e91ac31c20716bb602011d65 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:57:21 +0200 Subject: remove warnx --- src/modules/navigator/datalinkloss.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 2efca0a94..7b9a7b151 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -132,10 +132,6 @@ DataLinkLoss::set_dll_item() _mission_item.origin = ORIGIN_ONBOARD; _navigator->set_can_loiter_at_sp(true); - warnx("mission item: lat %.7f lon %.7f alt %.3f", - _mission_item.lat, - _mission_item.lon, - (double)_mission_item.altitude); break; } case DLL_STATE_FLYTOAIRFIELDHOMEWP: { @@ -164,10 +160,6 @@ DataLinkLoss::set_dll_item() /* convert mission item to current position setpoint and make it valid */ mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; - warnx("triplet current: lat %.7f lon %.7f alt %.3f", - pos_sp_triplet->current.lat, - pos_sp_triplet->current.lon, - (double)pos_sp_triplet->current.alt); _navigator->set_position_setpoint_triplet_updated(); } -- cgit v1.2.3 From 35daef948bb6dac06900d7bc74aa09fe35aceabd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 10:52:01 +0200 Subject: fix datalink loss detection logic --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 28aba759f..8a12e16ca 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1534,6 +1534,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; have_link = true; + } else if (!telemetry_lost[i]) { + /* telemetry was healthy also in last iteration + * we don't have to check a timeout */ + have_link = true; } } else { -- cgit v1.2.3 From a972a2857197706ed414b674dcaf291d48ac0f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 11:41:31 +0200 Subject: fix param name --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 3d1e231c6..30159dad9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -104,7 +104,7 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); * @min 0 * @max 1 */ -PARAM_DEFINE_INT32(DL_LOSS_EN, 0); +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); /** Datalink loss time threshold * -- cgit v1.2.3 From 72beef90c9a9bb901355483e275a47f166f654a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:09:10 +0200 Subject: set correct nav state for data link loss --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 157e35ef8..f82eec695 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -510,7 +510,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { -- cgit v1.2.3 From 94765f1fe01a3ae1ba72a330f77cfb4cacbd8c0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:17:26 +0200 Subject: datalinkloss: use vstatus from navigator For some reason the own subscription did not work (the task launch pattern used for the navigator may be the reason again) --- src/modules/navigator/datalinkloss.cpp | 14 +++++--------- src/modules/navigator/datalinkloss.h | 3 --- 2 files changed, 5 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 7b9a7b151..ab9e67a33 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -57,7 +57,6 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _vehicleStatus(&getSubscriptions(), ORB_ID(vehicle_status), 100), _param_commsholdwaittime(this, "CH_T"), _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), @@ -91,6 +90,7 @@ void DataLinkLoss::on_activation() { _dll_state = DLL_STATE_NONE; + updateParams(); advance_dll(); set_dll_item(); } @@ -99,6 +99,7 @@ void DataLinkLoss::on_active() { if (is_mission_item_reached()) { + updateParams(); advance_dll(); set_dll_item(); } @@ -109,9 +110,6 @@ DataLinkLoss::set_dll_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - /* make sure we have the latest params */ - updateParams(); - set_previous_pos_setpoint(); _navigator->set_can_loiter_at_sp(false); @@ -167,17 +165,15 @@ DataLinkLoss::set_dll_item() void DataLinkLoss::advance_dll() { - warnx("dll_state %u", _dll_state); switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - updateSubscriptions(); - if (_vehicleStatus.data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("too many data link losses, fly to airfield home"); + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } else { - warnx("fly to comms hold"); + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 5a46b5700..96b4ce010 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -64,9 +64,6 @@ public: virtual void on_active(); private: - /* Subscriptions */ - uORB::Subscription _vehicleStatus; - /* Params */ control::BlockParamFloat _param_commsholdwaittime; control::BlockParamInt _param_commsholdlat; // * 1e7 -- cgit v1.2.3 From 14189816f7e751b14725939fd5a200c39818f5df Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:28:39 +0200 Subject: dlloss: better output --- src/modules/navigator/datalinkloss.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index ab9e67a33..b914fdb34 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -180,7 +180,7 @@ DataLinkLoss::advance_dll() break; case DLL_STATE_FLYTOCOMMSHOLDWP: warnx("fly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home"); _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); -- cgit v1.2.3 From f480c10282efcb56b643b2c676303f1f57498fda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:37:46 +0200 Subject: state machine helper: use stay in failsafe flag --- src/modules/commander/state_machine_helper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f82eec695..3f4bfaa1c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -534,11 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } /* don't bother if RC is lost and mission is not yet finished */ - } else if (status->rc_signal_lost) { + } else if (status->rc_signal_lost && !stay_in_failsafe) { /* this mode is ok, we don't need RC for missions */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; - } else { + } else if (!stay_in_failsafe){ /* everything is perfect */ status->nav_state = NAVIGATION_STATE_AUTO_MISSION; } -- cgit v1.2.3 From ab9b234893448237d84b4e1f95b807be3a68e98f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 18:07:21 +0200 Subject: commander: mavlink output on flight termination --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8a12e16ca..2ab40a5eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1313,9 +1313,10 @@ int commander_thread_main(int argc, char *argv[]) /* Check for geofence violation */ if (pos_sp_triplet.geofence_violated) { //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present */ + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; + mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From b1008842204418f5d8cd0475547ecfb8f378b4c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:07:02 +0200 Subject: geofence: support AMSL mode --- src/modules/navigator/geofence.cpp | 22 ++++++++++++------ src/modules/navigator/geofence.h | 40 +++++++++++++++++++++----------- src/modules/navigator/geofence_params.c | 12 ++++++++++ src/modules/navigator/navigator.h | 8 +++++++ src/modules/navigator/navigator_main.cpp | 27 +++++++++++++++++++-- 5 files changed, 86 insertions(+), 23 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 266215308..2b9ce752b 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -62,7 +62,8 @@ Geofence::Geofence() : _altitude_min(0), _altitude_max(0), _verticesCount(0), - param_geofence_on(this, "ON") + _param_geofence_on(this, "ON"), + _param_altitude_mode(this, "ALTMODE") { /* Load initial params */ updateParams(); @@ -74,19 +75,26 @@ Geofence::~Geofence() } -bool Geofence::inside(const struct vehicle_global_position_s *vehicle) +bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; - return inside(lat, lon, vehicle->alt); + return inside(lat, lon, global_position.alt); +} + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { + + double lat = global_position.lat / 1e7d; + double lon = global_position.lon / 1e7d; + + return inside(lat, lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ - if (param_geofence_on.get() != 1) + if (_param_geofence_on.get() != 1) return true; if (valid()) { diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 2eb126ab5..e2c0f08d8 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -42,6 +42,8 @@ #define GEOFENCE_H_ #include +#include +#include #include #include @@ -49,29 +51,25 @@ class Geofence : public control::SuperBlock { -private: - orb_advert_t _fence_pub; /**< publish fence topic */ - - float _altitude_min; - float _altitude_max; - - unsigned _verticesCount; - - /* Params */ - control::BlockParamInt param_geofence_on; public: Geofence(); ~Geofence(); + /* Altitude mode, corresponding to the param GF_ALTMODE */ + enum { + GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_AMSL = 1 + }; + /** - * Return whether craft is inside geofence. + * Return whether system 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 + * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s *craft); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); int clearDm(); @@ -88,6 +86,20 @@ public: int loadFromFile(const char *filename); bool isEmpty() {return _verticesCount == 0;} + + int getAltitudeMode() { return _param_altitude_mode.get(); } + +private: + orb_advert_t _fence_pub; /**< publish fence topic */ + + float _altitude_min; + float _altitude_max; + + unsigned _verticesCount; + + /* Params */ + control::BlockParamInt _param_geofence_on; + control::BlockParamInt _param_altitude_mode; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 653b1ad84..29b42cd54 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -58,3 +58,15 @@ * @group Geofence */ PARAM_DEFINE_INT32(GF_ON, 1); + +/** + * Geofence altitude mode + * + * Select which altitude reference should be used + * 0 = GPS, 1 = AMSL + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_ALTMODE, 1); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 536977972..59a6752a9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -120,6 +120,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } @@ -141,6 +142,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ @@ -156,6 +158,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ navigation_capabilities_s _nav_caps; /**< navigation capabilities */ @@ -195,6 +198,11 @@ private: */ void global_position_update(); + /** + * Retrieve sensor values + */ + void sensor_combined_update(); + /** * Retrieve home position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 18bfc2cec..d77acf74e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -112,6 +113,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _sensor_combined{}, _home_pos{}, _mission_item{}, _nav_caps{}, @@ -178,6 +180,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::sensor_combined_update() +{ + orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); +} + void Navigator::home_position_update() { @@ -248,6 +256,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -261,6 +270,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + sensor_combined_update(); home_position_update(); navigation_capabilities_update(); params_update(); @@ -272,7 +282,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[6]; + struct pollfd fds[7]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -287,6 +297,8 @@ Navigator::task_main() fds[4].events = POLLIN; fds[5].fd = _param_update_sub; fds[5].events = POLLIN; + fds[6].fd = _sensor_combined_sub; + fds[6].events = POLLIN; while (!_task_should_exit) { @@ -311,6 +323,11 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* sensors combined updated */ + if (fds[6].revents & POLLIN) { + sensor_combined_update(); + } + /* parameters updated */ if (fds[5].revents & POLLIN) { params_update(); @@ -342,7 +359,13 @@ Navigator::task_main() global_position_update(); /* Check geofence violation */ - if (!_geofence.inside(&_global_pos)) { + bool inside = false; + if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { + inside = _geofence.inside(_global_pos); + } else { + inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + } + if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; if (_pos_sp_triplet.geofence_violated != true) { -- cgit v1.2.3 From 5832948371866aec8f0c7f16b13869f270d36aad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:37:14 +0200 Subject: geofence: lat/lon is double types changed but the geofence implentation was not updated, this was forgotten in 58792c5ca6e42bc251dd3c92b0e79217ff5d5403 --- src/modules/navigator/geofence.cpp | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2b9ce752b..7897c7ba0 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -77,18 +77,12 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s &global_position) { - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, global_position.alt); + return inside(global_position.lat, global_position.lon, global_position.alt); } -bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) { - - double lat = global_position.lat / 1e7d; - double lon = global_position.lon / 1e7d; - - return inside(lat, lon, baro_altitude_amsl); +bool Geofence::inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl) +{ + return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } bool Geofence::inside(double lat, double lon, float altitude) @@ -101,8 +95,9 @@ bool Geofence::inside(double lat, double lon, float altitude) if (!isEmpty()) { /* Vertical check */ - if (altitude > _altitude_max || altitude < _altitude_min) + if (altitude > _altitude_max || altitude < _altitude_min) { return false; + } /*Horizontal check */ /* Adaptation of algorithm originally presented as -- cgit v1.2.3 From 99860da9b70bfa87ef2834efa5e7b9ba96ee4e9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 16:36:35 +0200 Subject: commander: remove unnecessary output (navigator already does the output) --- src/modules/commander/commander.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2ab40a5eb..84a4be948 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,6 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - mavlink_log_emergency(mavlink_fd, "Geofence violated: terminating"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 7f9c996555975301288da58745f69b39f05facbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:30:19 +0200 Subject: engine fail: small state machine fix --- src/modules/navigator/enginefailure.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index de567f0dc..e007b208b 100644 --- a/src/modules/navigator/enginefailure.cpp +++ b/src/modules/navigator/enginefailure.cpp @@ -76,7 +76,8 @@ EngineFailure::on_inactive() void EngineFailure::on_activation() { - _ef_state = EF_STATE_LOITERDOWN; + _ef_state = EF_STATE_NONE; + advance_ef(); set_ef_item(); } @@ -139,6 +140,7 @@ EngineFailure::advance_ef() { switch (_ef_state) { case EF_STATE_NONE: + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down"); _ef_state = EF_STATE_LOITERDOWN; break; default: -- cgit v1.2.3 From 64ca94412e710164fb2ae69f6dfc3edbeff9c12b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:31:55 +0200 Subject: engine fail: fw pos control limits pitch and sets 0 throttle --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 65 +++++++++++++++++----- 1 file changed, 52 insertions(+), 13 deletions(-) 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 350ce6dec..0c35ccb2c 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 @@ -139,7 +139,8 @@ private: int _pos_sp_triplet_sub; int _att_sub; /**< vehicle attitude subscription */ int _airspeed_sub; /**< airspeed subscription */ - int _control_mode_sub; /**< vehicle status subscription */ + int _control_mode_sub; /**< control mode subscription */ + int _vehicle_status_sub; /**< vehicle status subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ @@ -154,7 +155,8 @@ private: struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ - struct vehicle_control_mode_s _control_mode; /**< vehicle status */ + struct vehicle_control_mode_s _control_mode; /**< control mode */ + struct vehicle_status_s _vehicle_status; /**< vehicle status */ struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ @@ -301,10 +303,15 @@ private: void control_update(); /** - * Check for changes in vehicle status. + * Check for changes in control mode */ void vehicle_control_mode_poll(); + /** + * Check for changes in vehicle status. + */ + void vehicle_status_poll(); + /** * Check for airspeed updates. */ @@ -408,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _att_sub(-1), _airspeed_sub(-1), _control_mode_sub(-1), + _vehicle_status_sub(-1), _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), @@ -425,6 +433,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _manual(), _airspeed(), _control_mode(), + _vehicle_status(), _global_pos(), _pos_sp_triplet(), _sensor_combined(), @@ -627,16 +636,27 @@ FixedwingPositionControl::parameters_update() void FixedwingPositionControl::vehicle_control_mode_poll() { - bool vstatus_updated; + bool updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_control_mode_sub, &vstatus_updated); + orb_check(_control_mode_sub, &updated); - if (vstatus_updated) { + if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } } +void +FixedwingPositionControl::vehicle_status_poll() +{ + bool updated; + + orb_check(_vehicle_status_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); + } +} + bool FixedwingPositionControl::vehicle_airspeed_poll() { @@ -1182,10 +1202,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - if (usePreTakeoffThrust) { - _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Set thrrust to 0 to minimize damage */ + _att_sp.thrust = 0.0f; } - else { + else if (usePreTakeoffThrust) { + _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else { _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : _tecs.get_throttle_demand(), throttle_max); } _att_sp.pitch_body = _mTecs.getEnabled() ? _mTecs.getPitchSetpoint() : _tecs.get_pitch_demand(); @@ -1212,13 +1235,16 @@ FixedwingPositionControl::task_main() _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - /* rate limit vehicle status updates to 5Hz */ + /* rate limit control mode updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); + /* rate limit vehicle status updates to 5Hz */ + orb_set_interval(_vehicle_status_sub, 200); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); @@ -1257,9 +1283,12 @@ FixedwingPositionControl::task_main() perf_begin(_loop_perf); - /* check vehicle status for changes to publication state */ + /* check vehicle control mode for changes to publication state */ vehicle_control_mode_poll(); + /* check vehicle status for changes to publication state */ + vehicle_status_poll(); + /* only update parameters if they changed */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -1372,7 +1401,12 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); } fwPosctrl::LimitOverride limitOverride; - if (climbout_mode) { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + limitOverride.enablePitchMinOverride(-1.0f); + limitOverride.enablePitchMaxOverride(5.0f); + + } else if (climbout_mode) { limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); } else { limitOverride.disablePitchMinOverride(); @@ -1380,6 +1414,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { + /* Force the slow downwards spiral */ + pitch_min_rad = M_DEG_TO_RAD_F * -1.0f; + pitch_max_rad = M_DEG_TO_RAD_F * 5.0f; + } /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, -- cgit v1.2.3 From 760a7ff548bcef6910f84beaa981600f3609358b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 07:45:01 +0200 Subject: gpsfailure: add skeleton class, activate in commander --- src/modules/commander/state_machine_helper.cpp | 7 +- src/modules/navigator/datalinkloss.cpp | 2 +- src/modules/navigator/gpsfailure.cpp | 170 +++++++++++++++++++++++++ src/modules/navigator/gpsfailure.h | 86 +++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 4 +- src/modules/navigator/navigator_main.cpp | 5 + 7 files changed, 272 insertions(+), 5 deletions(-) create mode 100644 src/modules/navigator/gpsfailure.cpp create mode 100644 src/modules/navigator/gpsfailure.h diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3f4bfaa1c..404bafb04 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -499,12 +499,15 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->data_link_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; - //} else if (status->gps_failure_cmd) { - //status->nav_state = NAVIGATION_STATE_AUTO_***; + } else if (status->gps_failure_cmd) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX + /* Finished handling commands which have priority , now handle failures */ } else if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; + } else if (status->gps_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index b914fdb34..d8a1de229 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** * @file datalinkloss.cpp - * Helper class for Data Link Loss Mode acording to the OBC rules + * Helper class for Data Link Loss Mode according to the OBC rules * * @author Thomas Gubler */ diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp new file mode 100644 index 000000000..4c526b76e --- /dev/null +++ b/src/modules/navigator/gpsfailure.cpp @@ -0,0 +1,170 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file gpsfailure.cpp + * Helper class for gpsfailure mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "gpsfailure.h" + +#define DELAY_SIGMA 0.01f + +GpsFailure::GpsFailure(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _gpsf_state(GPSF_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +GpsFailure::~GpsFailure() +{ +} + +void +GpsFailure::on_inactive() +{ + /* reset GPSF state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _gpsf_state = GPSF_STATE_NONE; + } +} + +void +GpsFailure::on_activation() +{ + _gpsf_state = GPSF_STATE_NONE; + updateParams(); + advance_gpsf(); + set_gpsf_item(); +} + +void +GpsFailure::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_gpsf(); + set_gpsf_item(); + } +} + +void +GpsFailure::set_gpsf_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_commsholdalt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + case GPSF_STATE_TERMINATE: { + //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; + //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; + //_mission_item.altitude_is_relative = false; + //_mission_item.altitude = _param_airfieldhomealt.get(); + //_mission_item.yaw = NAN; + //_mission_item.loiter_radius = _navigator->get_loiter_radius(); + //_mission_item.loiter_direction = 1; + //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + //_mission_item.pitch_min = 0.0f; + //_mission_item.autocontinue = true; + //_mission_item.origin = ORIGIN_ONBOARD; + + //_navigator->set_can_loiter_at_sp(true); + break; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +GpsFailure::advance_gpsf() +{ + switch (_gpsf_state) { + case GPSF_STATE_NONE: + _gpsf_state = GPSF_STATE_LOITER; + break; + case GPSF_STATE_LOITER: + _gpsf_state = GPSF_STATE_TERMINATE; + break; + default: + break; + } +} diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h new file mode 100644 index 000000000..88d386ec4 --- /dev/null +++ b/src/modules/navigator/gpsfailure.h @@ -0,0 +1,86 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file gpsfailure.h + * Helper class for Data Link Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_GPSFAILURE_H +#define NAVIGATOR_GPSFAILURE_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class GpsFailure : public MissionBlock +{ +public: + GpsFailure(Navigator *navigator, const char *name); + + ~GpsFailure(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + + enum GPSFState { + GPSF_STATE_NONE = 0, + GPSF_STATE_LOITER = 1, + GPSF_STATE_TERMINATE = 2, + } _gpsf_state; + + /** + * Set the GPSF item + */ + void set_gpsf_item(); + + /** + * Move to next GPSF item + */ + void advance_gpsf(); + +}; +#endif diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 0539087df..f6590605b 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -52,7 +52,8 @@ SRCS = navigator_main.cpp \ geofence_params.c \ datalinkloss.cpp \ enginefailure.cpp \ - datalinkloss_params.c + datalinkloss_params.c \ + gpsfailure.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 59a6752a9..b161c2984 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -61,12 +61,13 @@ #include "offboard.h" #include "datalinkloss.h" #include "enginefailure.h" +#include "gpsfailure.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 6 +#define NAVIGATOR_MODE_ARRAY_SIZE 7 class Navigator : public control::SuperBlock { @@ -184,6 +185,7 @@ private: DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode (FW only!) */ + GpsFailure _gpsFailure; /**< class that handles the OBC gpsfailure loss mode */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index d77acf74e..d77825715 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -132,6 +132,7 @@ Navigator::Navigator() : _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), + _gpsFailure(this, "GPSF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -145,6 +146,7 @@ Navigator::Navigator() : _navigation_mode_array[3] = &_offboard; _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; + _navigation_mode_array[6] = &_gpsFailure; updateParams(); } @@ -421,6 +423,9 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LANDENGFAIL: _navigation_mode = &_engineFailure; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + _navigation_mode = &_gpsFailure; + break; case NAVIGATION_STATE_LAND: case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: -- cgit v1.2.3 From 1a14ff250e5a2ead69576762fd5b7f176c4b6fac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:39:44 +0200 Subject: fw att control: use RC only if in manual --- src/modules/fw_att_control/fw_att_control_main.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4..ee112a40e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -717,7 +717,14 @@ FixedwingAttitudeControl::task_main() float pitch_sp = _parameters.pitchsp_offset_rad; float throttle_sp = 0.0f; - if (_vcontrol_mode.flag_control_velocity_enabled || _vcontrol_mode.flag_control_position_enabled) { + /* Read attitude setpoint from uorb if + * - velocity control or position control is enabled (pos controller is running) + * - manual control is disabled (another app may send the setpoint, but it should + * for sure not be set from the remote control values) + */ + if (_vcontrol_mode.flag_control_velocity_enabled || + _vcontrol_mode.flag_control_position_enabled || + !_vcontrol_mode.flag_control_manual_enabled) { /* read in attitude setpoint from attitude setpoint uorb topic */ roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad; pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad; -- cgit v1.2.3 From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:40:45 +0200 Subject: add obc gps failure mode --- src/modules/commander/commander.cpp | 18 +++- src/modules/navigator/gpsfailure.cpp | 95 +++++++++++---------- src/modules/navigator/gpsfailure.h | 13 ++- src/modules/navigator/gpsfailure_params.c | 97 ++++++++++++++++++++++ src/modules/navigator/module.mk | 3 +- src/modules/navigator/navigator.h | 12 +++ src/modules/navigator/navigator_main.cpp | 16 ++++ .../uORB/topics/position_setpoint_triplet.h | 1 + 8 files changed, 207 insertions(+), 48 deletions(-) create mode 100644 src/modules/navigator/gpsfailure_params.c diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 84a4be948..109ab403a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,11 +1311,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated) { + if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; + armed.fosrce_failsafe = true; status_changed = true; + warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -2060,6 +2061,7 @@ set_control_mode() case NAVIGATION_STATE_AUTO_LOITER: case NAVIGATION_STATE_AUTO_RTL: case NAVIGATION_STATE_AUTO_RTGS: + case NAVIGATION_STATE_AUTO_LANDENGFAIL: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; @@ -2071,6 +2073,18 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_AUTO_LANDGPSFAIL: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 4c526b76e..02e766ffb 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -57,7 +57,12 @@ GpsFailure::GpsFailure(Navigator *navigator, const char *name) : MissionBlock(navigator, name), - _gpsf_state(GPSF_STATE_NONE) + _param_loitertime(this, "LT"), + _param_openlooploiter_roll(this, "R"), + _param_openlooploiter_pitch(this, "P"), + _param_openlooploiter_thrust(this, "TR"), + _gpsf_state(GPSF_STATE_NONE), + _timestamp_activation(0) { /* load initial params */ updateParams(); @@ -82,6 +87,7 @@ void GpsFailure::on_activation() { _gpsf_state = GPSF_STATE_NONE; + _timestamp_activation = hrt_absolute_time(); updateParams(); advance_gpsf(); set_gpsf_item(); @@ -90,10 +96,34 @@ GpsFailure::on_activation() void GpsFailure::on_active() { - if (is_mission_item_reached()) { - updateParams(); - advance_gpsf(); + + switch (_gpsf_state) { + case GPSF_STATE_LOITER: { + /* Position controller does not run in this mode: + * navigator has to publish an attitude setpoint */ + _navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get(); + _navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get(); + _navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get(); + _navigator->publish_att_sp(); + + /* Measure time */ + hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation); + + //warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f", + //_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get()); + if (elapsed > _param_loitertime.get() * 1e6f) { + /* no recovery, adavance the state machine */ + warnx("gps not recovered, switch to next state"); + advance_gpsf(); + } + break; + } + case GPSF_STATE_TERMINATE: set_gpsf_item(); + advance_gpsf(); + break; + default: + break; } } @@ -102,68 +132,45 @@ GpsFailure::set_gpsf_item() { struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - set_previous_pos_setpoint(); - _navigator->set_can_loiter_at_sp(false); + /* Set pos sp triplet to invalid to stop pos controller */ + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; switch (_gpsf_state) { - case GPSF_STATE_LOITER: { - //_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_commsholdalt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; - } case GPSF_STATE_TERMINATE: { - //_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7; - //_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7; - //_mission_item.altitude_is_relative = false; - //_mission_item.altitude = _param_airfieldhomealt.get(); - //_mission_item.yaw = NAN; - //_mission_item.loiter_radius = _navigator->get_loiter_radius(); - //_mission_item.loiter_direction = 1; - //_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - //_mission_item.acceptance_radius = _navigator->get_acceptance_radius(); - //_mission_item.pitch_min = 0.0f; - //_mission_item.autocontinue = true; - //_mission_item.origin = ORIGIN_ONBOARD; - - //_navigator->set_can_loiter_at_sp(true); - break; + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("request flight termination"); } default: break; } reset_mission_item_reached(); - - /* convert mission item to current position setpoint and make it valid */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - pos_sp_triplet->next.valid = false; - _navigator->set_position_setpoint_triplet_updated(); } void GpsFailure::advance_gpsf() { + updateParams(); + switch (_gpsf_state) { case GPSF_STATE_NONE: _gpsf_state = GPSF_STATE_LOITER; + warnx("gpsf loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter"); break; case GPSF_STATE_LOITER: _gpsf_state = GPSF_STATE_TERMINATE; + warnx("gpsf terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination"); + warnx("mavlink sent"); break; + case GPSF_STATE_TERMINATE: + warnx("gpsf end"); + _gpsf_state = GPSF_STATE_END; default: break; } diff --git a/src/modules/navigator/gpsfailure.h b/src/modules/navigator/gpsfailure.h index 88d386ec4..e9e72babf 100644 --- a/src/modules/navigator/gpsfailure.h +++ b/src/modules/navigator/gpsfailure.h @@ -43,7 +43,11 @@ #include #include -#include +#include +#include +#include + +#include #include "navigator_mode.h" #include "mission_block.h" @@ -65,13 +69,20 @@ public: private: /* Params */ + control::BlockParamFloat _param_loitertime; + control::BlockParamFloat _param_openlooploiter_roll; + control::BlockParamFloat _param_openlooploiter_pitch; + control::BlockParamFloat _param_openlooploiter_thrust; enum GPSFState { GPSF_STATE_NONE = 0, GPSF_STATE_LOITER = 1, GPSF_STATE_TERMINATE = 2, + GPSF_STATE_END = 3, } _gpsf_state; + hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */ + /** * Set the GPSF item */ diff --git a/src/modules/navigator/gpsfailure_params.c b/src/modules/navigator/gpsfailure_params.c new file mode 100644 index 000000000..39d179eed --- /dev/null +++ b/src/modules/navigator/gpsfailure_params.c @@ -0,0 +1,97 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. 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. + * + ****************************************************************************/ + +/** + * @file gpsfailure_params.c + * + * Parameters for GPSF navigation mode + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * GPS Failure Navigation Mode parameters, accessible via MAVLink + */ + +/** + * Loiter time + * + * The amount of time in seconds the system should do open loop loiter and wait for gps recovery + * before it goes into flight termination. + * + * @unit seconds + * @min 0.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f); + +/** + * Open loop loiter roll + * + * Roll in degrees during the open loop loiter + * + * @unit deg + * @min 0.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f); + +/** + * Open loop loiter pitch + * + * Pitch in degrees during the open loop loiter + * + * @unit deg + * @min -30.0 + * @max 30.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f); + +/** + * Open loop loiter thrust + * + * Thrust value which is set during the open loop loiter + * + * @min 0.0 + * @max 1.0 + * @group GPSF + */ +PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f); + + diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index f6590605b..9e4ad053c 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -53,7 +53,8 @@ SRCS = navigator_main.cpp \ datalinkloss.cpp \ enginefailure.cpp \ datalinkloss_params.c \ - gpsfailure.cpp + gpsfailure.cpp \ + gpsfailure_params.c INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b161c2984..ec6e538e3 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -53,6 +53,7 @@ #include #include #include +#include #include "navigator_mode.h" #include "mission.h" @@ -108,6 +109,12 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); + + /** + * Publish the attitude sp, only to be used in very special modes when position control is deactivated + * Example: mode that is triggered on gps failure + */ + void publish_att_sp(); /** * Setters @@ -125,6 +132,7 @@ public: struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } struct mission_result_s* get_mission_result() { return &_mission_result; } + struct vehicle_attitude_setpoint_s* get_att_sp() { return &_att_sp; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } @@ -155,6 +163,9 @@ private: orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ orb_advert_t _mission_result_pub; + orb_advert_t _att_sp_pub; /**< publish att sp + used only in very special failsafe modes + when pos control is deactivated */ vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ @@ -166,6 +177,7 @@ private: position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ mission_result_s _mission_result; + vehicle_attitude_setpoint_s _att_sp; bool _mission_item_valid; /**< flags if the current mission item is valid */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index bc9951bf2..e0913bb57 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -110,6 +110,7 @@ Navigator::Navigator() : _param_update_sub(-1), _pos_sp_triplet_pub(-1), _mission_result_pub(-1), + _att_sp_pub(-1), _vstatus{}, _control_mode{}, _global_pos{}, @@ -119,6 +120,7 @@ Navigator::Navigator() : _nav_caps{}, _pos_sp_triplet{}, _mission_result{}, + _att_sp{}, _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, @@ -609,3 +611,17 @@ Navigator::publish_mission_result() _mission_result.reached = false; _mission_result.finished = false; } + +void +Navigator::publish_att_sp() +{ + /* lazily publish the attitude sp only once available */ + if (_att_sp_pub > 0) { + /* publish att sp*/ + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &_att_sp); + + } else { + /* advertise and publish */ + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); + } +} diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 1c78f5330..e2b1525a2 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -98,6 +98,7 @@ struct position_setpoint_triplet_s unsigned nav_state; /**< report the navigation state */ bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** -- cgit v1.2.3 From 406a52e6399b171cfe454b85774f12f9add40aee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 20:24:04 +0200 Subject: fix typo --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 109ab403a..243546787 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1314,7 +1314,7 @@ int commander_thread_main(int argc, char *argv[]) if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.fosrce_failsafe = true; + armed.force_failsafe = true; status_changed = true; warnx("Flight termination"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination -- cgit v1.2.3 From b22acadb8a255cfdf7a8c77ea0ea84587ff7d5e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:02:33 +0200 Subject: DL loss && gps fail: flight termination --- src/modules/commander/commander.cpp | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 243546787..8ae5a441a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1316,7 +1316,7 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination"); + warnx("Flight termination because of navigator request or geofence"); } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } @@ -1579,6 +1579,17 @@ int commander_thread_main(int argc, char *argv[]) } } + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + + hrt_abstime t1 = hrt_absolute_time(); /* print new state */ -- cgit v1.2.3 From d50135b611605891cb1fb8841490c41474c3ec31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:40:58 +0200 Subject: rc loss && gps loss: flight termination --- src/modules/commander/commander.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8ae5a441a..9968ab8e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1579,6 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check for failure combinations which lead to flight termination */ /* At this point the data link and the gps system have been checked * If both failed we want to terminate the flight */ if ((status.data_link_lost && status.gps_failure) || @@ -1589,6 +1590,21 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } + hrt_abstime t1 = hrt_absolute_time(); -- cgit v1.2.3 From 4d75222b67b8ae93f28c1bbec782ce7fd0ab9919 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:41:24 +0200 Subject: switch to rc loss mode if rc loss commanded --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 404bafb04..d105e4bdf 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -451,7 +451,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en case MAIN_STATE_ALTCTL: case MAIN_STATE_POSCTL: /* require RC for all manual modes */ - if (status->rc_signal_lost && armed) { + if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) { status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { -- cgit v1.2.3 From 6ae8800ad09caf9194240ec75067af5ef56a5a23 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:05:19 +0200 Subject: move and rename params airfield home is general --- src/modules/navigator/datalinkloss.cpp | 6 ++-- src/modules/navigator/datalinkloss_params.c | 33 ---------------------- src/modules/navigator/navigator_params.c | 43 +++++++++++++++++++++++++++++ 3 files changed, 46 insertions(+), 36 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index d8a1de229..893d6d93a 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -61,9 +61,9 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_commsholdlat(this, "CH_LAT"), _param_commsholdlon(this, "CH_LON"), _param_commsholdalt(this, "CH_ALT"), - _param_airfieldhomelat(this, "AH_LAT"), - _param_airfieldhomelon(this, "AH_LON"), - _param_airfieldhomealt(this, "AH_ALT"), + _param_airfieldhomelat(this, "NAV_AH_LAT", false), + _param_airfieldhomelon(this, "NAV_AH_LON", false), + _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 77a8763cb..02f7ca4c3 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,39 +91,6 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); -/** - * Airfield home Lat - * - * Latitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LAT, -265847810); - -/** - * Airfield home Lon - * - * Longitude of airfield home waypoint - * - * @unit degrees * 1e7 - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_INT32(NAV_DLL_AH_LON, 1518423250); - -/** - * Airfield home alt - * - * Altitude of airfield home waypoint - * - * @unit m - * @min 0.0 - * @group DLL - */ -PARAM_DEFINE_FLOAT(NAV_DLL_AH_ALT, 600.0f); - /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index afaf1c3c3..1f40e634e 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -75,3 +75,46 @@ PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); * @group Mission */ PARAM_DEFINE_INT32(NAV_DLL_OBC, 0); + +/** + * Set OBC mode for rc loss + * + * If set to 1 the behaviour on data link loss is set to a mode according to the OBC rules + * + * @min 0 + * @group Mission + */ +PARAM_DEFINE_INT32(NAV_RCL_OBC, 0); + +/** + * Airfield home Lat + * + * Latitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LAT, -265847810); + +/** + * Airfield home Lon + * + * Longitude of airfield home waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_INT32(NAV_AH_LON, 1518423250); + +/** + * Airfield home alt + * + * Altitude of airfield home waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_AH_ALT, 600.0f); -- cgit v1.2.3 From fd3746a233b0ef16758e0171da0ee7e71ff58887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:06:14 +0200 Subject: add OBC RC loss mode to navigator --- src/modules/navigator/gpsfailure.cpp | 2 +- src/modules/navigator/module.mk | 4 +- src/modules/navigator/navigator.h | 8 +- src/modules/navigator/navigator_main.cpp | 13 ++- src/modules/navigator/rcloss.cpp | 172 +++++++++++++++++++++++++++++++ src/modules/navigator/rcloss.h | 88 ++++++++++++++++ src/modules/navigator/rcloss_params.c | 59 +++++++++++ 7 files changed, 339 insertions(+), 7 deletions(-) create mode 100644 src/modules/navigator/rcloss.cpp create mode 100644 src/modules/navigator/rcloss.h create mode 100644 src/modules/navigator/rcloss_params.c diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 02e766ffb..f0bbbcf1c 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -141,7 +141,7 @@ GpsFailure::set_gpsf_item() case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("request flight termination"); + warnx("gps fail: request flight termination"); } default: break; diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 9e4ad053c..c075903b7 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -51,8 +51,10 @@ SRCS = navigator_main.cpp \ geofence.cpp \ geofence_params.c \ datalinkloss.cpp \ - enginefailure.cpp \ datalinkloss_params.c \ + rcloss.cpp \ + rcloss_params.c \ + enginefailure.cpp \ gpsfailure.cpp \ gpsfailure_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index ec6e538e3..709e3e724 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -63,12 +63,13 @@ #include "datalinkloss.h" #include "enginefailure.h" #include "gpsfailure.h" +#include "rcloss.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls */ -#define NAVIGATOR_MODE_ARRAY_SIZE 7 +#define NAVIGATOR_MODE_ARRAY_SIZE 8 class Navigator : public control::SuperBlock { @@ -109,7 +110,7 @@ public: * Publish the mission result so commander and mavlink know what is going on */ void publish_mission_result(); - + /** * Publish the attitude sp, only to be used in very special modes when position control is deactivated * Example: mode that is triggered on gps failure @@ -193,6 +194,8 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ + RCLoss _rcLoss; /**< class that handles RTL according to + OBC rules (rc loss mode) */ Offboard _offboard; /**< class that handles offboard */ DataLinkLoss _dataLinkLoss; /**< class that handles the OBC datalink loss mode */ EngineFailure _engineFailure; /**< class that handles the engine failure mode @@ -207,6 +210,7 @@ private: control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ control::BlockParamInt _param_datalinkloss_obc; /**< if true: obc mode on data link loss enabled */ + control::BlockParamInt _param_rcloss_obc; /**< if true: obc mode on rc loss enabled */ /** * Retrieve global position */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e0913bb57..c569ee7b5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -131,6 +131,7 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), + _rcLoss(this, "RCL"), _offboard(this, "OFF"), _dataLinkLoss(this, "DLL"), _engineFailure(this, "EF"), @@ -139,7 +140,8 @@ Navigator::Navigator() : _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), _param_acceptance_radius(this, "ACC_RAD"), - _param_datalinkloss_obc(this, "DLL_OBC") + _param_datalinkloss_obc(this, "DLL_OBC"), + _param_rcloss_obc(this, "RCL_OBC") { /* Create a list of our possible navigation types */ _navigation_mode_array[0] = &_mission; @@ -149,6 +151,7 @@ Navigator::Navigator() : _navigation_mode_array[4] = &_dataLinkLoss; _navigation_mode_array[5] = &_engineFailure; _navigation_mode_array[6] = &_gpsFailure; + _navigation_mode_array[7] = &_rcLoss; updateParams(); } @@ -413,7 +416,11 @@ Navigator::task_main() _navigation_mode = &_loiter; break; case NAVIGATION_STATE_AUTO_RTL: - _navigation_mode = &_rtl; + if (_param_rcloss_obc.get() != 0) { + _navigation_mode = &_rcLoss; + } else { + _navigation_mode = &_rtl; + } break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param @@ -421,7 +428,7 @@ Navigator::task_main() if (_param_datalinkloss_obc.get() != 0) { _navigation_mode = &_dataLinkLoss; } else { - _navigation_mode = &_rtl; /* TODO: change this to something else */ + _navigation_mode = &_rtl; } break; case NAVIGATION_STATE_AUTO_LANDENGFAIL: diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp new file mode 100644 index 000000000..b8b659efe --- /dev/null +++ b/src/modules/navigator/rcloss.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file rcloss.cpp + * Helper class for RC Loss Mode according to the OBC rules + * + * @author Thomas Gubler + */ + +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include "navigator.h" +#include "datalinkloss.h" + +#define DELAY_SIGMA 0.01f + +RCLoss::RCLoss(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_loitertime(this, "LT"), + _rcl_state(RCL_STATE_NONE) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RCLoss::~RCLoss() +{ +} + +void +RCLoss::on_inactive() +{ + /* reset RCL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rcl_state = RCL_STATE_NONE; + } +} + +void +RCLoss::on_activation() +{ + _rcl_state = RCL_STATE_NONE; + updateParams(); + advance_rcl(); + set_rcl_item(); +} + +void +RCLoss::on_active() +{ + if (is_mission_item_reached()) { + updateParams(); + advance_rcl(); + set_rcl_item(); + } +} + +void +RCLoss::set_rcl_item() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + set_previous_pos_setpoint(); + _navigator->set_can_loiter_at_sp(false); + + switch (_rcl_state) { + case RCL_STATE_LOITER: { + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + break; + } + case RCL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("gps fail: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + } + default: + break; + } + + reset_mission_item_reached(); + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + pos_sp_triplet->next.valid = false; + + _navigator->set_position_setpoint_triplet_updated(); +} + +void +RCLoss::advance_rcl() +{ + switch (_rcl_state) { + case RCL_STATE_NONE: + /* Check the number of data link losses. If above home fly home directly */ + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + _rcl_state = RCL_STATE_LOITER; + break; + case RCL_STATE_LOITER: + _rcl_state = RCL_STATE_TERMINATE; + warnx("time is up, no RC regain, terminating"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case RCL_STATE_TERMINATE: + warnx("rcl end"); + _rcl_state = RCL_STATE_END; + break; + default: + break; + } +} diff --git a/src/modules/navigator/rcloss.h b/src/modules/navigator/rcloss.h new file mode 100644 index 000000000..bcb74d877 --- /dev/null +++ b/src/modules/navigator/rcloss.h @@ -0,0 +1,88 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. 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. + * + ****************************************************************************/ +/** + * @file rcloss.h + * Helper class for RC Loss Mode acording to the OBC rules + * + * @author Thomas Gubler + */ + +#ifndef NAVIGATOR_RCLOSS_H +#define NAVIGATOR_RCLOSS_H + +#include +#include + +#include + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RCLoss : public MissionBlock +{ +public: + RCLoss(Navigator *navigator, const char *name); + + ~RCLoss(); + + virtual void on_inactive(); + + virtual void on_activation(); + + virtual void on_active(); + +private: + /* Params */ + control::BlockParamFloat _param_loitertime; + + enum RCLState { + RCL_STATE_NONE = 0, + RCL_STATE_LOITER = 1, + RCL_STATE_TERMINATE = 2, + RCL_STATE_END = 3 + } _rcl_state; + + /** + * Set the RCL item + */ + void set_rcl_item(); + + /** + * Move to next RCL item + */ + void advance_rcl(); + +}; +#endif diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c new file mode 100644 index 000000000..83d23cf49 --- /dev/null +++ b/src/modules/navigator/rcloss_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. 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. + * + ****************************************************************************/ + +/** + * @file rcloss_params.c + * + * Parameters for RC Loss (OBC) + * + * @author Thomas Gubler + */ + +#include + +#include + +/* + * OBC RC Loss mode parameters, accessible via MAVLink + */ + +/** + * Loiter Time + * + * The amount of time in seconds the system should loiter at current position before termination + * + * @unit seconds + * @min 0.0 + * @group RCL + */ +PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); -- cgit v1.2.3 From 29715102894121f711fd40acfcf6ec8fb4fa6630 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:00:28 +0200 Subject: commander: flight termination, require arming --- src/modules/commander/commander.cpp | 50 +++++++++++++++++++------------------ 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9968ab8e6..692e5e8ec 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1311,7 +1311,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); /* Check for geofence violation */ - if (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination) { + if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { //XXX: make this configurable to select different actions (e.g. navigation modes) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; @@ -1580,29 +1580,31 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for failure combinations which lead to flight termination */ - /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); - } - - /* At this point the rc signal and the gps system have been checked - * If we are in manual (controlled with RC): - * if both failed we want to terminate the flight */ - if ((status.main_state == MAIN_STATE_ACRO || - status.main_state == MAIN_STATE_MANUAL || - status.main_state == MAIN_STATE_ALTCTL || - status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (armed.armed) { + /* At this point the data link and the gps system have been checked + * If both failed we want to terminate the flight */ + if ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd)) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } + + /* At this point the rc signal and the gps system have been checked + * If we are in manual (controlled with RC): + * if both failed we want to terminate the flight */ + if ((status.main_state == MAIN_STATE_ACRO || + status.main_state == MAIN_STATE_MANUAL || + status.main_state == MAIN_STATE_ALTCTL || + status.main_state == MAIN_STATE_POSCTL) && + ((status.rc_signal_lost && status.rc_signal_lost) || + (status.rc_signal_lost && status.gps_failure_cmd))) { + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } -- cgit v1.2.3 From cc12e6051d1ae8100c035e935fe6dd1a453ce887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:13:51 +0200 Subject: obc rcloss: set altitude --- src/modules/navigator/rcloss.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index b8b659efe..427001a01 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -110,6 +110,7 @@ RCLoss::set_rcl_item() case RCL_STATE_LOITER: { _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; _mission_item.altitude_is_relative = false; _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); @@ -127,7 +128,7 @@ RCLoss::set_rcl_item() case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ pos_sp_triplet->flight_termination = true; - warnx("gps fail: request flight termination"); + warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; @@ -152,7 +153,7 @@ RCLoss::advance_rcl() case RCL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc los, loitering"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); _rcl_state = RCL_STATE_LOITER; break; case RCL_STATE_LOITER: -- cgit v1.2.3 From ffd2fa7386f9fe3ab017d98a2b0e8e21b0975833 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:25:50 +0200 Subject: commander: fix check for rc && gps loss --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 692e5e8ec..c4b76b391 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1598,8 +1598,8 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ALTCTL || status.main_state == MAIN_STATE_POSCTL) && - ((status.rc_signal_lost && status.rc_signal_lost) || - (status.rc_signal_lost && status.gps_failure_cmd))) { + ((status.rc_signal_lost && status.gps_failure) || + (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; warnx("Flight termination because of RC signal loss && gps failure"); -- cgit v1.2.3 From 3c10b78e2044ec2cbe36d4de35c7ac8a936521ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 14:02:22 +0200 Subject: stae machine helper: remove unnecessary check for RC loss --- src/modules/commander/commander.cpp | 2 +- src/modules/commander/state_machine_helper.cpp | 15 +-------------- 2 files changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c4b76b391..0bf5cfe34 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -566,7 +566,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s status_local->rc_signal_lost_cmd = false; if ((int)cmd->param2 <= 0) { /* reset all commanded failure modes */ - warnx("revert to normal mode"); + warnx("reset all non-flighttermination failsafe commands"); } else if ((int)cmd->param2 == 1) { /* trigger engine failure mode */ status_local->engine_failure_cmd = true; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d105e4bdf..4506942ab 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -548,22 +548,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en break; case MAIN_STATE_AUTO_LOITER: - /* go into failsafe on a engine failure or if datalink and RC is lost */ + /* go into failsafe on a engine failure */ if (status->engine_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; - } else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { - status->failsafe = true; - - if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; - } else if (status->condition_local_position_valid) { - status->nav_state = NAVIGATION_STATE_LAND; - } else if (status->condition_local_altitude_valid) { - status->nav_state = NAVIGATION_STATE_DESCEND; - } else { - status->nav_state = NAVIGATION_STATE_TERMINATION; - } - /* also go into failsafe if just datalink is lost */ } else if (status->data_link_lost && data_link_loss_enabled) { status->failsafe = true; -- cgit v1.2.3 From 8262739b6219f54e7ea31de93cd81304df311ab9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:14:15 +0200 Subject: geofence: can select gps instead of global position --- src/modules/navigator/geofence.cpp | 23 ++++++++++++++++++- src/modules/navigator/geofence.h | 14 +++++++++++- src/modules/navigator/geofence_params.c | 17 ++++++++++++-- src/modules/navigator/navigator.h | 9 ++++++++ src/modules/navigator/navigator_main.cpp | 38 +++++++++++++++++++++++++------- 5 files changed, 89 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 7897c7ba0..4a02a0c3f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -63,7 +63,8 @@ Geofence::Geofence() : _altitude_max(0), _verticesCount(0), _param_geofence_on(this, "ON"), - _param_altitude_mode(this, "ALTMODE") + _param_altitude_mode(this, "ALTMODE"), + _param_source(this, "SOURCE") { /* Load initial params */ updateParams(); @@ -85,6 +86,26 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f return inside(global_position.lat, global_position.lon, baro_altitude_amsl); } + +bool Geofence::inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + (double)gps_position.alt * 1.0e-3); + } + } else { + if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + return inside(global_position, baro_altitude_amsl); + } else { + return inside((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, + baro_altitude_amsl); + } + } +} + bool Geofence::inside(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index e2c0f08d8..91c74572e 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -57,10 +58,16 @@ public: /* Altitude mode, corresponding to the param GF_ALTMODE */ enum { - GF_ALT_MODE_GPS = 0, + GF_ALT_MODE_WGS84 = 0, GF_ALT_MODE_AMSL = 1 }; + /* Source, corresponding to the param GF_SOURCE */ + enum { + GF_SOURCE_GLOBALPOS = 0, + GF_SOURCE_GPS = 1 + }; + /** * Return whether system is inside geofence. * @@ -71,6 +78,8 @@ public: bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position, + const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); int clearDm(); @@ -88,6 +97,8 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } + + int getSource() { return _param_source.get(); } private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -100,6 +111,7 @@ private: /* Params */ control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; + control::BlockParamInt _param_source; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 29b42cd54..32902ee97 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -63,10 +63,23 @@ PARAM_DEFINE_INT32(GF_ON, 1); * Geofence altitude mode * * Select which altitude reference should be used - * 0 = GPS, 1 = AMSL + * 0 = WGS84, 1 = AMSL * * @min 0 * @max 1 * @group Geofence */ -PARAM_DEFINE_INT32(GF_ALTMODE, 1); +PARAM_DEFINE_INT32(GF_ALTMODE, 0); + +/** + * Geofence source + * + * Select which position source should be used. Selecting GPS instead of global position makes sure that there is + * no dependence on the position estimator + * 0 = global position, 1 = GPS + * + * @min 0 + * @max 1 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_SOURCE, 0); diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 709e3e724..840b43f1b 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -129,6 +130,7 @@ public: struct vehicle_status_s* get_vstatus() { return &_vstatus; } struct vehicle_control_mode_s* get_control_mode() { return &_control_mode; } struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct vehicle_gps_position_s* get_gps_position() { return &_gps_pos; } struct sensor_combined_s* get_sensor_combined() { return &_sensor_combined; } struct home_position_s* get_home_position() { return &_home_pos; } struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } @@ -152,6 +154,7 @@ private: int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ int _global_pos_sub; /**< global position subscription */ + int _gps_pos_sub; /**< gps position subscription */ int _sensor_combined_sub; /**< sensor combined subscription */ int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ @@ -171,6 +174,7 @@ private: vehicle_status_s _vstatus; /**< vehicle status */ vehicle_control_mode_s _control_mode; /**< vehicle control mode */ vehicle_global_position_s _global_pos; /**< global vehicle position */ + vehicle_gps_position_s _gps_pos; /**< gps position */ sensor_combined_s _sensor_combined; /**< sensor values */ home_position_s _home_pos; /**< home position for RTL */ mission_item_s _mission_item; /**< current mission item */ @@ -216,6 +220,11 @@ private: */ void global_position_update(); + /** + * Retrieve gps position + */ + void gps_position_update(); + /** * Retrieve sensor values */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c569ee7b5..81ceaaca2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -100,6 +100,7 @@ Navigator::Navigator() : _navigator_task(-1), _mavlink_fd(-1), _global_pos_sub(-1), + _gps_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), @@ -114,6 +115,7 @@ Navigator::Navigator() : _vstatus{}, _control_mode{}, _global_pos{}, + _gps_pos{}, _sensor_combined{}, _home_pos{}, _mission_item{}, @@ -187,6 +189,12 @@ Navigator::global_position_update() orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); } +void +Navigator::gps_position_update() +{ + orb_copy(ORB_ID(vehicle_gps_position), _gps_pos_sub, &_gps_pos); +} + void Navigator::sensor_combined_update() { @@ -263,6 +271,7 @@ Navigator::task_main() /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -277,6 +286,7 @@ Navigator::task_main() vehicle_status_update(); vehicle_control_mode_update(); global_position_update(); + gps_position_update(); sensor_combined_update(); home_position_update(); navigation_capabilities_update(); @@ -289,7 +299,7 @@ Navigator::task_main() const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[7]; + struct pollfd fds[8]; /* Setup of loop */ fds[0].fd = _global_pos_sub; @@ -306,6 +316,8 @@ Navigator::task_main() fds[5].events = POLLIN; fds[6].fd = _sensor_combined_sub; fds[6].events = POLLIN; + fds[7].fd = _gps_pos_sub; + fds[7].events = POLLIN; while (!_task_should_exit) { @@ -330,6 +342,16 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + bool have_geofence_position_data = false; + + /* gps updated */ + if (fds[7].revents & POLLIN) { + gps_position_update(); + if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) { + have_geofence_position_data = true; + } + } + /* sensors combined updated */ if (fds[6].revents & POLLIN) { sensor_combined_update(); @@ -364,14 +386,14 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - - /* Check geofence violation */ - bool inside = false; - if (_geofence.getAltitudeMode() == Geofence::GF_ALT_MODE_GPS) { - inside = _geofence.inside(_global_pos); - } else { - inside = _geofence.inside(_global_pos, _sensor_combined.baro_alt_meter); + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + have_geofence_position_data = true; } + } + + /* Check geofence violation */ + if (have_geofence_position_data) { + bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { /* inform other apps via the sp triplet */ _pos_sp_triplet.geofence_violated = true; -- cgit v1.2.3 From 81adc52671d920ffe184948267fcc1f9fbb027cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:30:02 +0200 Subject: geofence: add counter threshold for subsequent detections --- src/modules/navigator/geofence.cpp | 22 +++++++++++++++++++++- src/modules/navigator/geofence.h | 8 ++++++-- src/modules/navigator/geofence_params.c | 11 +++++++++++ 3 files changed, 38 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 4a02a0c3f..a3805dc0f 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -64,7 +64,9 @@ Geofence::Geofence() : _verticesCount(0), _param_geofence_on(this, "ON"), _param_altitude_mode(this, "ALTMODE"), - _param_source(this, "SOURCE") + _param_source(this, "SOURCE"), + _param_counter_threshold(this, "COUNT"), + _outside_counter(0) { /* Load initial params */ updateParams(); @@ -107,6 +109,24 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, } bool Geofence::inside(double lat, double lon, float altitude) +{ + bool inside_fence = inside_polygon(lat, lon, altitude); + + if (inside_fence) { + _outside_counter = 0; + return inside_fence; + } { + _outside_counter++; + if(_outside_counter > _param_counter_threshold.get()) { + return inside_fence; + } else { + return true; + } + } +} + + +bool Geofence::inside_polygon(double lat, double lon, float altitude) { /* Return true if geofence is disabled */ if (_param_geofence_on.get() != 1) diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 91c74572e..65ebb0c3d 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -77,9 +77,10 @@ public: */ bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); + bool inside(double lat, double lon, float altitude); + bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -97,7 +98,7 @@ public: bool isEmpty() {return _verticesCount == 0;} int getAltitudeMode() { return _param_altitude_mode.get(); } - + int getSource() { return _param_source.get(); } private: @@ -112,6 +113,9 @@ private: control::BlockParamInt _param_geofence_on; control::BlockParamInt _param_altitude_mode; control::BlockParamInt _param_source; + control::BlockParamInt _param_counter_threshold; + + uint8_t _outside_counter; }; diff --git a/src/modules/navigator/geofence_params.c b/src/modules/navigator/geofence_params.c index 32902ee97..fca3918e1 100644 --- a/src/modules/navigator/geofence_params.c +++ b/src/modules/navigator/geofence_params.c @@ -83,3 +83,14 @@ PARAM_DEFINE_INT32(GF_ALTMODE, 0); * @group Geofence */ PARAM_DEFINE_INT32(GF_SOURCE, 0); + +/** + * Geofence counter limit + * + * Set how many subsequent position measurements outside of the fence are needed before geofence violation is triggered + * + * @min -1 + * @max 10 + * @group Geofence + */ +PARAM_DEFINE_INT32(GF_COUNT, -1); -- cgit v1.2.3 From c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:29:30 +0200 Subject: datalink loss (obc): add termination after loitering at airfield home --- src/modules/navigator/datalinkloss.cpp | 25 ++++++++++++++++++++++++- src/modules/navigator/datalinkloss.h | 3 +++ src/modules/navigator/datalinkloss_params.c | 11 +++++++++++ src/modules/navigator/rcloss.cpp | 1 + 4 files changed, 39 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 893d6d93a..3310984b0 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -64,6 +64,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomelat(this, "NAV_AH_LAT", false), _param_airfieldhomelon(this, "NAV_AH_LON", false), _param_airfieldhomealt(this, "NAV_AH_ALT", false), + _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), _dll_state(DLL_STATE_NONE) { @@ -140,7 +141,8 @@ DataLinkLoss::set_dll_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get(); _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); _mission_item.pitch_min = 0.0f; _mission_item.autocontinue = true; @@ -149,6 +151,15 @@ DataLinkLoss::set_dll_item() _navigator->set_can_loiter_at_sp(true); break; } + case DLL_STATE_TERMINATE: { + /* Request flight termination from the commander */ + pos_sp_triplet->flight_termination = true; + warnx("not switched to manual: request flight termination"); + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = false; + pos_sp_triplet->next.valid = false; + break; + } default: break; } @@ -185,6 +196,18 @@ DataLinkLoss::advance_dll() _navigator->get_mission_result()->stay_in_failsafe = true; _navigator->publish_mission_result(); break; + case DLL_STATE_FLYTOAIRFIELDHOMEWP: + _dll_state = DLL_STATE_TERMINATE; + warnx("time is up, state should have been changed manually by now"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + break; + case DLL_STATE_TERMINATE: + warnx("dll end"); + _dll_state = DLL_STATE_END; + break; + default: break; } diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index 96b4ce010..d0c9ad09a 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -72,12 +72,15 @@ private: control::BlockParamInt _param_airfieldhomelat; // * 1e7 control::BlockParamInt _param_airfieldhomelon; // * 1e7 control::BlockParamFloat _param_airfieldhomealt; + control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; enum DLLState { DLL_STATE_NONE = 0, DLL_STATE_FLYTOCOMMSHOLDWP = 1, DLL_STATE_FLYTOAIRFIELDHOMEWP = 2, + DLL_STATE_TERMINATE = 3, + DLL_STATE_END = 4 } _dll_state; /** diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index 02f7ca4c3..db307c904 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -91,6 +91,17 @@ PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890); */ PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f); +/** + * Aifield hole wait time + * + * The amount of time in seconds the system should wait at the airfield home waypoint + * + * @unit seconds + * @min 0.0 + * @group DLL + */ +PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); + /** * Number of allowed Datalink timeouts * diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 427001a01..54f1813f5 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -132,6 +132,7 @@ RCLoss::set_rcl_item() pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; pos_sp_triplet->next.valid = false; + break; } default: break; -- cgit v1.2.3 From ae7c99393606373e3a549946481fe07de6fb4c84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:40:19 +0200 Subject: datalink loss: add param to allow skipping of comms hold wp --- src/modules/navigator/datalinkloss.cpp | 24 +++++++++++++++++------- src/modules/navigator/datalinkloss.h | 1 + src/modules/navigator/datalinkloss_params.c | 14 +++++++++++++- 3 files changed, 31 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 3310984b0..4e3d25840 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -66,6 +66,7 @@ DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) : _param_airfieldhomealt(this, "NAV_AH_ALT", false), _param_airfieldhomewaittime(this, "AH_T"), _param_numberdatalinklosses(this, "N"), + _param_skipcommshold(this, "CHSK"), _dll_state(DLL_STATE_NONE) { /* load initial params */ @@ -179,14 +180,23 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + if (!_param_skipcommshold.get()) { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); + _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } } else { - warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); - _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/datalinkloss.h b/src/modules/navigator/datalinkloss.h index d0c9ad09a..31e0e3907 100644 --- a/src/modules/navigator/datalinkloss.h +++ b/src/modules/navigator/datalinkloss.h @@ -74,6 +74,7 @@ private: control::BlockParamFloat _param_airfieldhomealt; control::BlockParamFloat _param_airfieldhomewaittime; control::BlockParamInt _param_numberdatalinklosses; + control::BlockParamInt _param_skipcommshold; enum DLLState { DLL_STATE_NONE = 0, diff --git a/src/modules/navigator/datalinkloss_params.c b/src/modules/navigator/datalinkloss_params.c index db307c904..6780c0c88 100644 --- a/src/modules/navigator/datalinkloss_params.c +++ b/src/modules/navigator/datalinkloss_params.c @@ -107,8 +107,20 @@ PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f); * * After more than this number of data link timeouts the aircraft returns home directly * - * @group commander + * @group DLL * @min 0 * @max 1000 */ PARAM_DEFINE_INT32(NAV_DLL_N, 2); + +/** + * Skip comms hold wp + * + * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to + * airfield home + * + * @group DLL + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0); -- cgit v1.2.3 From c0975af375c168be98804f025192bbb30710355d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 13:44:43 +0200 Subject: commander: check if baro is healthy --- src/modules/commander/commander.cpp | 19 +++++++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 21 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0bf5cfe34..ddfbd65a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1081,6 +1081,25 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); + /* Check if the barometer is healthy and issue a warning in the GCS if not so. + * Because the barometer is used for calculating AMSL altitude which is used to ensure + * vertical separation from other airtraffic the operator has to know when the + * barometer is inoperational. + * */ + if (hrt_elapsed_time(&sensors.baro_timestamp) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where baro was regained */ + if (status.barometer_failure) { + status.barometer_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro healthy"); + } + } else { + if (!status.barometer_failure) { + status.barometer_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "baro failed"); + } + } } orb_check(diff_pres_sub, &updated); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b465f8407..ad92f5b26 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -211,6 +211,8 @@ struct vehicle_status_s { bool gps_failure; /** Set to true if a gps failure is detected */ bool gps_failure_cmd; /** Set to true if a gps failure mode is commanded */ + bool barometer_failure; /** Set to true if a barometer failure is detected */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; -- cgit v1.2.3 From bdccd69030e56381d906afeabc8305dbe18e2de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 15:27:31 +0200 Subject: geofence: make some functions private, correctly update params --- src/modules/navigator/geofence.cpp | 2 ++ src/modules/navigator/geofence.h | 7 ++++--- src/modules/navigator/mission_feasibility_checker.cpp | 2 +- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index a3805dc0f..5504239c5 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -91,6 +91,8 @@ bool Geofence::inside(const struct vehicle_global_position_s &global_position, f bool Geofence::inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl) { + updateParams(); + if (getAltitudeMode() == Geofence::GF_ALT_MODE_WGS84) { if (getSource() == Geofence::GF_SOURCE_GLOBALPOS) { return inside(global_position); diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65ebb0c3d..65e5b4042 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -75,11 +75,8 @@ public: * @param craft pointer craft coordinates * @return true: system is inside fence, false: system is outside fence */ - bool inside(const struct vehicle_global_position_s &global_position); - bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); bool inside(const struct vehicle_global_position_s &global_position, const struct vehicle_gps_position_s &gps_position,float baro_altitude_amsl); - bool inside(double lat, double lon, float altitude); bool inside_polygon(double lat, double lon, float altitude); int clearDm(); @@ -116,6 +113,10 @@ private: control::BlockParamInt _param_counter_threshold; uint8_t _outside_counter; + + bool inside(double lat, double lon, float altitude); + bool inside(const struct vehicle_global_position_s &global_position); + bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); }; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f20..937e4fa5a 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { + if (!geofence.inside_polygon(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } -- cgit v1.2.3 From a432d0493c0761da075c7734c0f54f44d6121e78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:44:36 +0200 Subject: correctly initialize stay in failsafe flag --- src/modules/navigator/navigator_mode.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp index d91f7ab18..3807c5ea8 100644 --- a/src/modules/navigator/navigator_mode.cpp +++ b/src/modules/navigator/navigator_mode.cpp @@ -64,7 +64,7 @@ NavigatorMode::run(bool active) { /* first run */ _first_run = false; /* Reset stay in failsafe flag */ - _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->get_mission_result()->stay_in_failsafe = false; _navigator->publish_mission_result(); on_activation(); -- cgit v1.2.3 From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:45:15 +0200 Subject: move flight termination and geofence flags from setpoint triplet to mission result --- src/modules/commander/commander.cpp | 18 ++++++------- src/modules/navigator/datalinkloss.cpp | 31 ++++++++++++---------- src/modules/navigator/gpsfailure.cpp | 3 ++- src/modules/navigator/navigator_main.cpp | 16 +++++------ src/modules/navigator/rcloss.cpp | 3 ++- src/modules/uORB/topics/mission_result.h | 8 +++--- .../uORB/topics/position_setpoint_triplet.h | 2 -- 7 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ddfbd65a1..f05f970e5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1328,15 +1328,6 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet); - - /* Check for geofence violation */ - if (armed.armed && (pos_sp_triplet.geofence_violated || pos_sp_triplet.flight_termination)) { - //XXX: make this configurable to select different actions (e.g. navigation modes) - /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ - armed.force_failsafe = true; - status_changed = true; - warnx("Flight termination because of navigator request or geofence"); - } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { @@ -1429,6 +1420,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + + /* Check for geofence violation */ + if (armed.armed && (mission_result.geofence_violated || mission_result.flight_termination)) { + //XXX: make this configurable to select different actions (e.g. navigation modes) + /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ + armed.force_failsafe = true; + status_changed = true; + warnx("Flight termination because of navigator request or geofence"); + } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } /* RC input check */ diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 4e3d25840..66f1c8c73 100644 --- a/src/modules/navigator/datalinkloss.cpp +++ b/src/modules/navigator/datalinkloss.cpp @@ -154,7 +154,8 @@ DataLinkLoss::set_dll_item() } case DLL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("not switched to manual: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; @@ -180,23 +181,25 @@ DataLinkLoss::advance_dll() switch (_dll_state) { case DLL_STATE_NONE: /* Check the number of data link losses. If above home fly home directly */ - if (!_param_skipcommshold.get()) { - /* if number of data link losses limit is not reached fly to comms hold wp */ - if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { - warnx("%d data link losses, limit is %d, fly to airfield home", - _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to home"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; - } else { + /* if number of data link losses limit is not reached fly to comms hold wp */ + if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) { + warnx("%d data link losses, limit is %d, fly to airfield home", + _navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get()); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home"); + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; + } else { + if (!_param_skipcommshold.get()) { warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter); mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold"); _dll_state = DLL_STATE_FLYTOCOMMSHOLDWP; + } else { + /* comms hold wp not active, fly to airfield home directly */ + warnx("Skipping comms hold wp. Flying directly to airfield home"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); + _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } - } else { - /* comms hold wp not active, fly to airfield home directly */ - warnx("Skipping comms hold wp. Flying directly to airfield home"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped"); - _dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP; } break; case DLL_STATE_FLYTOCOMMSHOLDWP: diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index f0bbbcf1c..cd55f60b0 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -140,7 +140,8 @@ GpsFailure::set_gpsf_item() switch (_gpsf_state) { case GPSF_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("gps fail: request flight termination"); } default: diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 81ceaaca2..c173ecd50 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -395,11 +395,9 @@ Navigator::task_main() if (have_geofence_position_data) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); if (!inside) { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = true; - if (_pos_sp_triplet.geofence_violated != true) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = true; + publish_mission_result(); /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { @@ -407,11 +405,9 @@ Navigator::task_main() _geofence_violation_warning_sent = true; } } else { - /* inform other apps via the sp triplet */ - _pos_sp_triplet.geofence_violated = false; - if (_pos_sp_triplet.geofence_violated != false) { - _pos_sp_triplet_updated = true; - } + /* inform other apps via the mission result */ + _mission_result.geofence_violated = false; + publish_mission_result(); /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 54f1813f5..651e31184 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -127,7 +127,8 @@ RCLoss::set_rcl_item() } case RCL_STATE_TERMINATE: { /* Request flight termination from the commander */ - pos_sp_triplet->flight_termination = true; + _navigator->get_mission_result()->flight_termination = true; + _navigator->publish_mission_result(); warnx("rc not recovered: request flight termination"); pos_sp_triplet->previous.valid = false; pos_sp_triplet->current.valid = false; diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 65ddfb4ad..c7d25d1f0 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -55,9 +55,11 @@ struct mission_result_s { unsigned seq_reached; /**< Sequence of the mission item which has been reached */ unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool reached; /**< true if mission has been reached */ + bool finished; /**< true if mission has been completed */ + bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ + bool geofence_violated; /**< true if the geofence is violated */ + bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index e2b1525a2..ec2131abd 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -97,8 +97,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s next; unsigned nav_state; /**< report the navigation state */ - bool geofence_violated; /**< true if the geofence is violated */ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ }; /** -- cgit v1.2.3 From 98e74ed0e74e02f866fc7538c7d4153ae3c36743 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 22:18:01 +0200 Subject: commander: limit the output of a warnx --- src/modules/commander/commander.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index f05f970e5..2fcf7bebb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1427,7 +1427,11 @@ int commander_thread_main(int argc, char *argv[]) /* this will only trigger if geofence is activated via param and a geofence file is present, also there is a circuit breaker to disable the actual flight termination in the px4io driver */ armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of navigator request or geofence"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of navigator request or geofence"); + flight_termination_printed = true; + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 8a9da209d194b4f35000935379901ed6091091f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 23:17:56 +0200 Subject: limit warnx output on flight termination --- src/modules/commander/commander.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2fcf7bebb..8b065560f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1610,7 +1610,11 @@ int commander_thread_main(int argc, char *argv[]) (status.data_link_lost_cmd && status.gps_failure_cmd)) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of data link loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of data link loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); } @@ -1625,7 +1629,11 @@ int commander_thread_main(int argc, char *argv[]) (status.rc_signal_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; - warnx("Flight termination because of RC signal loss && gps failure"); + static bool flight_termination_printed = false; + if (!flight_termination_printed) { + warnx("Flight termination because of RC signal loss && gps failure"); + flight_termination_printed = true; + } mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } -- cgit v1.2.3 From 3d528a2c979e7d0df1171afc1f038759c7b01383 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:22:59 +0200 Subject: introduce new nav state to allow normal rtl with RC switch --- src/modules/commander/state_machine_helper.cpp | 2 +- src/modules/navigator/navigator_main.cpp | 5 ++++- src/modules/uORB/topics/vehicle_status.h | 1 + 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 098ff1a3d..e3b5d30e4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -462,7 +462,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->failsafe = true; if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->nav_state = NAVIGATION_STATE_AUTO_RTL; + status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER; } else if (status->condition_local_position_valid) { status->nav_state = NAVIGATION_STATE_LAND; } else if (status->condition_local_altitude_valid) { diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c173ecd50..9a8c54e7e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -433,13 +433,16 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_LOITER: _navigation_mode = &_loiter; break; - case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RCRECOVER: if (_param_rcloss_obc.get() != 0) { _navigation_mode = &_rcLoss; } else { _navigation_mode = &_rtl; } break; + case NAVIGATION_STATE_AUTO_RTL: + _navigation_mode = &_rtl; + break; case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ad92f5b26..9dccb2309 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -102,6 +102,7 @@ typedef enum { NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RCRECOVER, /**< RC recover mode */ NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ NAVIGATION_STATE_AUTO_LANDENGFAIL, /**< Auto land on engine failure */ NAVIGATION_STATE_AUTO_LANDGPSFAIL, /**< Auto land on gps failure (e.g. open loop loiter down) */ -- cgit v1.2.3 From 5e5322c593c6bd8ccd894f47ab8fd88b72e51677 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:46:09 +0200 Subject: fix flight termination circuit breaker name, tested --- src/drivers/px4io/px4io.cpp | 2 +- src/modules/systemlib/circuit_breaker.c | 2 +- src/modules/systemlib/circuit_breaker.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d5f569d71..b5c5ef17c 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1170,7 +1170,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERMINATION", CBRK_FLIGHTTERMINATION_KEY)) { + if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8a0b51b71..b0f95aedf 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERMINATION, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 54c4fced6..445a89d3a 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -53,7 +53,7 @@ #define CBRK_RATE_CTRL_KEY 140253 #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 -#define CBRK_FLIGHTTERMINATION_KEY 121212 +#define CBRK_FLIGHTTERM_KEY 121212 #include -- cgit v1.2.3 From 9cc1f1ab9db4af9af18e6879ba82cbcfa8e588f3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 23:12:28 +0200 Subject: flight termination on gps failure && datalink loss: do not activate in manual modes --- src/modules/commander/commander.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 8b065560f..5673037b8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1605,9 +1605,14 @@ int commander_thread_main(int argc, char *argv[]) /* Check for failure combinations which lead to flight termination */ if (armed.armed) { /* At this point the data link and the gps system have been checked - * If both failed we want to terminate the flight */ - if ((status.data_link_lost && status.gps_failure) || - (status.data_link_lost_cmd && status.gps_failure_cmd)) { + * If we are not in a manual (RC stick controlled mode) + * and both failed we want to terminate the flight */ + if (status.main_state != MAIN_STATE_MANUAL && + status.main_state != MAIN_STATE_ACRO && + status.main_state != MAIN_STATE_ALTCTL && + status.main_state != MAIN_STATE_POSCTL && + ((status.data_link_lost && status.gps_failure) || + (status.data_link_lost_cmd && status.gps_failure_cmd))) { armed.force_failsafe = true; status_changed = true; static bool flight_termination_printed = false; -- cgit v1.2.3 From 3d01da35d02505e751536e2cc09637797b37bed6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:27 +0200 Subject: write sysid & compid to telemetry status --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ src/modules/uORB/topics/telemetry_status.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1115304d4..43222880e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -534,6 +534,8 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) tstatus.remote_noise = rstatus.remnoise; tstatus.rxerrors = rstatus.rxerrors; tstatus.fixed = rstatus.fixed; + tstatus.system_id = msg->sysid; + tstatus.component_id = msg->compid; if (_telemetry_status_pub < 0) { _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 1297c1a9d..93193f32b 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -67,6 +67,8 @@ struct telemetry_status_s { uint8_t noise; /**< background noise level */ uint8_t remote_noise; /**< remote background noise level */ uint8_t txbuf; /**< how full the tx buffer is as a percentage */ + uint8_t system_id; /**< system id of the remote system */ + uint8_t component_id; /**< component id of the remote system */ }; /** -- cgit v1.2.3 From 3f8793210b47bd8e09ed2adaabc2fab966db5df6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:52 +0200 Subject: datalink check: ignore onboard computer --- src/modules/commander/commander.cpp | 20 +++++++++++++++++--- src/modules/commander/commander_params.c | 10 ++++++++++ 2 files changed, 27 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9885176b7..07fcb5d40 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,6 +685,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); + param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); /* welcome user */ warnx("starting"); @@ -879,12 +880,14 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; + uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; + telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -971,6 +974,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; + uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid + is not validated for the datalink loss check */ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1033,6 +1038,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); + param_get(_param_onboard_sysid, &onboard_sysid); } orb_check(sp_man_sub, &updated); @@ -1079,6 +1085,7 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; + telemetry_sysid[i] = telemetry.system_id; } } @@ -1562,10 +1569,17 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - have_link = true; - } else if (!telemetry_lost[i]) { + + /* If this is not an onboard link/onboard computer: + * set flag that we have a valid link */ + if (telemetry_sysid[i] != onboard_sysid) { + have_link = true; + } + } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout */ + * we don't have to check a timeout, + * telemetry from onboard computers is not accepted as a valid datalink + */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 30159dad9..98c0982b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -128,3 +128,13 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); * @max 30 */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); + +/** Onboard computer system id + * + * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check + * + * @group commander + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(COM_ONBSYSID, 42); -- cgit v1.2.3 From d6b669f4971d8d7329f0bfd5ade64bed2bd3120f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40..0ada8e978 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1442,8 +1442,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } -- cgit v1.2.3 From 31a17ce29a76a2f58e6feb92abe1de01eed5a8eb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbb..b2355d4d8 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } -- cgit v1.2.3 From e3cac1999a0b4398e669f90cd28279ec2a0784a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e..fc9120f13 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,9 +386,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ -- cgit v1.2.3 From 973c034d6ec502009a8fd92c14ef02c4d1769a64 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: engine failure detection --- src/modules/commander/commander.cpp | 52 +++++++++++++++++++++++++++++--- src/modules/commander/commander_params.c | 32 ++++++++++++++++++++ src/modules/systemlib/circuit_breaker.c | 14 +++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 4 files changed, 95 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ada8e978..b9a0bd2cb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1576,10 +1590,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1612,6 +1627,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 98c0982b2..15c299a8b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @max 255 */ PARAM_DEFINE_INT32(COM_ONBSYSID, 42); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b0f95aedf..9e5429988 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); */ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 445a89d3a..6a55e4948 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -54,6 +54,7 @@ #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 #include -- cgit v1.2.3 From d18f3ee70d5fbeb150c6b37ccafa4f622494ec19 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: make rc loss timeout a param --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b9a0bd2cb..b7a16e4e6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1466,7 +1468,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c299a8b..7d06003c9 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -170,3 +170,14 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @max 7.0f */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); -- cgit v1.2.3 From 21009e89a4c748d8a61174058bb378c1d6306b8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: flight termination mavlink outtput: limit rate --- src/modules/commander/commander.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b7a16e4e6..134f23c0b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1688,9 +1688,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1709,7 +1712,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } -- cgit v1.2.3 From a8239b2c4516c36d30767c0ae61b30f1e2dde096 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: if V_RCL_LT < 0 go directly to termination --- src/modules/navigator/rcloss.cpp | 15 +++++++++++---- src/modules/navigator/rcloss_params.c | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 651e31184..5564a1c42 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -153,10 +153,17 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - /* Check the number of data link losses. If above home fly home directly */ - warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); - _rcl_state = RCL_STATE_LOITER; + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } break; case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 83d23cf49..f1a01c73b 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -51,9 +51,10 @@ * Loiter Time * * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering * * @unit seconds - * @min 0.0 + * @min -1.0 * @group RCL */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); -- cgit v1.2.3 From 1fb8e76f0a4508cadd63b47e6f94a6638b699bcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: fix typo in comment --- 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 ee08f3e98..f44985d50 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 @@ -1218,7 +1218,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { -- cgit v1.2.3 From e17411769847f8681dc05da72ed4a06ff27a7a32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: gps failure has priority over engine falure, in case both fail make sure that the gps failure mode does not turn on the engine --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e3b5d30e4..9568752ae 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -511,10 +511,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX /* Finished handling commands which have priority , now handle failures */ - } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 517333c80..7cf721a35 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -860,8 +860,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); -- cgit v1.2.3 From b5ffcfe3d12a0e2e98cb5319e0286c8215150672 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: Revert "datalink check: ignore onboard computer" This reverts commit 3f8793210b47bd8e09ed2adaabc2fab966db5df6. Conflicts: src/modules/commander/commander.cpp src/modules/commander/commander_params.c --- src/modules/commander/commander.cpp | 23 +++-------------------- src/modules/commander/commander_params.c | 10 ---------- 2 files changed, 3 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 134f23c0b..b43f08f67 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1590,18 +1581,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7d06003c9..f4bc7fd7f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -129,16 +129,6 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -/** Onboard computer system id - * - * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check - * - * @group commander - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(COM_ONBSYSID, 42); - /** Engine Failure Throttle Threshold * * Engine failure triggers only above this throttle value -- cgit v1.2.3 From c7966d56f5457be2eab4d6ee9cfadb7c3f22674b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5..0f431ded2 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042..9d647cb68 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc9120f13..4f92954fd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else -- cgit v1.2.3 From 5a0e0d041229700d2ae9c14e4ab2798d34fff14c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: navigator: fix status information, remove fence_valid flag (this is handled by the geofence class) --- src/modules/navigator/navigator.h | 1 - src/modules/navigator/navigator_main.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 840b43f1b..bf42acff9 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -191,7 +191,6 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4f92954fd..b63394544 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -536,7 +535,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -544,7 +543,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } -- cgit v1.2.3 From d113fcfc54c246f3d5ac22ad5485f7103aecab41 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 +++++++++++++++++------------------ 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b43f08f67..bf15bbeb6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); -- cgit v1.2.3 From 964fddb387b8e635d2cd13fb2248643791304489 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:08:25 +0200 Subject: make geofence update rate independent from positon update rate --- src/modules/navigator/navigator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b63394544..fad46998e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -87,6 +87,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -343,7 +344,7 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - bool have_geofence_position_data = false; + static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { @@ -388,17 +389,18 @@ Navigator::task_main() if (fds[0].revents & POLLIN) { global_position_update(); static int gposcounter = 0; - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && - gposcounter % 10 == 0) { - /* Geofence is checked only every 10th gpos update */ + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } gposcounter++; } /* Check geofence violation */ - if (have_geofence_position_data) { + static hrt_abstime last_geofence_check = 0; + if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _mission_result.geofence_violated = true; -- cgit v1.2.3 From 038e1cac03198259d6f7630c6bb7c65c35f44fae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:17 +0200 Subject: increase default engine failure threshold --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f4bc7fd7f..1b0c4258b 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @min 0.0f * @max 7.0f */ -PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); /** RC loss time threshold * -- cgit v1.2.3 From ab400089bc2a42f1f0ace569d8f0ee58f4338e1d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:40 +0200 Subject: disable flight termination as default for now --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 9e5429988..64420be37 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection -- cgit v1.2.3 From 70606d400bdceaa86331670938c1e0ae988ed97c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:30 +0200 Subject: remove wrong comments --- src/modules/navigator/navigator_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e..bf5e36d39 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,9 +432,6 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: - /* Some failsafe modes prohibit the fallback to mission - * usually this is done after some time to make sure - * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -450,7 +447,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here + case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { -- cgit v1.2.3 From 1072a3380c2d6bdea010bb5091c6d0b23fe6f224 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:46 +0200 Subject: enable engine failure circuit breaker --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64420be37..83c8344d5 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -120,7 +120,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { -- cgit v1.2.3 From d4c0dc2ba0271f4d9c8044fd2a3a178cbb9987e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:20:30 +0200 Subject: add and activate circuit breaker for gps failure detection --- src/modules/commander/commander.cpp | 5 +++-- src/modules/systemlib/circuit_breaker.c | 14 ++++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf15bbeb6..b86f3678b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1417,8 +1417,9 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 83c8344d5..12187d70e 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 6a55e4948..b3431722f 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -55,6 +55,7 @@ #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include -- cgit v1.2.3 From 8b7c57a0d050d51f45f8c66536e5450e7a494d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:39:40 +0200 Subject: px4io driver: update cb only when changed --- src/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f8d6f5f6..fbb5d4f2e 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -295,6 +295,7 @@ private: float _battery_amp_bias; ///< current sensor bias float _battery_mamphour_total;///< amp hours consumed so far uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp + bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _cb_flighttermination(true) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -1051,6 +1053,9 @@ PX4IO::task_main() } } + /* Update Circuit breakers */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } } @@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { + if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; -- cgit v1.2.3 From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:40:03 +0200 Subject: commander: update gps and engine cb only when changed --- src/modules/commander/commander.cpp | 20 ++++++++++++++------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b86f3678b..9ebe006f0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; int32_t ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine - was healty*/ + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + if (status.circuit_breaker_engaged_gpsfailure_check || (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ @@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + if (!status.circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9dccb2309..505039d90 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -239,6 +239,8 @@ struct vehicle_status_s { bool circuit_breaker_engaged_power_check; bool circuit_breaker_engaged_airspd_check; + bool circuit_breaker_engaged_enginefailure_check; + bool circuit_breaker_engaged_gpsfailure_check; }; /** -- cgit v1.2.3