diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index a02c42f898..b155300faa 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -392,3 +392,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 8d633e9f21..30375908a9 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -437,7 +437,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; }