diff --git a/msg/FailsafeFlags.msg b/msg/FailsafeFlags.msg index e0094a575c..ec3ceb67e5 100644 --- a/msg/FailsafeFlags.msg +++ b/msg/FailsafeFlags.msg @@ -59,4 +59,4 @@ bool flight_time_limit_exceeded # Maximum flight time exceeded bool position_accuracy_low # Position estimate has dropped below threshold, but is currently still declared valid bool navigator_failure # Navigator failed to execute a mode bool parachute_unhealthy # Parachute system missing or unhealthy -bool gps_redundancy_lost # Number of active GPS receivers dropped below COM_GPS_MIN +bool gnss_lost # Active GNSS count dropped below SYS_HAS_NUM_GNSS, or two receivers report inconsistent positions diff --git a/src/lib/systemlib/system_params.yaml b/src/lib/systemlib/system_params.yaml index e1c7d2cb86..ac8547325c 100644 --- a/src/lib/systemlib/system_params.yaml +++ b/src/lib/systemlib/system_params.yaml @@ -136,18 +136,17 @@ parameters: type: boolean default: 1 reboot_required: true - SYS_HAS_NUM_GPS: + SYS_HAS_NUM_GNSS: description: - short: Control if and how many GPS receivers are expected + short: Control if and how many GNSS receivers are expected long: |- - 0: Disable GPS redundancy check (any number of GPS is acceptable). - 1-N: Require the presence of N GPS receivers for arming and during flight. + 0: Disable GNSS redundancy check (any number of GNSS receivers is acceptable). + 1-N: Require the presence of N GNSS receivers for arming and during flight. If the active count drops below this value in flight, COM_GPS_LOSS_ACT is triggered. type: int32 default: 0 min: 0 max: 2 - reboot_required: false SYS_HAS_MAG: description: short: Control if and how many magnetometers are expected diff --git a/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp b/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp index 67820e5f34..42f6d83cdc 100644 --- a/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp @@ -89,7 +89,7 @@ public: f = 0.f; param_set(param_find("SENS_GPS1_OFFY"), &f); int i = 0; - param_set(param_find("SYS_HAS_NUM_GPS"), &i); + param_set(param_find("SYS_HAS_NUM_GNSS"), &i); param_set(param_find("COM_GPS_LOSS_ACT"), &i); // Construct check after params are set so ParamFloat reads correct initial values diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp index 0403a835e3..4f5e364ad5 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp @@ -40,12 +40,18 @@ using namespace time_literals; // eph is firmware-dependent and not a rigorous 1-sigma, so 3 is a heuristic consistency gate // rather than a precise statistical claim. It relaxes the gate automatically when receivers degrade. static constexpr float GPS_DIVERGENCE_SIGMA = 3.0f; -static constexpr uint64_t GPS_DIVERGENCE_SUSTAIN = 2_s; // must be sustained before warning + +// Matches the 1 s staleness threshold used by other sensor checks in this framework. +static constexpr uint64_t GNSS_ONLINE_TIMEOUT = 1_s; + +// Multipath spikes typically resolve within 1 s; 2 s filters these out while still +// detecting real receiver faults promptly. +static constexpr uint64_t GNSS_DIVERGENCE_SUSTAIN = 2_s; void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporter) { // Always reset — will be set below only when the condition is active - reporter.failsafeFlags().gps_redundancy_lost = false; + reporter.failsafeFlags().gnss_lost = false; // Separate "online" (present + fresh data) from "fixed" (online + 3D fix). // online_count tracks receivers that are communicating; fixed_count tracks those @@ -60,7 +66,7 @@ void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporte if (_sensor_gps_sub[i].copy(&gps) && (gps.device_id != 0) - && (hrt_elapsed_time(&gps.timestamp) < 2_s)) { + && (hrt_elapsed_time(&gps.timestamp) < GNSS_ONLINE_TIMEOUT)) { online_count++; if (gps.fix_type >= 3) { @@ -92,7 +98,7 @@ void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporte // Pre-arm: trigger immediately so the operator can decide before takeoff. // In-flight: require sustained divergence to avoid false alarms from transient multipath. - const uint64_t sustain = context.isArmed() ? GPS_DIVERGENCE_SUSTAIN : 0_s; + const uint64_t sustain = context.isArmed() ? GNSS_DIVERGENCE_SUSTAIN : 0_s; if (divergence_m > gate_m) { if (_divergence_since == 0) { @@ -124,58 +130,52 @@ void GpsRedundancyChecks::checkAndReport(const Context &context, Report &reporte } } - // Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GPS + // Track the highest fixed count seen — used to detect any GPS loss regardless of SYS_HAS_NUM_GNSS if (fixed_count > _peak_fixed_count) { _peak_fixed_count = fixed_count; } - const int required = _param_sys_has_num_gps.get(); + const int required = _param_sys_has_num_gnss.get(); const bool below_required = (required > 0 && fixed_count < required); const bool dropped_below_peak = (_peak_fixed_count > 1 && fixed_count < _peak_fixed_count); - if (!below_required && !dropped_below_peak && !divergence_active) { - return; - } + reporter.failsafeFlags().gnss_lost = below_required || divergence_active; - reporter.failsafeFlags().gps_redundancy_lost = below_required || divergence_active; + if (below_required || dropped_below_peak) { + // act==0: warn only, never blocks arming; act>0: blocks arming and shows red + const bool block_arming = below_required && (_param_com_gps_loss_act.get() > 0); + const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None; + const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning; + const uint8_t expected = below_required ? (uint8_t)required : (uint8_t)_peak_fixed_count; - if (!below_required && !dropped_below_peak) { - return; - } + // Differentiate: if online_count is also low the receiver is offline; otherwise it lost fix. + if (online_count < fixed_count || online_count < required) { + /* EVENT + * @description + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GPS_LOSS_ACT. + * + */ + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_redundancy_offline"), + log_level, + "GPS receiver offline: {1} of {2} online", + (uint8_t)online_count, expected); - // act==0: warn only, never blocks arming; act>0: blocks arming and shows red - const bool block_arming = below_required && (_param_com_gps_loss_act.get() > 0); - const NavModes nav_modes = block_arming ? NavModes::All : NavModes::None; - const events::Log log_level = block_arming ? events::Log::Error : events::Log::Warning; - const uint8_t expected = below_required ? (uint8_t)required : (uint8_t)_peak_fixed_count; - - // Differentiate: if online_count is also low the receiver is offline; otherwise it lost fix. - if (online_count < fixed_count || online_count < required) { - /* EVENT - * @description - * - * Configure the minimum required GPS count with SYS_HAS_NUM_GPS. - * Configure the failsafe action with COM_GPS_LOSS_ACT. - * - */ - reporter.healthFailure(nav_modes, health_component_t::gps, - events::ID("check_gps_redundancy_offline"), - log_level, - "GPS receiver offline: {1} of {2} online", - (uint8_t)online_count, expected); - - } else { - /* EVENT - * @description - * - * Configure the minimum required GPS count with SYS_HAS_NUM_GPS. - * Configure the failsafe action with COM_GPS_LOSS_ACT. - * - */ - reporter.healthFailure(nav_modes, health_component_t::gps, - events::ID("check_gps_redundancy_no_fix"), - log_level, - "GPS receiver lost 3D fix: {1} of {2} fixed", - (uint8_t)fixed_count, expected); + } else { + /* EVENT + * @description + * + * Configure the minimum required GPS count with SYS_HAS_NUM_GNSS. + * Configure the failsafe action with COM_GPS_LOSS_ACT. + * + */ + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_redundancy_no_fix"), + log_level, + "GPS receiver lost 3D fix: {1} of {2} fixed", + (uint8_t)fixed_count, expected); + } } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp index 4add9ba7dc..1b9a44027d 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2024 PX4 Development Team. All rights reserved. + * Copyright (c) 2026 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,7 +56,7 @@ private: hrt_abstime _divergence_since{0}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, - (ParamInt) _param_sys_has_num_gps, + (ParamInt) _param_sys_has_num_gnss, (ParamInt) _param_com_gps_loss_act, (ParamFloat) _param_gps0_offx, (ParamFloat) _param_gps0_offy, diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index 0c81296c87..1c1f18c97c 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -133,10 +133,11 @@ parameters: default: 1 COM_GPS_LOSS_ACT: description: - short: GPS redundancy loss failsafe mode + short: GPS loss failsafe mode long: |- - Action the system takes when the number of active GPS receivers drops - below SYS_HAS_NUM_GPS during flight. + Action the system takes when a GNSS failure is detected during flight. + Triggers when the active GNSS count drops below SYS_HAS_NUM_GNSS or when + two receivers report positions inconsistent with their reported accuracy. type: enum values: 0: Warning diff --git a/src/modules/commander/failsafe/failsafe.cpp b/src/modules/commander/failsafe/failsafe.cpp index eab1c2f888..6c0b525050 100644 --- a/src/modules/commander/failsafe/failsafe.cpp +++ b/src/modules/commander/failsafe/failsafe.cpp @@ -664,7 +664,7 @@ void Failsafe::checkStateAndMode(const hrt_abstime &time_us, const State &state, CHECK_FAILSAFE(status_flags, fd_imbalanced_prop, fromImbalancedPropActParam(_param_com_imb_prop_act.get())); CHECK_FAILSAFE(status_flags, fd_motor_failure, fromActuatorFailureActParam(_param_com_actuator_failure_act.get())); - CHECK_FAILSAFE(status_flags, gps_redundancy_lost, fromGpsRedundancyActParam(_param_com_gps_loss_act.get())); + CHECK_FAILSAFE(status_flags, gnss_lost, fromGpsRedundancyActParam(_param_com_gps_loss_act.get()));