From bec9eef5b0283b6d74e95cc73c2b20e5b09885d4 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Mon, 21 Jun 2021 13:56:53 +0200 Subject: [PATCH] state_machine_helper: react on link losses during takeoff The same way like in loiter. This should be further unified in the code but at least we can increase safety in the case the takeoff altitude is very high and the vehicle never reaches it but looses links. --- .../commander/state_machine_helper.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 0ad1a48bb4..a78e76b9f1 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -694,6 +694,34 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) { // nothing to do - everything done in check_invalid_pos_nav_state + } else if (status.data_link_lost && data_link_loss_act_configured && !landed && is_armed) { + // 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 && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status_flags.rc_signal_found_once && is_armed && !landed) { + // RC link lost, rc loss not disabled in loiter, 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 && !(param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && 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 + // Lost all communication, by default it's considered unsafe to continue the mission + // This is only reached when flying mission completely without RC (it was not present since boot) + 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, 0); + + } else if (status.rc_signal_lost && (param_com_rcl_except & RCLossExceptionBits::RCL_EXCEPT_HOLD) + && status.data_link_lost && !data_link_loss_act_configured + && is_armed && !landed) { + // All links lost, all link loss reactions disabled -> return + // Pilot disabled all reactions, 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); } else { status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF;