From 7baa337d9bcf7077a5e5080e951899cb595f6ff6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 19:25:53 +0200 Subject: [PATCH 01/85] 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 04450a44fb..522c6e8860 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 331a9a728f..ba46bd5681 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 4a19321809..4e8c6c53ef 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 */ }; /** From b5ef8fd2cd54d180b5debe362a4c1f07f64394af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 19 Jul 2014 20:03:37 +0200 Subject: [PATCH 02/85] 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 0000000000..17e85b2841 --- /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 0000000000..242cfac8dc --- /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 0000000000..bfe6ce7e18 --- /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 b501989961..a1e42ec383 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 8edbb63b35..d0b2ed841f 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 ba46bd5681..1ce6770c98 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 084afe3401..afaf1c3c31 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); From 8739308999b410ac8e2a92cf3e5fa63c5e18f5ba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:40:26 +0200 Subject: [PATCH 03/85] WIP, datalinkloss: implementing basic behavior --- src/modules/navigator/datalinkloss.cpp | 224 +++++--------------- src/modules/navigator/datalinkloss.h | 33 ++- src/modules/navigator/datalinkloss_params.c | 123 ++++++----- 3 files changed, 144 insertions(+), 236 deletions(-) diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp index 17e85b2841..2bd80165d5 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 (_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; + //[> 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(); + //[> 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; + 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 = 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.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.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get(); _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.autocontinue = true; _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; + 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_LAND; + _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: 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; - _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; } @@ -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 242cfac8dc..101c88a254 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 bfe6ce7e18..a836fc8caf 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). - * - * @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. + * The amount of time in seconds the system should wait at the comms hold waypoint * * @unit seconds - * @min -1 - * @max - * @group RTL + * @min 0.0 + * @group DLL */ -PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); +PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f); + +/** + * Comms hold Lat + * + * Latitude of comms hold waypoint + * + * @unit degrees * 1e7 + * @min 0.0 + * @group DLL + */ +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); + +/** + * Comms hold alt + * + * Altitude of comms hold waypoint + * + * @unit m + * @min 0.0 + * @group DLL + */ +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); From dcf114aa65273d5d5ce522565fc364fc347ba3fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 17:53:04 +0200 Subject: [PATCH 04/85] 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 7d4d677d09..8eba64c1d2 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 4750f9d5cb..25effbd217 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); From 86b9e367a6cc5791f83df3223190a470798c00ff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 20 Jul 2014 18:23:41 +0200 Subject: [PATCH 05/85] 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 8eba64c1d2..46065fef10 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 b46c00b758..eda1dfaec9 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; From a9a8f1435fa798b289aa4e4af9312041abdbcf94 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 21 Jul 2014 23:56:32 +0200 Subject: [PATCH 06/85] 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 25effbd217..980a7a2bbc 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 2bd80165d5..a98e21139e 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 101c88a254..650cc7bc54 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 a836fc8caf..038c80a1a1 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); From 24f380137ecb91fb9647e22e1d29c13da5fc0357 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 22:58:19 +0200 Subject: [PATCH 07/85] 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 +-- .../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 5c89e01238..cb09a68e31 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 7b26e3e8cb..4e1cfb9873 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 bb1b87e712..4285d29777 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 a98e21139e..52f263aff0 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 ba766cd10f..7ce0e2f891 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 4da6a11553..1b8c8c8740 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 d0b2ed841f..363877bb83 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 1ce6770c98..77124e8f65 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 f432156652..3c6754c556 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 beb797e624..65ddfb4ad4 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*/ }; /** From 756ea3019ec3a50623d846c019c6474488273a34 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:00:34 +0200 Subject: [PATCH 08/85] 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 52f263aff0..91bb5b1104 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; From f10d1277a5e795256a247cb6f130cefba12c5ffc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 23:07:11 +0200 Subject: [PATCH 09/85] 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 363877bb83..fc0d47b388 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 */ From a35814d15b1317f73f325e98f0500f5fd1233583 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 08:57:58 +0200 Subject: [PATCH 10/85] 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 cb09a68e31..fe1974c889 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 f73ae71f30..3d1e231c6a 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); From bffa9e3fa83b84b51b2446291682519433fd3fff Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 19:27:40 +0200 Subject: [PATCH 11/85] 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 650cc7bc54..5a46b5700e 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 0000000000..ea654c5fd4 --- /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 0000000000..2c48c2fce2 --- /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 a1e42ec383..0539087dfd 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 fc0d47b388..fe6639dfe7 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 77124e8f65..21ab691ff6 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"), From bc4d7952f384fc079cd30327a5b4e32da90bbcb6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:20 +0200 Subject: [PATCH 12/85] 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 91bb5b1104..19f3356331 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; From a08b7a9f359cc0cf9c6d0631bb844b54f50adcab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:22:40 +0200 Subject: [PATCH 13/85] enginefailure: set mission item --- src/modules/navigator/enginefailure.cpp | 28 ++++++++++++------------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/modules/navigator/enginefailure.cpp b/src/modules/navigator/enginefailure.cpp index ea654c5fd4..de567f0dc6 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; + _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); + _navigator->set_can_loiter_at_sp(true); break; } default: From 303664d94af4949bcc7f1c3393f82eb242a60c5e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 21:49:22 +0200 Subject: [PATCH 14/85] 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 21ab691ff6..043d883b2d 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 eda1dfaec9..c2926dce8f 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) */ From 1b2f9070ea063255aca4a882e4adda7f89e082e7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 22:29:04 +0200 Subject: [PATCH 15/85] engine_failure flag Added engine_failure flag to behicle status, alsoset_nav_state in the state machine helper takes this flag into account --- .../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 4e1cfb9873..bc68035966 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 c2926dce8f..a474886219 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; From 351598626054e5d853e8768d2f5d04226510c12a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 24 Jul 2014 23:44:38 +0200 Subject: [PATCH 16/85] 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 a474886219..e0072b52d5 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) */ From db5d668439be63e4c8fd7dab49b81c5e162ee095 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 27 Jul 2014 00:55:43 +0200 Subject: [PATCH 17/85] 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 fe1974c889..50beb5da85 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 707abb5458..0751b57fe0 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; From 9ee6ab366d2a7c6f24e2f08021da9cd861663bdc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 17:30:05 +0200 Subject: [PATCH 18/85] 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 64317a18a8..8a0b51b71b 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 b605846082..54c4fced62 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 From 0f2b66fa8b3782c965966e9c0b0b6f16b80d7f38 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 14 Aug 2014 22:38:48 +0200 Subject: [PATCH 19/85] failsafe: enable support for commands --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++ .../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 c6f56d5e3a..b635a22cb6 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 ecfe62e03e..157e35ef8d 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 043d883b2d..4af37fa3eb 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 301503b82e..b465f8407d 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; From 6fe0482b277cd7120878787171c735e2d424c1c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 00:07:38 +0200 Subject: [PATCH 20/85] 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 b635a22cb6..4f996b5261 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; From 85d8781bc32fc25b30350aab518a8b9823cce661 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:04 +0200 Subject: [PATCH 21/85] 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 19f3356331..6c5012bfdd 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(); From 8bc7d7f43dd350c19993cd12b514a6a0b5366b3c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:26:43 +0200 Subject: [PATCH 22/85] 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 fe6639dfe7..5369779723 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 4af37fa3eb..18bfc2cece 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(); } From b71778d7e1d03adf8b1366982f0e86753ac072be Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 01:41:48 +0200 Subject: [PATCH 23/85] 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 038c80a1a1..77a8763cb1 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 From 8dbe6a6dc0d753a0f092fc2d4be508e48429c008 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 14:35:45 +0200 Subject: [PATCH 24/85] 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 32069cf09b..afb03789ff 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; From 5406ba78a8de82c49aa9ea8e033c82670950259d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:53:03 +0200 Subject: [PATCH 25/85] 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 6c5012bfdd..2efca0a949 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 c76192f667..79865e2ae0 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 3c6754c556..d91f7ab181 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 142a734098..b6c4b8fdff 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(); From 2791a7097686d327e91ac31c20716bb602011d65 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 20:57:21 +0200 Subject: [PATCH 26/85] 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 2efca0a949..7b9a7b151c 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(); } From 35daef948bb6dac06900d7bc74aa09fe35aceabd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 10:52:01 +0200 Subject: [PATCH 27/85] 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 28aba759fd..8a12e16ca6 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 { From a972a2857197706ed414b674dcaf291d48ac0f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 11:41:31 +0200 Subject: [PATCH 28/85] 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 3d1e231c6a..30159dad9c 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 * From 72beef90c9a9bb901355483e275a47f166f654a8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:09:10 +0200 Subject: [PATCH 29/85] 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 157e35ef8d..f82eec6956 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) { From 94765f1fe01a3ae1ba72a330f77cfb4cacbd8c0e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 13:17:26 +0200 Subject: [PATCH 30/85] 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 7b9a7b151c..ab9e67a336 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 5a46b5700e..96b4ce010d 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 From 14189816f7e751b14725939fd5a200c39818f5df Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:28:39 +0200 Subject: [PATCH 31/85] 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 ab9e67a336..b914fdb347 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(); From f480c10282efcb56b643b2c676303f1f57498fda Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 14:37:46 +0200 Subject: [PATCH 32/85] 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 f82eec6956..3f4bfaa1ca 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; } From ab9b234893448237d84b4e1f95b807be3a68e98f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 16 Aug 2014 18:07:21 +0200 Subject: [PATCH 33/85] 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 8a12e16ca6..2ab40a5eb0 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 } From b1008842204418f5d8cd0475547ecfb8f378b4c7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:07:02 +0200 Subject: [PATCH 34/85] 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 2662153084..2b9ce752b2 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 2eb126ab56..e2c0f08d87 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 653b1ad849..29b42cd54e 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 5369779723..59a6752a99 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 18bfc2cece..d77acf74e2 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) { From 5832948371866aec8f0c7f16b13869f270d36aad Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 12:37:14 +0200 Subject: [PATCH 35/85] 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 2b9ce752b2..7897c7ba0e 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 From 99860da9b70bfa87ef2834efa5e7b9ba96ee4e9b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 16:36:35 +0200 Subject: [PATCH 36/85] 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 2ab40a5eb0..84a4be948f 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 } From 7f9c996555975301288da58745f69b39f05facbe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:30:19 +0200 Subject: [PATCH 37/85] 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 de567f0dc6..e007b208b2 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: From 64ca94412e710164fb2ae69f6dfc3edbeff9c12b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 07:31:55 +0200 Subject: [PATCH 38/85] engine fail: fw pos control limits pitch and sets 0 throttle --- .../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 350ce6dec1..0c35ccb2cc 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, From 760a7ff548bcef6910f84beaa981600f3609358b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 20 Aug 2014 07:45:01 +0200 Subject: [PATCH 39/85] gpsfailure: add skeleton class, activate in commander --- .../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 3f4bfaa1ca..404bafb042 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 b914fdb347..d8a1de2297 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 0000000000..4c526b76ef --- /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 0000000000..88d386ec45 --- /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 0539087dfd..f6590605b4 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 59a6752a99..b161c29849 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 d77acf74e2..d778257157 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: From 1a14ff250e5a2ead69576762fd5b7f176c4b6fac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:39:44 +0200 Subject: [PATCH 40/85] 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 0cea13cc4d..ee112a40e7 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; From 752a0a562564ccc6f7d49ceebe810de7e6a6d358 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 00:40:45 +0200 Subject: [PATCH 41/85] 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 84a4be948f..109ab403ac 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 4c526b76ef..02e766ffba 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 88d386ec45..e9e72babf8 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 0000000000..39d179eed7 --- /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 f6590605b4..9e4ad053c9 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 b161c29849..ec6e538e34 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 bc9951bf2e..e0913bb575 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 1c78f53305..e2b1525a23 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 */ }; /** From 406a52e6399b171cfe454b85774f12f9add40aee Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 20:24:04 +0200 Subject: [PATCH 42/85] 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 109ab403ac..243546787b 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 From b22acadb8a255cfdf7a8c77ea0ea84587ff7d5e0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:02:33 +0200 Subject: [PATCH 43/85] 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 243546787b..8ae5a441aa 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 */ From d50135b611605891cb1fb8841490c41474c3ec31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:40:58 +0200 Subject: [PATCH 44/85] 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 8ae5a441aa..9968ab8e6e 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(); From 4d75222b67b8ae93f28c1bbec782ce7fd0ab9919 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 21:41:24 +0200 Subject: [PATCH 45/85] 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 404bafb042..d105e4bdf6 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) { From 6ae8800ad09caf9194240ec75067af5ef56a5a23 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:05:19 +0200 Subject: [PATCH 46/85] 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 d8a1de2297..893d6d93ae 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 77a8763cb1..02f7ca4c31 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 afaf1c3c31..1f40e634ed 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); From fd3746a233b0ef16758e0171da0ee7e71ff58887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 23:06:14 +0200 Subject: [PATCH 47/85] 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 02e766ffba..f0bbbcf1cb 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 9e4ad053c9..c075903b75 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 ec6e538e34..709e3e7242 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 e0913bb575..c569ee7b54 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 0000000000..b8b659efe0 --- /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 0000000000..bcb74d8778 --- /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 0000000000..83d23cf499 --- /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); From 29715102894121f711fd40acfcf6ec8fb4fa6630 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:00:28 +0200 Subject: [PATCH 48/85] commander: flight termination, require arming --- src/modules/commander/commander.cpp | 48 +++++++++++++++-------------- 1 file changed, 25 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9968ab8e6e..692e5e8ec4 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"); - } + 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"); + /* 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"); + } } From cc12e6051d1ae8100c035e935fe6dd1a453ce887 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:13:51 +0200 Subject: [PATCH 49/85] 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 b8b659efe0..427001a013 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: From ffd2fa7386f9fe3ab017d98a2b0e8e21b0975833 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 13:25:50 +0200 Subject: [PATCH 50/85] 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 692e5e8ec4..c4b76b3918 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"); From 3c10b78e2044ec2cbe36d4de35c7ac8a936521ae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 23 Aug 2014 14:02:22 +0200 Subject: [PATCH 51/85] 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 c4b76b3918..0bf5cfe343 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 d105e4bdf6..4506942ab2 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; From 8262739b6219f54e7ea31de93cd81304df311ab9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:14:15 +0200 Subject: [PATCH 52/85] 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 7897c7ba0e..4a02a0c3f3 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 e2c0f08d87..91c74572e3 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 29b42cd54e..32902ee97a 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 709e3e7242..840b43f1b8 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 c569ee7b54..81ceaaca2d 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; From 81adc52671d920ffe184948267fcc1f9fbb027cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 11:30:02 +0200 Subject: [PATCH 53/85] 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 4a02a0c3f3..a3805dc0fd 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 91c74572e3..65ebb0c3db 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 32902ee97a..fca3918e1d 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); From c037cfe6f2daef8ff96cad965c4b040a9d8c62f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:29:30 +0200 Subject: [PATCH 54/85] 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 893d6d93ae..3310984b0b 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 96b4ce010d..d0c9ad09af 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 02f7ca4c31..db307c904f 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 427001a013..54f1813f5f 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; From ae7c99393606373e3a549946481fe07de6fb4c84 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 12:40:19 +0200 Subject: [PATCH 55/85] 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 3310984b0b..4e3d258403 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 d0c9ad09af..31e0e39078 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 db307c904f..6780c0c88c 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); From c0975af375c168be98804f025192bbb30710355d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 13:44:43 +0200 Subject: [PATCH 56/85] 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 0bf5cfe343..ddfbd65a19 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 b465f8407d..ad92f5b264 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; From bdccd69030e56381d906afeabc8305dbe18e2de6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 15:27:31 +0200 Subject: [PATCH 57/85] 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 a3805dc0fd..5504239c5e 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 65ebb0c3db..65e5b4042b 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 606521f200..937e4fa5ad 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; } From a432d0493c0761da075c7734c0f54f44d6121e78 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:44:36 +0200 Subject: [PATCH 58/85] 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 d91f7ab181..3807c5ea82 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(); From 06317046f2da215db328be660f900d265cdf9102 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 17:45:15 +0200 Subject: [PATCH 59/85] 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 ddfbd65a19..f05f970e58 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 4e3d258403..66f1c8c731 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 f0bbbcf1cb..cd55f60b06 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 81ceaaca2d..c173ecd504 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 54f1813f5f..651e311840 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 65ddfb4ad4..c7d25d1f08 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 e2b1525a23..ec2131abd8 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 */ }; /** From 98e74ed0e74e02f866fc7538c7d4153ae3c36743 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 24 Aug 2014 22:18:01 +0200 Subject: [PATCH 60/85] 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 f05f970e58..2fcf7bebb1 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 } From 8a9da209d194b4f35000935379901ed6091091f9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 23:17:56 +0200 Subject: [PATCH 61/85] 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 2fcf7bebb1..8b065560f4 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"); } } From 3d528a2c979e7d0df1171afc1f038759c7b01383 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:22:59 +0200 Subject: [PATCH 62/85] 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 098ff1a3d9..e3b5d30e49 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 c173ecd504..9a8c54e7e6 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 ad92f5b264..9dccb2309d 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) */ From 5e5322c593c6bd8ccd894f47ab8fd88b72e51677 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 22:46:09 +0200 Subject: [PATCH 63/85] 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 d5f569d71a..b5c5ef17c9 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 8a0b51b71b..b0f95aedf2 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 54c4fced62..445a89d3a7 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 From 9cc1f1ab9db4af9af18e6879ba82cbcfa8e588f3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 26 Aug 2014 23:12:28 +0200 Subject: [PATCH 64/85] 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 8b065560f4..5673037b88 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; From c79bc7073eb65f08b724a3893d183d20781f06e2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:34 +0200 Subject: [PATCH 65/85] Support for termination failsafe in PWM out driver --- src/drivers/drv_pwm_output.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 5aff6825be..bc586f3950 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -205,10 +205,13 @@ ORB_DECLARE(output_pwm); #define PWM_SERVO_GET_DISABLE_LOCKDOWN _IOC(_PWM_SERVO_BASE, 22) /** force safety switch off (to disable use of safety switch) */ -#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) +#define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23) /** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */ -#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) +#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24) + +/** make failsafe non-recoverable (termination) if it occurs */ +#define PWM_SERVO_SET_TERMINATION_FAILSAFE _IOC(_PWM_SERVO_BASE, 25) /* * From 1fbdca4ee988d5816eebbd9fef95ce498bacfd14 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 07:59:53 +0200 Subject: [PATCH 66/85] Add command to run termination failsafe --- src/systemcmds/pwm/pwm.c | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index c8d698b86a..478c2a772f 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -654,9 +654,28 @@ pwm_main(int argc, char *argv[]) } } exit(0); + } else if (!strcmp(argv[1], "terminatefail")) { + + if (argc < 3) { + errx(1, "arg missing [on|off]"); + } else { + + if (!strcmp(argv[2], "on")) { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 1); + } else { + /* force failsafe */ + ret = ioctl(fd, PWM_SERVO_SET_TERMINATION_FAILSAFE, 0); + } + + if (ret != OK) { + warnx("FAILED setting termination failsafe %s", argv[2]); + } + } + exit(0); } - usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail"); + usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail"); return 0; } From 49846d476f77290093c24097e05d5a8d60d1a4f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:12 +0200 Subject: [PATCH 67/85] IO firmware supports termination failsafe --- src/modules/px4iofirmware/mixer.cpp | 10 ++++++++++ src/modules/px4iofirmware/protocol.h | 3 ++- src/modules/px4iofirmware/registers.c | 18 ++++++++++++++++-- 3 files changed, 28 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 606c639f94..1eacda97a6 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -154,6 +154,16 @@ mixer_tick(void) } } + /* + * Check if failsafe termination is set - if yes, + * set the force failsafe flag once entering the first + * failsafe condition. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + (source == MIX_FAILSAFE)) { + r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; + } + /* * Check if we should force failsafe - and do it if we have to */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 0507836871..eae7f9567a 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -180,6 +180,7 @@ #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_ARMING_LOCKDOWN (1 << 7) /* If set, the system operates normally, but won't actuate any servos */ #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE (1 << 8) /* If set, the system will always output the failsafe values */ +#define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE (1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 0da778b6f6..7a5a5e4846 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-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 @@ -190,7 +190,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \ PX4IO_P_SETUP_ARMING_LOCKDOWN | \ - PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) + PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \ + PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -518,6 +519,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; } + /* + * If the failsafe termination flag is set, do not allow the autopilot to unset it + */ + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + + /* + * If failsafe termination is enabled and force failsafe bit is set, do not allow + * the autopilot to clear it. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) { + value |= (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE); + } + r_setup_arming = value; break; From a7109609ecfeba0a11121c1b83a46b1463f55931 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 08:00:28 +0200 Subject: [PATCH 68/85] support termination failsafe in IO driver --- src/drivers/px4io/px4io.cpp | 22 +++++++++++++++++++++- 1 file changed, 21 insertions(+), 1 deletion(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 97919538ff..3fdb1dd1f8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1175,6 +1175,14 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } + // XXX this is for future support in the commander + // but can be removed if unneeded + // if (armed.termination_failsafe) { + // set |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE; + // } + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -2038,7 +2046,8 @@ PX4IO::print_status(bool extended_status) ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : ""), ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN) ? " LOCKDOWN" : ""), - ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : "") + ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) ? " FORCE_FAILSAFE" : ""), + ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "") ); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", @@ -2262,6 +2271,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg) } break; + case PWM_SERVO_SET_TERMINATION_FAILSAFE: + /* if failsafe occurs, do not allow the system to recover */ + if (arg == 0) { + /* clear termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE, 0); + } else { + /* set termination failsafe flag */ + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE); + } + break; + case DSM_BIND_START: /* only allow DSM2, DSM-X and DSM-X with more than 7 channels */ From 91d50301c61cf495e83cab59621ef83cff24da3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 10:46:10 +0200 Subject: [PATCH 69/85] Do not enter RC override if FMU is lost and termination failsafe mode requested --- src/modules/px4iofirmware/mixer.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1eacda97a6..0c65b76425 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -139,7 +139,9 @@ mixer_tick(void) (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { + !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && + /* do not enter manual override if we asked for termination failsafe and FMU is lost */ + !(r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; From c85d7aae06b49f43e32dde05c00bcee1310aee9f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 20:36:59 +0200 Subject: [PATCH 70/85] Make sure we got to valid input at least once before kicking in failsafe --- src/modules/px4iofirmware/mixer.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 0c65b76425..17751bd6d2 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -71,6 +71,7 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; +static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -162,7 +163,8 @@ mixer_tick(void) * failsafe condition. */ if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && - (source == MIX_FAILSAFE)) { + (source == MIX_FAILSAFE) && + input_valid_initialized) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -180,6 +182,9 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); + + /* we got valid input, kick off the full failsafe checks */ + input_valid_initialized = true; } /* From 6773eb988017063366dce95bbfec8fae1f1913f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Aug 2014 21:39:33 +0200 Subject: [PATCH 71/85] Introduce FMU initialised status flag, only run failsafe trap if initialized once --- src/modules/px4iofirmware/mixer.cpp | 67 +++++++++++++++------------- src/modules/px4iofirmware/protocol.h | 1 + 2 files changed, 36 insertions(+), 32 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 17751bd6d2..3e19333d8e 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -58,7 +58,7 @@ extern "C" { /* * Maximum interval in us before FMU signal is considered lost */ -#define FMU_INPUT_DROP_LIMIT_US 200000 +#define FMU_INPUT_DROP_LIMIT_US 500000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -71,7 +71,6 @@ extern "C" { static bool mixer_servos_armed = false; static bool should_arm = false; static bool should_always_enable_pwm = false; -static bool input_valid_initialized = false; /* the input was valid at least once */ static volatile bool in_mixer = false; /* selected control values and count for mixing */ @@ -99,7 +98,8 @@ mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + if ((system_state.fmu_data_received_time == 0) || + hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { @@ -110,6 +110,9 @@ mixer_tick(void) } else { r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + + /* this flag is never cleared once OK */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED; } /* default to failsafe mixing - it will be forced below if flag is set */ @@ -157,14 +160,41 @@ mixer_tick(void) } } + /* + * Decide whether the servos should be armed right now. + * + * We must be armed, and we must have a PWM source; either raw from + * FMU or from the mixer. + * + */ + should_arm = ( + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) + ) + ); + + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); + /* * Check if failsafe termination is set - if yes, * set the force failsafe flag once entering the first * failsafe condition. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + if ( /* if we have requested flight termination style failsafe (noreturn) */ + (r_setup_arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) && + /* and we ended up in a failsafe condition */ (source == MIX_FAILSAFE) && - input_valid_initialized) { + /* and we should be armed, so we intended to provide outputs */ + should_arm && + /* and FMU is initialized */ + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED)) { r_setup_arming |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } @@ -182,35 +212,8 @@ mixer_tick(void) r_status_flags |= PX4IO_P_STATUS_FLAGS_FAILSAFE; } else { r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FAILSAFE); - - /* we got valid input, kick off the full failsafe checks */ - input_valid_initialized = true; } - /* - * Decide whether the servos should be armed right now. - * - * We must be armed, and we must have a PWM source; either raw from - * FMU or from the mixer. - * - * XXX correct behaviour for failsafe may require an additional case - * here. - */ - should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) - /* and FMU is armed */ && ( - ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) - /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) - /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) - ) - ); - - should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) - && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - /* * Run the mixers. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index eae7f9567a..4739f6e400 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -111,6 +111,7 @@ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ #define PX4IO_P_STATUS_FLAGS_SAFETY_OFF (1 << 12) /* safety is off */ +#define PX4IO_P_STATUS_FLAGS_FMU_INITIALIZED (1 << 13) /* FMU was initialized and OK once */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* [1] VBatt is very close to regulator dropout */ From 3d01da35d02505e751536e2cc09637797b37bed6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:27 +0200 Subject: [PATCH 72/85] 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 1115304d42..43222880e9 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 1297c1a9d3..93193f32ba 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 */ }; /** From 3f8793210b47bd8e09ed2adaabc2fab966db5df6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 10:34:52 +0200 Subject: [PATCH 73/85] 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 9885176b70..07fcb5d40e 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 30159dad9c..98c0982b21 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); From 2d3b6a88dee5220c1b35d6e32f264217da8e36b8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 17:18:06 +0200 Subject: [PATCH 74/85] swissfang: don't build MC apps on FMU1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2b..99c0eb8c1a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -67,17 +67,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -MODULES += modules/attitude_estimator_ekf +#MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -MODULES += modules/position_estimator_inav +#MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -MODULES += modules/mc_att_control -MODULES += modules/mc_pos_control +#MODULES += modules/mc_att_control +#MODULES += modules/mc_pos_control # # Logging From 033e4892ca2864c61e68c11c952d867ca6dc129e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: [PATCH 75/85] 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 07fcb5d40e..447e2189d5 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); @@ -1572,10 +1586,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 @@ -1608,6 +1623,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 98c0982b21..15c299a8bd 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 b0f95aedf2..9e5429988b 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 445a89d3a7..6a55e4948d 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 From 1d9c99956f3f84e4350734f1eef41df4c03411f8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: [PATCH 76/85] 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 447e2189d5..8dd6fb0861 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); @@ -1462,7 +1464,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 15c299a8bd..7d06003c91 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); From 5418412777ca96362cb5d866b2d0f5bb7efc3131 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:05:03 +0200 Subject: [PATCH 77/85] do send termination info only once --- 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 8dd6fb0861..bd5b464aa4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ 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"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } From 74601939a82174fcb6ef2d681e1f32b14a0a4ba0 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 13:45:21 +0200 Subject: [PATCH 78/85] Revert "do send termination info only once" This reverts commit 5418412777ca96362cb5d866b2d0f5bb7efc3131. --- 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 bd5b464aa4..8dd6fb0861 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,9 @@ 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"); } /* At this point the rc signal and the gps system have been checked @@ -1703,9 +1703,9 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of RC signal loss && gps failure"); - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); flight_termination_printed = true; } + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); } } From 9608e7adeb069878d5cedd3ece9edc7f4ac40ce4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: [PATCH 79/85] 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 8dd6fb0861..3a82720913 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1684,9 +1684,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 @@ -1705,7 +1708,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"); + } } } From eb90979ba8254d7ade8ce530c7e4643d48c4196b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: [PATCH 80/85] 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 651e311840..5564a1c42e 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 83d23cf499..f1a01c73b5 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); From 97f7c0088f07cb86463501e81b58c5dba971a50c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: [PATCH 81/85] 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 79992bf451..d177f75522 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 @@ -1223,7 +1223,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) { From e8503ab3f90353957136d60dbc3fa7668249119c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: [PATCH 82/85] 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 e3b5d30e49..9568752aed 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 bef2f7ca53..e770c11a27 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -893,8 +893,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); From 940264f6ab565caee7a5426cfa1023b306c97e74 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: [PATCH 83/85] 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 3a82720913..c53b143491 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; } } @@ -1586,18 +1577,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 7d06003c91..f4bc7fd7fb 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 From 2148464a705edea20a0f0311b1904079f598eab8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: [PATCH 84/85] 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 5504239c5e..0f431ded26 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 65e5b4042b..9d647cb68a 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 9a8c54e7e6..6710247a7b 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 From 54380e34b344108c72d2c46e9ca5e6b0b22390c8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: [PATCH 85/85] 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 840b43f1b8..bf42acff98 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 6710247a7b..0e7f116ea4 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"), @@ -532,7 +531,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"); @@ -540,7 +539,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"); } }