From 168d99cd18f08bd28c246f07e9dabb2bfeb335fb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 9 Jul 2025 13:53:36 +0200 Subject: [PATCH] commander: do not trigger obsolete failsafe when deactivating failsafe deferring Previously, when deferring was active and e.g. RC loss was triggered, and RC regained, the action was not cleared, as the RC loss action only clears on mode switch/disarm (when set to RTL for example). When deferring was then disabled, the RC loss failsafe would still trigger. This changes the behavior to immediately remove those actions when deferring is active. It also ensures to reset the Hold delay when deferring is disabled and no failsafe is being deferred. --- .../commander/failsafe/failsafe_test.cpp | 40 +++++++++++++++++++ src/modules/commander/failsafe/framework.cpp | 17 +++++++- 2 files changed, 55 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index a02c42f898..5bae2d601b 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -370,6 +370,46 @@ TEST_F(FailsafeTest, defer) ASSERT_FALSE(failsafe.failsafeDeferred()); } +TEST_F(FailsafeTest, defer_and_clear) +{ + FailsafeTester failsafe(nullptr); + + failsafe_flags_s failsafe_flags{}; + FailsafeBase::State state{}; + state.armed = true; + state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_POSCTL; + state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; + hrt_abstime time = 3847124342; + + uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + + failsafe.deferFailsafes(true, -1); + ASSERT_TRUE(failsafe.getDeferFailsafes()); + ASSERT_FALSE(failsafe.failsafeDeferred()); + // Manual control lost -> deferred + time += 10_ms; + failsafe_flags.manual_control_signal_lost = true; + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + ASSERT_TRUE(failsafe.failsafeDeferred()); + + // Clear flag (the failsafe action only clears on mode switch, but we still expect it to clear as it's being deferred) + failsafe_flags.manual_control_signal_lost = false; + time += 5_s; + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + ASSERT_FALSE(failsafe.failsafeDeferred()); + + // Wait a bit, don't defer anymore -> no failsafe triggered + time += 1_s; + failsafe.deferFailsafes(false, 0); + updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags); + ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode); + ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None); + ASSERT_FALSE(failsafe.getDeferFailsafes()); + ASSERT_FALSE(failsafe.failsafeDeferred()); +} + TEST_F(FailsafeTest, skip_failsafe) { FailsafeTester failsafe(nullptr); diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 0632c0681e..7b3e48c9dd 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -396,7 +396,11 @@ bool FailsafeBase::checkFailsafe(int caller_id, bool last_state_failure, bool cu void FailsafeBase::removeAction(ActionOptions &action) const { - if (action.clear_condition == ClearCondition::WhenConditionClears) { + // If failsafes are being deferred and the action can be deferred, remove it immediately independent of the + // clear_condition to avoid triggering a failsafe after deferring is disabled. + const bool remove_while_deferring = _defer_failsafes && action.can_be_deferred; + + if (action.clear_condition == ClearCondition::WhenConditionClears || remove_while_deferring) { // Remove action PX4_DEBUG("Caller %i: state changed to valid, removing action", action.id); action.setInvalid(); @@ -482,8 +486,13 @@ 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::Disarm && + selected_action != Action::Terminate && + selected_action != Action::Hold; + if (_current_delay > 0 && !_user_takeover_active && allow_user_takeover <= UserTakeoverAllowed::AlwaysModeSwitchOnly - && selected_action != Action::Disarm && selected_action != Action::Terminate && selected_action != Action::Hold) { + && action_can_be_delayed) { returned_state.delayed_action = selected_action; selected_action = Action::Hold; allow_user_takeover = UserTakeoverAllowed::AlwaysModeSwitchOnly; @@ -711,6 +720,10 @@ bool FailsafeBase::deferFailsafes(bool enabled, int timeout_s) return false; } + if (!enabled && _failsafe_defer_started == 0) { + _current_delay = 0; + } + if (timeout_s == 0) { _defer_timeout = DEFAULT_DEFER_TIMEOUT;