diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 81eba458ae..9e5dc1d2c6 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -63,7 +63,8 @@ protected: CHECK_FAILSAFE(status_flags, offboard_control_signal_lost, ActionOptions(Action::Hold)); CHECK_FAILSAFE(status_flags, navigator_failure, ActionOptions(Action::Warn)); - CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, ActionOptions(Action::None)); + CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, + ActionOptions(Action::None).allowUserTakeover(UserTakeoverAllowed::AlwaysModeSwitchOnly)); _last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred()); @@ -338,6 +339,45 @@ TEST_F(FailsafeTest, no_delay_for_warn) ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn); } +TEST_F(FailsafeTest, none_action_does_not_restrict_user_takeover) +{ + FailsafeTester failsafe(nullptr); + + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_MANUAL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 3847124342; + failsafe_flags_s failsafe_flags{}; + bool user_intended_mode_updated = false; + + // Add an active Action::None entry with a restrictive takeover setting. + failsafe_flags.fd_imbalanced_prop = true; + uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + + // Battery time low -> Hold for the delay + time += 10_ms; + failsafe_flags.battery_low_remaining_time = true; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold); + + // Delay over -> RTL + time += 5_s; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL); + + // User wants takeover via sticks. The active Action::None must not downgrade this to mode-switch-only. + time += 10_ms; + updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, true, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, vehicle_status_s::NAVIGATION_STATE_POSCTL); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Warn); + ASSERT_TRUE(failsafe.userTakeoverActive()); +} + TEST_F(FailsafeTest, no_immediate_takeover_when_failsafe_on_mode_switch) { FailsafeTester failsafe(nullptr); diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 0af4123d67..259fe7b5e5 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -467,6 +467,10 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s const ActionOptions &cur_action = _actions[action_idx]; if (cur_action.valid()) { + if (cur_action.action == Action::None) { + continue; + } + if (cur_action.allow_user_takeover > allow_user_takeover) { // Use the most restrictive setting among all active actions allow_user_takeover = cur_action.allow_user_takeover;