diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 5bae2d601b..d58f75d8e7 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -432,3 +432,28 @@ TEST_F(FailsafeTest, skip_failsafe) ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn); } + +TEST_F(FailsafeTest, user_termination) +{ + FailsafeTester failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_TERMINATION; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 5_s; + + // User intended termination -> failsafe termination + uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + EXPECT_EQ(updated_user_intented_mode, state.user_intended_mode); + EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); + + // Links lost during termination -> stay in termination + failsafe_flags.gcs_connection_lost = true; + failsafe_flags.manual_control_signal_lost = true; + + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + EXPECT_EQ(updated_user_intented_mode, state.user_intended_mode); + EXPECT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Terminate); +} diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index a5395b117b..ab3e30c847 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -443,7 +443,8 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s returned_state.updated_user_intended_mode = state.user_intended_mode; returned_state.cause = Cause::Generic; - if (_selected_action == Action::Terminate) { // Terminate never clears + if (state.user_intended_mode == vehicle_status_s::NAVIGATION_STATE_TERMINATION + || _selected_action == Action::Terminate) { // Terminate never clears returned_state.action = Action::Terminate; return; }