From 26dba1407bd1fbc65c23870a22fed904afba6347 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 3 Feb 2022 14:18:28 +0100 Subject: [PATCH] Commander: VTOL quadchuate also triggers a RTL in Loiter and VTOL_Takeoff modes, not just in Mission (#19123) Signed-off-by: Silvan Fuhrer --- src/modules/commander/state_machine_helper.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4edfc608a9..e07ec91174 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -629,6 +629,10 @@ 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, true)) { // nothing to do - everything done in check_invalid_pos_nav_state + + } else if (status_flags.vtol_transition_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } 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, event_failsafe_reason_t::no_datalink); @@ -739,6 +743,10 @@ 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_flags.vtol_transition_failure) { + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + } 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, event_failsafe_reason_t::no_datalink);