Files
PX4-Autopilot/msg/SensorsStatusGnss.msg
T
4964fa5995 feat(sensors/gnss): select the GNSS receiver on failures and the strict checks (#28798)
* feat(sensors/gnss): select the GNSS receiver on failures and reported accuracy

The interim selection took the receiver with the best fix type and the most satellites, and the preferred one whenever it had a 3D fix. Fix type and satellite count compare poorly across receivers, and nothing held a switch, so a one-sample fix blip flipped the output and back.

A preferred receiver (SENS_GNSS_PRIME, or with -1 the moving base of a moving base pair) is used until it fails: no usable, on-time sample for 2 s, or clearly less available than the other receiver. The selection returns to it once usable again, after 10 s while armed. Without one, it moves after 5 s to a receiver reporting at most half the eph and no worse epv, floored at 5 cm. SENS_GNSS_PRIME defaults to -1. vehicle_gnss carries the reason for the last switch, sensors_status_gnss each receiver's availability. GpsBlending becomes GnssSelector.

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

* docs(sensors/gnss): rewrite the SENS_GNSS_PRIME description

The description walked through the selection's internal timings and thresholds, and claimed that every switch resets the EKF2 position, which it doesn't while another source constrains the position. The availability comment left out that late samples count as unavailable.

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

* fix(sensors/gnss): update the selection while no receiver publishes

The selector and sensors_status_gnss only updated when a sample arrived. When every receiver stopped, the last status kept reporting the receiver healthy and available, the receiver never timed out, and on its return the gap counted as available time.

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

* refactor(sensors/gnss): keep the GNSS samples in the module, not the selector

The selector stored full samples and antenna offsets only to hand them back, a leftover from blending; it reads just each sample's timestamp, eph and epv. The module now keeps every receiver's latest sample and publishes the selected one, which is the sample its checks ran on, and the heading path reads the receiver state from it instead of copying every receiver out of uORB. A timed-out receiver is reset in one place, without the writes to the fix type and satellite count that only blending ranked on.

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

* feat(sensors/gnss): rank receivers on the requirements and RTK fix, without returning to a failed one

The eph ratio switched receivers on reported accuracy alone, and the 10 s return to the primary went back to a receiver likely to fail again in the same flight, at the cost of another EKF2 position reset. While disarmed, the selection fell back to the other receiver, so a vehicle could take off on it with a failing primary.

While disarmed the primary is now selected whenever it publishes, so commander reports its failed checks before arming. While armed, a receiver left because it failed is selected again only when the selected one fails, until disarmed. Without a primary, receivers rank by meeting GNSS_REQ_EPH/EPV/SACC/FIX, which GnssChecks now also evaluates in flight, then by an RTK fixed solution; the selection moves up after 10 s armed or 2 s disarmed.

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

* feat(gnss_checks): always run strict checks to have the information for the selection algorithm, switch to simplified checks in-air when the strict checks fail

* feat(GnssSelector): rework selection algorithm

* fix(lib/gnss): pass the checks again and relax them only after a strict pass

493c86ce58 set the fail time on every sample that hadn't passed yet, including samples still inside the required pass duration, so the duration restarted on every sample and no receiver ever passed: EKF2 never fused GNSS. It also relaxed the checks only while flying, so a vehicle armed on the ground lost GNSS for GNSS_REQ_TIME on a single strict failure, and a receiver that first published in flight got the relaxed checks without ever passing the strict ones.

The relaxed checks apply again once the strict ones passed, until disarmed on the ground. The strict checks still run on every sample, and meetsRequirements() is their result for that sample, without a pass duration: the selection holds its own, and a duration here would turn one failed sample into GNSS_REQ_TIME of not meeting the requirements. Once relaxed, only the relaxed checks are reported as failed.

* fix(sensors/gnss): restore failover and switch holds, leave a primary that misses the requirements

a23b25aa3b failed over to the preferred receiver where it meant the higher ranked one, which selects instance -1 without a preferred receiver, read the availability of instance -1, stayed on a failed primary whose late samples still met the requirements, and replaced a failed receiver only with one that met the requirements or was far more available. Without the hold, two receivers whose RTK fixed solutions came and went switched on every change, each switch an EKF2 position reset.

Its design stays: the primary is kept while it meets the requirements and left when it hasn't met them through the hold while the other receiver has. Receivers rank by being usable, then meeting the requirements, then, without a primary, an RTK fixed solution, and the primary ranks above an equal receiver that meets them. So it is taken back once it meets them through the hold, unless it failed, and an RTK fixed solution never outranks it, as a moving-base rover reports one while being the worse position source. A silent or failed receiver is replaced at once by the best publishing or usable one, a rank switch waits 10 s armed and 2 s disarmed, and while disarmed the primary is selected only while it publishes, so that a vehicle without it still has a position and the pre-arm check reports the missing primary. getSelectionReason() returns a value set in update().

* feat(sensors/gnss): double the switch hold with every rank switch in a flight

The hold rides through changes shorter than 10 s, but a receiver that goes in and out of the requirements for longer still costs two EKF2 position resets per cycle, for as long as it keeps doing so: under sustained interference, switching is worse than staying on one receiver and letting the failsafes act. Doubling the hold with every rank switch while armed, up to 160 s, bounds the switches in a flight while a single recovery, such as the corrections coming back, still returns after 20 s. Failovers keep switching at once.

* fix(sensors/gnss): report a receiver as healthy only while the selection can use it

sensors_status_gnss.healthy took only the checks into account, while the selection also requires samples at the receiver's usual rate. A receiver whose rate collapsed showed as healthy next to its falling availability and the selection leaving it, and commander's redundancy check counted it.

* docs(releases): note the SENS_GNSS_PRIME default change in the upgrade guide

Setups that never set SENS_GNSS_PRIME move from preferring the main serial receiver to Auto without migration.

* fix(sensors/gnss): learn a receiver's lower update rate instead of failing it for good

The late threshold followed the shortest interval a receiver ever reached, so a receiver that slowed to below a third of its rate had every sample late from then on and stayed out of the selection, although receivers slow down for reasons that don't make their samples worse, such as processing RTCM corrections. With the no-return rule, a primary that slowed down was left for the rest of the flight.

Samples are now late against the receiver's recent interval, which follows a lower rate within a sample: only the first sample at the lower rate is late, so a single gap still counts. Below about 1 Hz the first two intervals at the lower rate still add up to the 2 s that fail a receiver.

* feat(lib/gnss): meetRequirements() requires to pass the strict checks for min_health_time_us. Report checks as strict for as long as the strict checks pass.

* refactor(lib/gnss/test): remove hardcoded min_health_time_us from setParams(GnssChecks::Params params) method

* feat(sensors/gnss):  restructure receiver selection

- don't select a previously failing receiver as better if the current is still usable and hasn't failed
- priority of availability over higher rank (and preferred), but still choose the highest rank (or preferred) in case multiple have high availability

* fix(sensors/gnss): prefer the primary receiver only while it meets the requirements

Once both receivers missed the requirements, the selection moved back to the primary after the hold: an EKF2 position reset with no better receiver to show for it. The primary is kept while it meets the requirements; when neither does, nothing favours it.

* docs(lib/gnss): describe when meetsRequirements() restarts its pass duration

The comments still said that meetsRequirements() holds no pass duration and that strict() is true only before the first pass. A sample that fails only the strict checks deliberately doesn't restart the duration once the relaxed checks apply, so that one bad sample doesn't drop the receiver's rank for GNSS_REQ_TIME and switch it away after the selection's hold.

* refactor(sensors/gnss): name the two GNSS check sets strict and relaxed

The same two sets of checks went by strict, initial fix, requirements, simplified, pre-flight and in-flight, and failed_checks reported whichever set decided usable, so its meaning changed with the vehicle state. Each result now names its set: vehicle_gnss and sensors_status_gnss report failed_strict_checks and failed_relaxed_checks for every sample, and strict says which of them decides usable. In code the sets are runStrictChecks() and runRelaxedChecks(), the held strict result is passedStrict(), and the selector ranks on RANK_STRICT and reports SELECTION_STRICT. No behaviour changes.

* refactor(ekf2): drop EKF2_REQ_SACC and the GNSS restart wait, move the heading wait to EKF2_GPS_YAW_RST

EKF2 kept its own copies of two GNSS check thresholds after the checks moved to the sensors module. Whether a receiver's sample is good is the sensors module's call and reaches EKF2 on vehicle_gnss; EKF2 keeps only waits about its own estimate.

Trusting GNSS vertical velocity while the accelerometer clips, and feeding GNSS velocity to the yaw estimator, now use the sample's strict speed accuracy check (GNSS_REQ_SACC) from vehicle_gnss.failed_strict_checks, evaluated on every sample, also in flight. vehicle_gnss.usable alone would not do: in flight the relaxed checks decide it, which only bound the speed accuracy at 10 m/s.

The wait before GNSS fusion restarts after EKF2 stops it only reproduced the requalification EKF2 forced by resetting the checks when they lived in EKF2. GNSS_REQ_TIME now does that in the sensors module, and a restart after a stop EKF2 decided on its own is already gated on GNSS agreeing with the estimate. The replay test output is unchanged.

The wait before a yaw reset to the GNSS heading after a heading fusion failure or a spoofing or jamming report is EKF2's own and moves to EKF2_GPS_YAW_RST. Saved EKF2_REQ_SACC and EKF2_REQ_GPS_H values migrate to GNSS_REQ_SACC and GNSS_REQ_TIME.

* fix(sensors/gnss): checking for better availability now works both ways

* test(sensors/gnss): add unit test for availability taking precedence over rank. Assisted by Codex 6.1 Sol

* refactor(lib/gnss): name the relaxed-checks gate _initial_strict_checks_passed

"since disarm" read as the moment of disarming, while the flag is cleared on every sample the vehicle sits disarmed on the ground.

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

* refactor(ekf2): keep starting_conditions_passing in the GNSS fusion control

The other aid source controls decide a start on starting_conditions_passing; keep that structure even though it now equals the continuing conditions.

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

* fix(sensors/gnss): keep the strict rank through a failure shorter than the fail time

In flight a sample that fails the relaxed checks restarts GNSS_REQ_TIME for passedStrict(), and GNSS_REQ_TIME (10 s) is as long as the armed switch hold. A one-second failure on the primary receiver, such as a spoofing flag, therefore switched to the other receiver after the hold and back once the doubled hold passed: two EKF2 position resets for a blip.

A receiver now gains the strict rank only once its strict checks held for GNSS_REQ_TIME, and keeps it while its samples pass them until it fails (FAIL_TIME_US without a usable sample). occasionalFailureDoesNotSwitch kept passed_strict true through the failures, which GnssChecks never reports, so it missed this.

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

* fix(sensors/gnss): report a receiver unhealthy only once the selection fails it

healthy followed isUsable(), which goes false for a single sample arriving 300 ms late. The commander's GNSS redundancy check counts healthy receivers without hysteresis, so with SYS_HAS_NUM_GNSS set one late sample triggered the GNSS loss failsafe, and with two receivers it reported "GPS n unhealthy". healthy now means usable within FAIL_TIME_US (2 s), when the selection fails over; that still covers a receiver whose rate collapsed below about 1 Hz.

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

* fix(sensors/gnss): count receiver availability from arming

While disarmed the strict checks keep a receiver unusable for GNSS_REQ_TIME after every failure, and the availability kept counting that time. Arming within a few seconds of the primary receiver requalifying after a ground outage left the other receiver more available by over 0.2 for the whole armed hold, so the selection left a healthy primary 10 s after arming and returned 20 s later. Availability is meant to catch intermittent failures in flight, so it now starts over at arming.

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

* fix(sensors/gnss): don't count a receiver that hadn't qualified at arming as failed in flight

While disarmed the primary receiver is selected whenever it publishes, even before it passes its checks. Armed in that state, the selection failed over at once and marked the primary failed for the flight, so it was never used again even after it passed its checks, while a primary that was silent at arming is taken once it qualifies. A receiver is now marked failed only if it was working at some point since arming.

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

* docs(sensors/gnss): correct the selection comments and parameter descriptions

The AVAILABILITY_MARGIN comment described a half margin the code never applied, SELECTION_RTK_FIXED claimed both receivers pass the strict checks, and SENS_GNSS_PRIME said the primary returns after 10 s, while the doubled hold makes the first return take 20 s, and left out switches on availability. GNSS_REQ_SACC now says it also gates the EKF-GSF yaw estimator and the GNSS vertical velocity while the accelerometer clips, which turning off the speed accuracy check disables. Drops _time_usable_since_us, which nothing read.

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

* docs(releases): list the GNSS check parameters that moved from EKF2

EKF2_GPS_CHECK, EKF2_REQ_* and EKF2_REQ_GPS_H are migrated to GNSS_CHECK, GNSS_REQ_* and GNSS_REQ_TIME, but the upgrade guide had no entry for them, and the GNSS rename entry still said the EKF2 parameters were unchanged.

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

---------

Co-authored-by: Jacob Dahl <dahl.jakejacob@gmail.com>
Co-authored-by: thomas <thomas@rigi.tech>
2026-10-19 20:18:40 -06:00

23 lines
1.6 KiB
Plaintext

# Per-receiver GNSS health and check diagnostics
#
# Published by the sensors module. Describes the latest sample of each receiver: to gate on a sample, use vehicle_gnss.usable.
uint64 timestamp # [us] Time since system start
uint32 device_id_selected # [-] Receiver that vehicle_gnss carries
uint8 MAX_RECEIVERS = 4 # Length of the per-receiver arrays
uint32[4] device_ids # [-] One entry per receiver, 0 where unused
int8[4] order # [-] Fixed position of the receiver: 0 for the preferred one, otherwise for the first to publish, then in order of first publication. Changes only with the preference, -1 until the receiver publishes
bool[4] healthy # Usable within the last 2 s: passed its checks for long enough and published without gaps. The selection fails over from a receiver that isn't
float32[4] availability # [-] Fraction of recent time the receiver passed its checks and published without gaps, 0 to 1
uint16[4] failed_strict_checks # [-] Enabled checks the latest sample fails at the strict thresholds, VehicleGnss CHECK_* bits
uint16[4] failed_relaxed_checks # [-] Enabled checks the latest sample fails at the relaxed thresholds, VehicleGnss CHECK_* bits
bool[4] strict # The strict checks decide whether its samples are usable, as VehicleGnss strict
float32[4] drift_rate_horizontal # [m/s] Horizontal position drift rate at rest
float32[4] drift_rate_vertical # [m/s] Vertical position drift rate at rest
float32[4] speed_horizontal_filtered # [m/s] Filtered horizontal speed at rest
# TOPICS sensors_status_gnss