diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index ad0f2e0615..a02c42f898 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -369,3 +369,26 @@ TEST_F(FailsafeTest, defer) ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); ASSERT_FALSE(failsafe.failsafeDeferred()); } + +TEST_F(FailsafeTest, skip_failsafe) +{ + FailsafeTester failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_RTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 5_s; + + uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // RC lost while in RTL -> stay in RTL and only warn + failsafe_flags.manual_control_signal_lost = true; + + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn); +} diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 729205ebe8..6c0edb58c4 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -608,6 +608,15 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s } } + // If already in RTL, do not go into RTL again (would cause a Hold delay first, then re-start RTL) + if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL) { + if ((selected_action == Action::RTL || returned_state.delayed_action == Action::RTL) + && modeCanRun(status_flags, vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)) { + selected_action = Action::Warn; + returned_state.delayed_action = Action::None; + } + } + // If already precision landing, do not go into RTL or Land if (returned_state.updated_user_intended_mode == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND) { if ((selected_action == Action::RTL || selected_action == Action::Land ||