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

refactor(gpsRedundancyCheck): address code review feedback

Browse Source
This commit is contained in:
gguidone 2026-04-10 14:56:57 +02:00
parent a98b6d20c1
commit 3365e34e66
7 changed files with 60 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/FailsafeFlags.msg
View File

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

9
src/lib/systemlib/system_params.yaml
View File

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

2
src/modules/commander/HealthAndArmingChecks/GpsRedundancyCheckTest.cpp
View File

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

94
src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.cpp
View File

@ -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
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t, uint8_t>(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
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t, uint8_t>(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
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GPS</param>.
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t, uint8_t>(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
* <profile name="dev">
* Configure the minimum required GPS count with <param>SYS_HAS_NUM_GNSS</param>.
* Configure the failsafe action with <param>COM_GPS_LOSS_ACT</param>.
* </profile>
*/
reporter.healthFailure<uint8_t, uint8_t>(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);
}
}
}

4
src/modules/commander/HealthAndArmingChecks/checks/gpsRedundancyCheck.hpp
View File

@ -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<px4::params::SYS_HAS_NUM_GPS>) _param_sys_has_num_gps,
(ParamInt<px4::params::SYS_HAS_NUM_GNSS>) _param_sys_has_num_gnss,
(ParamInt<px4::params::COM_GPS_LOSS_ACT>) _param_com_gps_loss_act,
(ParamFloat<px4::params::SENS_GPS0_OFFX>) _param_gps0_offx,
(ParamFloat<px4::params::SENS_GPS0_OFFY>) _param_gps0_offy,

7
src/modules/commander/commander_params.yaml
View File

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

2
src/modules/commander/failsafe/failsafe.cpp
View File

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