From 3eb02559226dd3bf14ce2c9548f02e3a69697d53 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Thu, 5 Feb 2026 10:15:04 +0100 Subject: [PATCH] fix commander: do not trigger Hold (delay) when failsafe action is Warn The new unit test failed before and passes with the fix. --- .../commander/failsafe/failsafe_test.cpp | 33 +++++++++++++++++++ src/modules/commander/failsafe/framework.cpp | 1 + 2 files changed, 34 insertions(+) diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index 2c190c660c..81eba458ae 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -62,6 +62,9 @@ protected: CHECK_FAILSAFE(status_flags, battery_low_remaining_time, ActionOptions(Action::RTL).causedBy(Cause::RemainingFlightTimeLow)); 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)); + _last_state_test = checkFailsafe(_caller_id_test, _last_state_test, status_flags.fd_motor_failure && status_flags.fd_critical_failure, ActionOptions(Action::Terminate).cannotBeDeferred()); } @@ -305,6 +308,36 @@ TEST_F(FailsafeTest, can_takeover_degraded_failsafe) ASSERT_TRUE(failsafe.userTakeoverActive()); } +TEST_F(FailsafeTest, no_delay_for_warn) +{ + // Ensure there is no Hold/delay when the current action is Warn + 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; + + uint8_t updated_user_intented_mode = failsafe.update(time, state, user_intended_mode_updated, false, failsafe_flags); + + // Navigator failure -> Warn + time += 10_ms; + failsafe_flags.navigator_failure = 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::Warn); + + // Imbalanced props -> Warn (no delay) + time += 5_s; + failsafe_flags.fd_imbalanced_prop = 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::Warn); +} + 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 19528b59e9..f8e7e29a0e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -488,6 +488,7 @@ void FailsafeBase::getSelectedAction(const State &state, const failsafe_flags_s // Check if we should enter delayed Hold const bool action_can_be_delayed = selected_action != Action::None && + selected_action != Action::Warn && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold;