mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-11-22 22:50:49 +08:00
* 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 pass493c86ce58set 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 requirementsa23b25aa3bfailed 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>
42 lines
3.0 KiB
Plaintext
42 lines
3.0 KiB
Plaintext
# Selected GNSS solution
|
|
#
|
|
# Published by the sensors module for every sample of the selected receiver, usable or not, with the selection state and the check result for that sample.
|
|
# Heading is on vehicle_gnss_heading.
|
|
|
|
uint64 timestamp # [us] Time since system start
|
|
uint64 timestamp_sample # [us] Measurement time, delay corrected. Use this rather than receiver.timestamp_sample, which holds the same value
|
|
|
|
SensorGnss receiver # The selected receiver's sensor_gnss sample with the corrected timestamp_sample
|
|
|
|
float32[3] antenna_offset # [m] [@frame FRD] Antenna position of the selected receiver
|
|
|
|
# Selection state
|
|
uint8 selected_instance # [-] sensor_gnss instance of the selected receiver
|
|
uint8 selection_count # [-] Increments when the selected receiver changes
|
|
uint8 selection_reason # [@enum SELECTION] Why this receiver is selected
|
|
uint8 SELECTION_PREFERRED = 0 # The preferred receiver
|
|
uint8 SELECTION_ONLY = 1 # No other receiver has published
|
|
uint8 SELECTION_RANKED = 2 # No receiver is preferred, and no other one ranks higher
|
|
uint8 SELECTION_TIMEOUT = 3 # The previously selected or the preferred receiver stopped publishing
|
|
uint8 SELECTION_UNHEALTHY = 4 # The previously selected or the preferred receiver failed its checks or delivered samples late, continuously or often
|
|
uint8 SELECTION_STRICT = 5 # Passes the strict checks, which the previously selected receiver didn't
|
|
uint8 SELECTION_RTK_FIXED = 6 # Passes the strict checks with an RTK fixed solution, and the previously selected receiver ranked lower
|
|
|
|
# Check result for this sample. The strict checks are the GNSS_REQ_* thresholds, satellite count, PDOP and drift at rest on the ground; the relaxed checks are fixed, looser limits on fix type, accuracy, spoofing and jamming.
|
|
bool usable # Sample may be used: the checks that decide it have passed without a failure for long enough
|
|
bool strict # The strict checks decide usable: they haven't passed since the vehicle was disarmed on the ground, or they pass. Otherwise the relaxed checks decide
|
|
uint16 failed_strict_checks # [@enum CHECK] Enabled checks this sample fails at the strict thresholds, evaluated on every sample
|
|
uint16 failed_relaxed_checks # [@enum CHECK] Enabled checks this sample fails at the relaxed thresholds, evaluated on every sample
|
|
uint16 CHECK_NSATS = 1 # Too few satellites
|
|
uint16 CHECK_PDOP = 2 # Position dilution of precision too high
|
|
uint16 CHECK_EPH = 4 # Horizontal position accuracy too low
|
|
uint16 CHECK_EPV = 8 # Vertical position accuracy too low
|
|
uint16 CHECK_SACC = 16 # Speed accuracy too low
|
|
uint16 CHECK_HDRIFT = 32 # Horizontal position drift at rest too high
|
|
uint16 CHECK_VDRIFT = 64 # Vertical position drift at rest too high
|
|
uint16 CHECK_HSPEED = 128 # Horizontal speed at rest too high
|
|
uint16 CHECK_VSPEED = 256 # Vertical speed at rest too high
|
|
uint16 CHECK_SPOOFED = 512 # Receiver reports spoofing
|
|
uint16 CHECK_FIX = 1024 # Fix type too low
|
|
uint16 CHECK_JAMMED = 2048 # Receiver reports jamming
|