fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: do not trigger obsolete failsafe when deactivating failsafe deferring

Browse Source 
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.
This commit is contained in:
Beat Küng 2025-07-09 13:53:36 +02:00 committed by Silvan Fuhrer
parent b8dacf5ae4
commit 168d99cd18
2 changed files with 55 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/commander/failsafe/failsafe_test.cpp
View File

@ -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);

17
src/modules/commander/failsafe/framework.cpp
View File

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