From f13c3a7d44c58f69c77f9e4f82cf3d6a41ec9551 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 21 Jan 2021 15:11:02 +0100 Subject: [PATCH] state_machine_helper: stop mission on RC loss This allows to still fly missions completely without RC but reacts if RC is lost during the mission because the safety pilot expects to be able to take over. --- .../commander/state_machine_helper.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4cdaa4eb6e..54417eb772 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -514,22 +514,32 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_ } else if (status->data_link_lost && data_link_loss_act_configured && is_armed && !landed) { - // Data link lost, data link loss reaction configured, do configured reaction + // Data link lost, data link loss reaction configured -> do configured reaction 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, 0); - } else if (status->rc_signal_lost + } else if (status->rc_signal_lost && rc_loss_act_configured && status_flags.rc_signal_found_once + && is_armed && !landed) { + // RC link lost, rc loss reaction configured, RC was used before -> RC loss reaction after delay + // Safety pilot expects to be able to take over by RC in case anything unexpected happens + 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, param_com_rcl_act_t); + + } else if (status->rc_signal_lost && rc_loss_act_configured && status->data_link_lost && !data_link_loss_act_configured && is_armed && !landed) { - // All links lost, no data link loss reaction configured, immediately do RC loss reaction + // All links lost, no data link loss reaction configured -> immediately do RC loss reaction + // Lost all communication, by default it's considered unsafe to continue the mission + // Note this case is reached after the previous one when flying mission completely without RC enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); - set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, param_com_rcl_act_t); + set_link_loss_nav_state(status, armed, status_flags, internal_state, rc_loss_act, 0); } else if (status->rc_signal_lost && !rc_loss_act_configured && status->data_link_lost && !data_link_loss_act_configured && is_armed && !landed && mission_finished) { - // All links lost, all link loss reactions disabled, return after mission to prevent stuck vehicle + // All links lost, all link loss reactions disabled -> return after mission + // Pilot disabled all reactions, finish mission but then return to avoid lost vehicle enable_failsafe(status, old_failsafe, mavlink_log_pub, reason_no_datalink); set_link_loss_nav_state(status, armed, status_flags, internal_state, link_loss_actions_t::AUTO_RTL, 0);