fix(failsafe): let GNSS loss recover during the Hold delay (#29814)

* fix(failsafe): allow GNSS loss to recover during the Hold delay

A GNSS failure that clears during the COM_FAIL_ACT_T Hold still latched
Return or Land, because COM_GNSSLOSS_ACT uses the OnModeChangeOrDisarm
clear condition. The vehicle then abandoned a mission it could have
continued.

If the failure clears while the vehicle is still holding, the action is
dropped and the user-intended mode continues, e.g. the mission resumes.
Once Return or Land has started, it stays latched as before. With
COM_FAIL_ACT_T=0 there is no Hold, so nothing changes.

Assisted-by: Claude:claude-opus-5-5

* fix(failsafe): restart the Hold delay after a failure clears during it

When a failsafe cleared during its Hold delay, the remaining delay kept
counting down. A new failure within that time did not reload it and only
got what was left, so it could Return almost without a Hold.

Reset the delay once no failsafe is pending, so that the next failure
gets the full COM_FAIL_ACT_T again. This applies to every failsafe.
Repeated failures still escalate: the start delay shrinks while a Hold
is active and refills at a quarter of that rate.

Assisted-by: Claude:claude-opus-5-5

* revert(failsafe): keep the remaining Hold delay after a failure clears

This reverts commit 5e9a8e8a14.

Restarting the delay for every new failure changed how a flapping
condition is handled, for every failsafe and not only GNSS. Without the
reset, a failure that comes back keeps the delay left over from the
previous one, so the action still fires on time. With it, only the start
delay escalation limits the flapping, which can take much longer, or
never fire if the condition is active less than a fifth of the time.

Keep the PR scoped to letting GNSS loss recover during the Hold.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

* fix(failsafe): only clear GNSS loss during the Hold delay before Return starts

_current_delay is shared between failsafes. If another failsafe with a
delay is added on the same update as GNSS recovers, it re-arms the delay
and the GNSS Return or Land that was already running got removed.

Also require that the selected action is not beyond Hold yet.

Assisted-by: Claude:claude-opus-5-5
Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>

---------

Signed-off-by: Andrea Bernasconi <andrea.bernasconi@auterion.com>
Co-authored-by: Anil Kircaliali <anil.kircaliali@auterion.com>
This commit is contained in:
Andrea BernasconiandAnil Kircaliali authored and GitHub committed 2026-11-18 09:38:57 +01:00
1 parent 52c4777580
commit 70a735246f
6 files changed
+101 -1

No files matched your search

+2
View File
@@ -316,6 +316,8 @@ Loss of a single GPS when none are required is handled by other GPS health check
When `COM_GNSSLOSS_ACT` is set to `Warning`, both triggers produce a warning only and never block arming.
When set to `Warning, block arming`, both triggers block arming and only produce a warning in flight.
When set to `Return`, `Land`, or `Terminate`, both triggers block arming and fire the configured in-flight failsafe action (subject to the `SYS_HAS_NUM_GNSS` condition above).
If the failure clears during the [COM_FAIL_ACT_T](#COM_FAIL_ACT_T) Hold, `Return` and `Land` are skipped and the vehicle continues in its previous mode (e.g. the mission resumes).
Once `Return` or `Land` has started, it continues even if the GNSS failure clears.
| Parameter | Description |
| ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
@@ -128,6 +128,7 @@ parameters:
when SYS_HAS_NUM_GNSS=0), or when two receivers report positions inconsistent
with their reported accuracy and SYS_HAS_NUM_GNSS=2 (otherwise warning only).
The reported accuracy of each receiver is floored with EKF2_GPS_P_NOISE.
Return and Land are skipped if the failure clears during the COM_FAIL_ACT_T Hold.
Any value above Warning also blocks arming.
type: enum
values:
@@ -431,11 +431,13 @@ FailsafeBase::ActionOptions Failsafe::fromGnssLossActParam(int param_value)
case gps_redundancy_failsafe_mode::Return_mode:
options.action = Action::RTL;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
options.clear_during_delay = true; // a short loss only causes a Hold
break;
case gps_redundancy_failsafe_mode::Land_mode:
options.action = Action::Land;
options.clear_condition = ClearCondition::OnModeChangeOrDisarm;
options.clear_during_delay = true; // a short loss only causes a Hold
break;
case gps_redundancy_failsafe_mode::Terminate:
@@ -1218,3 +1218,92 @@ TEST_F(FailsafeTest, BatteryFailsafeUsesLowBatActParam)
EXPECT_EQ(selectedActionFor(4, kEmergency, false), FailsafeBase::Action::Terminate);
EXPECT_EQ(selectedActionFor(6, kEmergency, false), FailsafeBase::Action::Terminate);
}
TEST_F(FailsafeTest, GnssLostRecoversDuringHoldDelay)
{
int32_t com_gnssloss_act = 1; // Return
param_set(param_handle(px4::params::COM_GNSSLOSS_ACT), &com_gnssloss_act);
Failsafe failsafe(nullptr);
failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
// GNSS lost -> Hold
time += 10_ms;
failsafe_flags.gnss_lost = true;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
// Recovers during the Hold delay -> back to the mission
time += 300_ms;
failsafe_flags.gnss_lost = false;
uint8_t updated_user_intented_mode = failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
ASSERT_EQ(updated_user_intented_mode, state.user_intended_mode);
// Lost for longer than the Hold delay -> RTL
time += 20_s;
failsafe_flags.gnss_lost = true;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
time += 6_s;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// Recovering now does not abort the RTL
time += 10_ms;
failsafe_flags.gnss_lost = false;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
}
TEST_F(FailsafeTest, GnssRecoveryDoesNotAbortRtlWhenAnotherFailsafeStartsADelay)
{
int32_t com_gnssloss_act = 1; // Return
param_set(param_handle(px4::params::COM_GNSSLOSS_ACT), &com_gnssloss_act);
Failsafe failsafe(nullptr);
failsafe_flags_s failsafe_flags{};
mode_util::getModeRequirements(vehicle_status_s::VEHICLE_TYPE_ROTARY_WING, failsafe_flags);
FailsafeBase::State state{};
state.armed = true;
state.user_intended_mode = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
state.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
hrt_abstime time = 5_s;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::None);
// GNSS lost for longer than the Hold delay -> RTL
time += 10_ms;
failsafe_flags.gnss_lost = true;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::Hold);
time += 6_s;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// Let the start delay refill, so that a new failsafe starts a delay again
time += 8_s;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
// Another failsafe starts a delay on the same update as GNSS recovers: the RTL continues
time += 10_ms;
failsafe_flags.navigator_failure = true;
failsafe_flags.gnss_lost = false;
failsafe.update(time, state, false, false, failsafe_flags);
ASSERT_EQ(failsafe.selectedAction(), FailsafeBase::Action::RTL);
}
+6 -1
View File
@@ -404,7 +404,12 @@ void FailsafeBase::removeAction(ActionOptions &action) const
// 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) {
// The action did not start yet (still in the delayed Hold), so recovering is allowed.
// _current_delay is shared, so also check that no action beyond Hold is already running.
const bool remove_during_delay = action.clear_during_delay && _current_delay > 0
&& _selected_action <= Action::Hold;
if (action.clear_condition == ClearCondition::WhenConditionClears || remove_while_deferring || remove_during_delay) {
// Remove action
PX4_DEBUG("Caller %i: state changed to valid, removing action", action.id);
action.setInvalid();
@@ -195,6 +195,7 @@ protected:
Cause cause{Cause::Generic};
UserTakeoverAllowed allow_user_takeover{UserTakeoverAllowed::Auto};
bool can_be_deferred{true};
bool clear_during_delay{false}; ///< also clear if the condition clears during the Hold delay (COM_FAIL_ACT_T)
bool state_failure{false}; ///< used when the clear_condition isn't set to clear immediately
bool activated{false}; ///< true if checkFailsafe was called during current update