diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index 19783cbb41f..5900f18d03d 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -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 | | ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index 7ff0cbf87af..8015d6f4c3e 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -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: diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index a8658655548..997f5b4e35e 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -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: diff --git a/src/modules/commander/failsafe/failsafe_test.cpp b/src/modules/commander/failsafe/failsafe_test.cpp index aed63a22820..669fe7afec5 100644 --- a/src/modules/commander/failsafe/failsafe_test.cpp +++ b/src/modules/commander/failsafe/failsafe_test.cpp @@ -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); +} diff --git a/src/modules/commander/failsafe/framework.cpp b/src/modules/commander/failsafe/framework.cpp index 53e9b6586aa..a0661c4b85e 100644 --- a/src/modules/commander/failsafe/framework.cpp +++ b/src/modules/commander/failsafe/framework.cpp @@ -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(); diff --git a/src/modules/commander/failsafe/framework.h b/src/modules/commander/failsafe/framework.h index 7c1c8f01ec1..23be7df1637 100644 --- a/src/modules/commander/failsafe/framework.h +++ b/src/modules/commander/failsafe/framework.h @@ -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