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;