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);