From 26d74bf57d666f248f580bbb0811a92d3d8d2f2c Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 13 Jul 2020 13:10:59 +0200 Subject: [PATCH] Commander: enable failsafe delay for position and mission mode Instead of directly doing the link loss reaction which by default is RTL a delay can be configured such that the drone first switches to hold and waits for the link to be regained. --- msg/vehicle_status.msg | 1 + src/modules/commander/Commander.cpp | 3 ++- src/modules/commander/Commander.hpp | 1 + src/modules/commander/commander_params.c | 15 +++++++++++ .../commander/state_machine_helper.cpp | 25 ++++++++++++++++--- src/modules/commander/state_machine_helper.h | 3 ++- 6 files changed, 43 insertions(+), 5 deletions(-) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index a968bebf0a..b841549637 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -63,6 +63,7 @@ uint64 nav_state_timestamp # time when current nav_state activated uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state (e.g.:RTL, Hover, Terminate, ...) +uint64 failsafe_timestamp # time when failsafe was activated uint8 system_type # system type, contains mavlink MAV_TYPE uint8 system_id # system id, contains MAVLink's system ID field diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 7d6bd001a2..255bc0b1d6 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -2545,7 +2545,8 @@ Commander::run() (link_loss_actions_t)_param_nav_rcl_act.get(), (offboard_loss_actions_t)_param_com_obl_act.get(), (offboard_loss_rc_actions_t)_param_com_obl_rc_act.get(), - (position_nav_loss_actions_t)_param_com_posctl_navl.get()); + (position_nav_loss_actions_t)_param_com_posctl_navl.get(), + _param_com_ll_delay.get()); if (nav_state_changed) { _status.nav_state_timestamp = hrt_absolute_time(); diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index d47d18454e..576ac7655d 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -189,6 +189,7 @@ private: (ParamInt) _param_nav_dll_act, (ParamInt) _param_com_dl_loss_t, + (ParamFloat) _param_com_ll_delay, (ParamInt) _param_com_hldl_loss_t, (ParamInt) _param_com_hldl_reg_t, diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f0fe913968..2523fa636d 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -968,3 +968,18 @@ PARAM_DEFINE_FLOAT(COM_LKDOWN_TKO, 3.0f); * @value 1 Enabled */ PARAM_DEFINE_INT32(COM_ARM_ARSP_EN, 1); + +/** + * Delay between link loss and configured reaction in Position and Mission mode + * + * A non-zero, positive value makes the failsafe reaction first stop the vehicle and wait + * before proceeding with the configured failsafe reaction NAV_RCL_ACT. + * A zero or negative value disables the delay. + * + * @group Commander + * @unit s + * @min -1.0 + * @max 60.0 + * @decimal 3 + */ +PARAM_DEFINE_FLOAT(COM_LL_DELAY, 15.0f); diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index afc880ffeb..e2d194fcbe 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -45,10 +45,13 @@ #include #include #include +#include #include "state_machine_helper.h" #include "commander_helper.h" +using namespace time_literals; + static constexpr const char reason_no_rc[] = "No manual control stick input"; static constexpr const char reason_no_offboard[] = "no offboard"; static constexpr const char reason_no_rc_and_no_offboard[] = "no RC and no offboard"; @@ -385,6 +388,7 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai void enable_failsafe(vehicle_status_s *status, bool old_failsafe, orb_advert_t *mavlink_log_pub, const char *reason) { if (!old_failsafe && status->arming_state == vehicle_status_s::ARMING_STATE_ARMED) { + status->failsafe_timestamp = hrt_absolute_time(); mavlink_log_critical(mavlink_log_pub, "Failsafe enabled: %s", reason); } @@ -399,7 +403,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, const offboard_loss_rc_actions_t offb_loss_rc_act, - const position_nav_loss_actions_t posctl_nav_loss_act) + const position_nav_loss_actions_t posctl_nav_loss_act, + const float param_com_ll_delay) { const navigation_state_t nav_state_old = status->nav_state; @@ -465,7 +470,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ if (rc_lost && is_armed) { enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_rc); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); + if (hrt_elapsed_time(&status->failsafe_timestamp) > (param_com_ll_delay * 1_s)) { + /* only start reaction after configured delay */ + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act); + + } else { + /* wait in hold mode if delay is configured */ + set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_LOITER); + } /* As long as there is RC, we can fallback to ALTCTL, or STAB. */ /* A local position estimate is enough for POSCTL for multirotors, @@ -508,7 +520,14 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ * check for datalink lost: this should always trigger RTL */ enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act); + if (hrt_elapsed_time(&status->failsafe_timestamp) > (param_com_ll_delay * 1_s)) { + /* only start reaction after configured delay */ + set_link_loss_nav_state(status, armed, status_flags, internal_state, data_link_loss_act); + + } else { + /* wait in hold mode if delay is configured */ + set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_LOITER); + } } else if (!data_link_loss_act_configured && status->rc_signal_lost && status->data_link_lost && !landed && mission_finished && is_armed) { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index f6727d19a2..0adb54602d 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -131,7 +131,8 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ const bool stay_in_failsafe, const vehicle_status_flags_s &status_flags, bool landed, const link_loss_actions_t rc_loss_act, const offboard_loss_actions_t offb_loss_act, const offboard_loss_rc_actions_t offb_loss_rc_act, - const position_nav_loss_actions_t posctl_nav_loss_act); + const position_nav_loss_actions_t posctl_nav_loss_act, + const float param_com_ll_delay); /* * Checks the validty of position data against the requirements of the current navigation