commander failsafe: never override user intended termination + unit test

Author: MaEtUgR

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);
+}

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;
 	}