From d04e5623f6947d4a8144bc3ddcd8fdf77cfc87b2 Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Thu, 19 Nov 2026 09:28:20 -0900 Subject: [PATCH] feat(gnss): compare receivers in sensors, limit divergence in commander (#29604) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * feat(sensors/gnss): report how far each receiver is from the selected one Commander compared the receivers itself, reading the sensors module's antenna offsets and comparing samples taken at different times. The sensors module has the receiver-to-slot matching and the corrected sample times, so it now publishes each receiver's disagreement with the selected one on sensors_status_gnss, horizontally and vertically. The other receiver is moved along its velocity to the selected sample's time. The antenna baseline is rotated with the attitude, so swapped or misplaced antennas show once the primary estimator's heading is aligned. Before that only the distances are compared, since the yaw is arbitrary. Co-authored-by: Andrea Bernasconi Assisted-by: Claude:claude-opus-5-5 * feat(commander): limit the GNSS divergence with COM_GNSS_H_MAX and COM_GNSS_V_MAX The divergence gate came from the receivers' reported accuracy floored with EKF2_GPS_P_NOISE, so it was hard to predict and read another module's parameter. Commander now takes the disagreement from sensors_status_gnss and applies fixed horizontal and vertical limits. Three times the receivers' combined reported accuracy still widens them, so standalone receivers don't warn on default parameters. Before arming a divergence is reported at once and clears after 2 s of agreement. In flight it has to persist for 2 s. Co-authored-by: Andrea Bernasconi Assisted-by: Claude:claude-opus-5-5 * fix(commander): keep the GNSS divergence limits fixed Widening the limits to three times the receivers' combined reported accuracy made them floors, not limits. With the default GNSS_REQ_EPH and GNSS_REQ_EPV the gate opened to 12.7 m horizontally and 21 m vertically before arming, and to about 210 m in flight once a receiver fell back to the relaxed checks. The limits now apply as set, with defaults of 5 m and 10 m so standalone receivers don't warn. Assisted-by: Claude:claude-opus-5-5 * fix(commander): don't take an unknown GNSS disagreement for agreement The sensors module leaves the disagreement unknown when the two latest samples are more than 0.5 s apart, while both receivers stay healthy. Commander treated that as nothing to compare, so a divergence before arming cleared for that check and an arm request could pass, and in flight the 2 s timer restarted. It happens on any gap over 0.5 s from a healthy receiver, and on 2 Hz receivers whose latencies differ. An unknown disagreement now keeps the state and its timer. A divergence still clears at once when there is no healthy pair to compare. Assisted-by: Claude:claude-opus-5-5 * docs(commander): swapped GNSS antennas hide behind a moving-base heading The moving-base heading takes its offset from the same antenna positions. With swapped antennas the heading turns by 180°, and the expected baseline turns with it, so the divergence check sees agreement. Assisted-by: Claude:claude-opus-5-5 * docs(commander): reword the moving-base heading caveat The sentence didn't say plainly that a moving-base heading from the same receivers stops the check from detecting swapped antennas. Assisted-by: Claude:claude-opus-5-5 * docs(commander): fold the moving-base caveat into the swapped-antenna conditions The caveat read as a repeat of the alignment sentence. It is one more condition for detecting swapped antennas, so it now sits with the others. Assisted-by: Claude:claude-opus-5-5 * refactor(gnss): rotate the antenna baseline with the attitude as it is The heading gate kept a limit below twice the antenna baseline from reporting a divergence before the heading aligned. With a mag the heading aligns before the receivers turn healthy. A heading from the same two receivers can't reveal swapped antennas anyway. The gate also never opened with an external INS, since only EKF2 publishes estimator_status_flags. Dropping it saves 168 B of flash on fmu-v2. The attitude age limit goes with it, since the last published attitude is a better guess than level and facing north. Assisted-by: Claude:claude-opus-5-5 --------- Co-authored-by: Andrea Bernasconi --- docs/en/config/safety.md | 8 +- msg/SensorsStatusGnss.msg | 2 + .../checks/gnssRedundancyCheck.cpp | 99 +++-- .../checks/gnssRedundancyCheck.hpp | 12 +- .../gnssRedundancyChecksTest.cpp | 364 ++++++++++++------ src/modules/commander/commander_params.yaml | 37 +- .../vehicle_gps_position/CMakeLists.txt | 2 + .../GnssInconsistency.hpp | 107 +++++ .../GnssInconsistencyTest.cpp | 175 +++++++++ .../VehicleGPSPosition.cpp | 41 +- .../VehicleGPSPosition.hpp | 7 + 11 files changed, 686 insertions(+), 168 deletions(-) create mode 100644 src/modules/sensors/vehicle_gps_position/GnssInconsistency.hpp create mode 100644 src/modules/sensors/vehicle_gps_position/GnssInconsistencyTest.cpp diff --git a/docs/en/config/safety.md b/docs/en/config/safety.md index 3487d4a3023..c7cc690248a 100644 --- a/docs/en/config/safety.md +++ b/docs/en/config/safety.md @@ -307,8 +307,10 @@ Note that if there is no horizontal aiding source anymore, the position estimate Triggers on either of: - **Count drop**: receivers with a 3D fix drop below [SYS_HAS_NUM_GNSS](#SYS_HAS_NUM_GNSS). No failsafe action when `SYS_HAS_NUM_GNSS=0` (default). -- **Position divergence**: two receivers disagree beyond their expected separation (configured via [SENS_GNSS0_OFFX/Y](../advanced_config/parameter_reference.md#SENS_GNSS0_OFFX), [SENS_GNSS1_OFFX/Y](../advanced_config/parameter_reference.md#SENS_GNSS1_OFFX)) plus reported accuracy. Only triggers a failsafe action if `SYS_HAS_NUM_GNSS=2`. - Receivers might report an optimistic accuracy, so the reported accuracy of each receiver is floored with [EKF2_GPS_P_NOISE](../advanced_config/parameter_reference.md#EKF2_GPS_P_NOISE). +- **Position divergence**: a receiver disagrees with the selected one by more than [COM_GNSS_H_MAX](#COM_GNSS_H_MAX) horizontally or [COM_GNSS_V_MAX](#COM_GNSS_V_MAX) vertically, after their antenna positions ([SENS_GNSS0_OFFX/Y/Z](../advanced_config/parameter_reference.md#SENS_GNSS0_OFFX), [SENS_GNSS1_OFFX/Y/Z](../advanced_config/parameter_reference.md#SENS_GNSS1_OFFX)). Only triggers a failsafe action if `SYS_HAS_NUM_GNSS=2`. + Swapped antennas are detected with a limit below twice the antenna separation and a heading that doesn't come from these two receivers. + Until the heading is aligned, a limit that low also reports a divergence. + Before arming a divergence is reported at once and clears after 2 s of agreement. In flight it has to persist for 2 s. At least a warning is emitted, additional failsafe actions can be configured using [COM_GNSSLOSS_ACT](#COM_GNSSLOSS_ACT). Loss of a single GPS when none are required is handled by other GPS health checks. @@ -323,6 +325,8 @@ Once `Return` or `Land` has started, it continues even if the GNSS failure clear | ----------------------------------------------------------------------------------------------------------- | ----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | | [SYS_HAS_NUM_GNSS](../advanced_config/parameter_reference.md#SYS_HAS_NUM_GNSS) | Number of usable GNSS receivers required. If two are required then they also need to be consistent. | | [COM_GNSSLOSS_ACT](../advanced_config/parameter_reference.md#COM_GNSSLOSS_ACT) | Failsafe action when a GNSS failure is detected. `0`: Warning only (default), `1`: Return, `2`: Land, `3`: Terminate, `4`: Warning, block arming. Values above Warning also block arming. | +| [COM_GNSS_H_MAX](../advanced_config/parameter_reference.md#COM_GNSS_H_MAX) | Horizontal disagreement between two receivers above which they diverge (default 5 m). | +| [COM_GNSS_V_MAX](../advanced_config/parameter_reference.md#COM_GNSS_V_MAX) | Vertical disagreement between two receivers above which they diverge (default 10 m). | ## Offboard Loss Failsafe diff --git a/msg/SensorsStatusGnss.msg b/msg/SensorsStatusGnss.msg index 40a134608e1..a2d66d81c76 100644 --- a/msg/SensorsStatusGnss.msg +++ b/msg/SensorsStatusGnss.msg @@ -12,6 +12,8 @@ uint32[4] device_ids # [-] One entry per receiver, 0 where unuse 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 +float32[4] inconsistency_horizontal # [m] [@invalid NaN] Horizontal distance from where the selected receiver and the antenna positions put the receiver, at the time of the selected sample. 0 for the selected receiver +float32[4] inconsistency_vertical # [m] [@invalid NaN] Vertical distance from where the selected receiver and the antenna positions put the receiver, at the time of the selected sample. 0 for the selected receiver 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 diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp index 89eeecef2f1..7f892722087 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.cpp @@ -32,24 +32,18 @@ ****************************************************************************/ #include "gnssRedundancyCheck.hpp" -#include + +#include using namespace matrix; using namespace time_literals; -GnssRedundancyChecks::GnssRedundancyChecks() - : _param_ekf2_gps_p_noise_handle(param_find("EKF2_GPS_P_NOISE")) -{ - _divergence_hysteresis.set_hysteresis_time_from(false, 2_s); - _divergence_hysteresis.set_hysteresis_time_from(true, 2_s); -} - void GnssRedundancyChecks::checkAndReport(const Context &context, Report &reporter) { bool gps_online[GPS_MAX_INSTANCES] {}; bool gnss_healthy[GPS_MAX_INSTANCES] {}; uint8_t healthy_count = 0; - sensor_gnss_s healthy_gnss[GPS_MAX_INSTANCES] {}; + int selected = -1; sensors_status_gnss_s status{}; const bool status_valid = _sensors_status_gnss_sub.copy(&status) && (hrt_elapsed_time(&status.timestamp) < 1_s); @@ -65,7 +59,11 @@ void GnssRedundancyChecks::checkAndReport(const Context &context, Report &report // The sensors module indexes its status by the same sensor_gnss instance if (status_valid && (status.device_ids[i] == gnss.device_id) && status.healthy[i]) { gnss_healthy[i] = true; - healthy_gnss[healthy_count++] = gnss; + healthy_count++; + + if (gnss.device_id == status.device_id_selected) { + selected = i; + } } } } @@ -75,37 +73,53 @@ void GnssRedundancyChecks::checkAndReport(const Context &context, Report &report _peak_healthy_count = healthy_count; } - // Position divergence check: flag if two healthy receivers disagree beyond their - // combined uncertainty. Gate = 3 * RSS(eph), centered on the expected lever-arm separation. - float divergence_m = 0.f; + // Position divergence check: flag a healthy receiver that disagrees with the selected one, after their antenna + // positions, by more than the limits + float divergence_h_m = 0.f; + float divergence_v_m = 0.f; + bool diverging = false; + bool paired = false; + bool unknown = false; - if (healthy_count >= 2) { - float north, east; - get_vector_to_next_waypoint(healthy_gnss[0].latitude, healthy_gnss[0].longitude, - healthy_gnss[1].latitude, healthy_gnss[1].longitude, - &north, &east); - const float separation_m = Vector2f(north, east).length(); - - const Vector2f offset0(_param_sens_gnss0_offx.get(), _param_sens_gnss0_offy.get()); - const Vector2f offset1(_param_sens_gnss1_offx.get(), _param_sens_gnss1_offy.get()); - const float expected_d = (offset0 - offset1).length(); - divergence_m = fabsf(separation_m - expected_d); - // Use quadrature sum for standard deviation of the difference taking the firmware dependent eph as standard deviation - // and a heuristic factor of 3 because then it's unlikely just noise. - // Receivers might report an optimistic eph, so floor it with the minimum GNSS position noise. - // EKF2 might not be part of the build, in that case fall back to the EKF2_GPS_P_NOISE default. - float eph_min = 0.5f; - - if (_param_ekf2_gps_p_noise_handle != PARAM_INVALID) { - param_get(_param_ekf2_gps_p_noise_handle, &eph_min); + for (int i = 0; i < GPS_MAX_INSTANCES; i++) { + if ((selected < 0) || (i == selected) || !gnss_healthy[i]) { + continue; } - const float divergence_gate_m = 3.f * Vector2f(math::max(healthy_gnss[0].eph, eph_min), - math::max(healthy_gnss[1].eph, eph_min)).length(); - _divergence_hysteresis.set_state_and_update(divergence_m > divergence_gate_m, hrt_absolute_time()); + paired = true; + const float horizontal = status.inconsistency_horizontal[i]; + const float vertical = status.inconsistency_vertical[i]; + + // The sensors module doesn't compare samples too far apart in time. That says nothing about agreement. + if (!PX4_ISFINITE(horizontal)) { + unknown = true; + continue; + } + + diverging |= (horizontal > _param_com_gnss_h_max.get()); + divergence_h_m = math::max(divergence_h_m, horizontal); + + if (PX4_ISFINITE(vertical)) { + diverging |= (vertical > _param_com_gnss_v_max.get()); + divergence_v_m = math::max(divergence_v_m, vertical); + } + } + + const hrt_abstime now = hrt_absolute_time(); + + // Before arming, a disagreement is reported at once. In flight it has to persist for 2 s, so short glitches pass. + _divergence_hysteresis.set_hysteresis_time_from(false, context.isArmed() ? 2_s : 0_s); + // A disagreement clears after 2 s of continuous agreement, or at once without a healthy pair to compare + _divergence_hysteresis.set_hysteresis_time_from(true, paired ? 2_s : 0_s); + + if (unknown && !diverging) { + // Keep the state and any running timer until that receiver can be compared again + _divergence_hysteresis.update(now); } else { - _divergence_hysteresis.set_state_and_update(false, hrt_absolute_time()); + _divergence_hysteresis.set_state_and_update(diverging, now); + _divergence_h_m = divergence_h_m; + _divergence_v_m = divergence_v_m; } const bool below_required = (_param_sys_has_num_gnss.get() > 0) && (healthy_count < _param_sys_has_num_gnss.get()); @@ -154,17 +168,18 @@ void GnssRedundancyChecks::checkAndReport(const Context &context, Report &report if (_divergence_hysteresis.get_state()) { /* EVENT * @description - * Two GNSS receivers report positions that are inconsistent with their reported accuracy. + * Two GNSS receivers report positions that disagree by more than their antenna positions explain. * * + * Configure the limits with COM_GNSS_H_MAX and COM_GNSS_V_MAX. * Configure the failsafe action with COM_GNSSLOSS_ACT. * The failsafe action is only triggered when SYS_HAS_NUM_GNSS is set to 2. * */ - reporter.healthFailure(nav_modes, health_component_t::gps, - events::ID("check_gps_position_divergence"), - log_level, - "GPS receivers disagree by {1:.1}m", - (double)divergence_m); + reporter.healthFailure(nav_modes, health_component_t::gps, + events::ID("check_gps_position_divergence"), + log_level, + "GPS receivers disagree by {1:.1}m horizontally, {2:.1}m vertically", + _divergence_h_m, _divergence_v_m); } } diff --git a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp index afbe95b0744..b5b2ad0c8ec 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/gnssRedundancyCheck.hpp @@ -43,7 +43,7 @@ class GnssRedundancyChecks : public HealthAndArmingCheckBase { public: - GnssRedundancyChecks(); + GnssRedundancyChecks() = default; ~GnssRedundancyChecks() = default; void checkAndReport(const Context &context, Report &reporter) override; @@ -57,15 +57,13 @@ private: uint8_t _peak_healthy_count{0}; systemlib::Hysteresis _divergence_hysteresis; - - const param_t _param_ekf2_gps_p_noise_handle; + float _divergence_h_m{0.f}; ///< disagreement of the last comparison, for the report + float _divergence_v_m{0.f}; DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, (ParamInt) _param_sys_has_num_gnss, (ParamInt) _param_com_gnssloss_act, - (ParamFloat) _param_sens_gnss0_offx, - (ParamFloat) _param_sens_gnss0_offy, - (ParamFloat) _param_sens_gnss1_offx, - (ParamFloat) _param_sens_gnss1_offy + (ParamFloat) _param_com_gnss_h_max, + (ParamFloat) _param_com_gnss_v_max ) }; diff --git a/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp b/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp index 07c9bfd192a..56b345ab7d6 100644 --- a/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp +++ b/src/modules/commander/HealthAndArmingChecks/gnssRedundancyChecksTest.cpp @@ -31,6 +31,7 @@ * ****************************************************************************/ + #include #include "checks/gnssRedundancyCheck.hpp" @@ -49,19 +50,15 @@ * @skip-file */ -// Base position (PX4 SITL default home). -static constexpr double BASE_LAT = 47.397742; -static constexpr double BASE_LON = 8.545594; +// The sensors module compares the receivers; these tests publish its result. GNSS0 (device ID 1) is the selected +// receiver, and the limits COM_GNSS_H_MAX and COM_GNSS_V_MAX default to 5 m and 10 m. +static constexpr float RTK_ACCURACY = 0.02f; +static constexpr float AGREEING = 0.3f; +static constexpr float WITHIN_LIMIT = 4.f; +static constexpr float DIVERGING = 12.f; -// With GPS0 at +35cm X and GPS1 at -35cm X, expected_d = 0.70m. -// The reported eph (RTK, 0.02m) is floored with EKF2_GPS_P_NOISE = 0.5m: -// gate = 3 * sqrt(0.5² + 0.5²) ≈ 2.12m. -// AGREEING_LAT puts GPS1 ~0.70m north of GPS0 so separation ≈ expected_d. -// DIVERGING_FAR_LAT puts GPS1 ~5.0m north, well outside the gate. -static constexpr double AGREEING_LAT = BASE_LAT + 6.3e-6; // ~0.70m north -static constexpr double DIVERGING_FAR_LAT = BASE_LAT + 4.5e-5; // ~5.0m north - -// Divergence has to be sustained for the 2s hysteresis time before it is reported. +// In flight a divergence has to be sustained for the 2 s hysteresis time before it is reported. Before arming it is +// reported at once and only clears after 2 s of continuous agreement. static constexpr useconds_t HYSTERESIS_WAIT_US = 2100000; class GnssRedundancyChecksTest : public ::testing::Test @@ -74,45 +71,61 @@ public: // Reset params that tests modify so state doesn't leak between tests. param_reset(param_find("SYS_HAS_NUM_GNSS")); param_reset(param_find("COM_GNSSLOSS_ACT")); - - // Set lever arms so expected_d = 0.70m, enabling the "too close" direction of divergence detection. - float v = 0.35f; param_set(param_find("SENS_GNSS0_OFFX"), &v); - v = -0.35f; param_set(param_find("SENS_GNSS1_OFFX"), &v); - - // Floor for the reported accuracy, the test geometry above assumes 0.5m. - v = 0.5f; param_set(param_find("EKF2_GPS_P_NOISE"), &v); + param_reset(param_find("COM_GNSS_H_MAX")); + param_reset(param_find("COM_GNSS_V_MAX")); // Claim uORB instances 0 and 1 before the check subscribes on first copy(). sensor_gnss_s empty{}; _gnss0_pub.publish(empty); _gnss1_pub.publish(empty); - } - sensor_gnss_s makeGnss(double lat, double lon, float eph = 0.02f, uint8_t fix_type = 6) - { - sensor_gnss_s gnss{}; - gnss.timestamp = hrt_absolute_time(); - gnss.device_id = 1; - gnss.latitude = lat; - gnss.longitude = lon; - gnss.eph = eph; - gnss.fix_type = fix_type; - return gnss; + _status.device_id_selected = 1; } // Publish a receiver and the sensors module's status for it, healthy with a 3D fix. Instances 0 and 1 get device IDs // 1 and 2. - void publishGnss(int instance, sensor_gnss_s gnss) + void publishGnss(int instance, float eph = RTK_ACCURACY, float epv = RTK_ACCURACY, uint8_t fix_type = 6) { + sensor_gnss_s gnss{}; + gnss.timestamp = hrt_absolute_time(); gnss.device_id = instance + 1; + gnss.eph = eph; + gnss.epv = epv; + gnss.fix_type = fix_type; (instance == 0 ? _gnss0_pub : _gnss1_pub).publish(gnss); _status.device_ids[instance] = gnss.device_id; - _status.healthy[instance] = (gnss.fix_type >= 3); + _status.healthy[instance] = (fix_type >= 3); _status.timestamp = hrt_absolute_time(); _status_pub.publish(_status); } + // Publish both receivers, GNSS1 the given distance from where GNSS0 and the antenna positions put it + void publishBoth(float horizontal, float vertical = 0.f, float accuracy = RTK_ACCURACY) + { + _status.inconsistency_horizontal[0] = 0.f; + _status.inconsistency_vertical[0] = 0.f; + _status.inconsistency_horizontal[1] = horizontal; + _status.inconsistency_vertical[1] = vertical; + publishGnss(0, accuracy, accuracy); + publishGnss(1, accuracy, accuracy); + } + + void setRequiredGnss(int32_t sys_has_num_gnss) + { + param_set(param_find("SYS_HAS_NUM_GNSS"), &sys_has_num_gnss); + } + + void setGnssLossAct(int32_t com_gnssloss_act) + { + param_set(param_find("COM_GNSSLOSS_ACT"), &com_gnssloss_act); + } + + void setFloatParam(const char *name, float value) + { + param_set(param_find(name), &value); + } + // Run the check and store results in _failsafe_flags, _health_warning_gps and _can_arm_all_modes. void runCheck(bool armed = false) { @@ -130,25 +143,15 @@ public: _can_arm_all_modes = reporter.armingCheckResults().can_arm == NavModes::All; } - void setGnssLossAct(int32_t com_gnssloss_act) + // The receivers report the given disagreement past the hysteresis time + void runSustained(float horizontal, float vertical = 0.f, bool armed = false, float accuracy = RTK_ACCURACY) { - param_set(param_find("COM_GNSSLOSS_ACT"), &com_gnssloss_act); - } - - // Two fixed receivers diverging well beyond the gate with SYS_HAS_NUM_GNSS = 2, held past the hysteresis time. - void runSustainedDivergence() - { - int required = 2; - param_set(param_find("SYS_HAS_NUM_GNSS"), &required); - - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(DIVERGING_FAR_LAT, BASE_LON)); - runCheck(); + publishBoth(horizontal, vertical, accuracy); + runCheck(armed); px4_usleep(HYSTERESIS_WAIT_US); - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(DIVERGING_FAR_LAT, BASE_LON)); - runCheck(); + publishBoth(horizontal, vertical, accuracy); + runCheck(armed); } uORB::PublicationMulti _gnss0_pub{ORB_ID(sensor_gnss)}; @@ -161,7 +164,7 @@ public: GnssRedundancyChecks _check; }; -// No GPS data → no flags. +// No GNSS data → no flags. TEST_F(GnssRedundancyChecksTest, NoGpsNoFlags) { runCheck(); @@ -169,102 +172,243 @@ TEST_F(GnssRedundancyChecksTest, NoGpsNoFlags) EXPECT_FALSE(_health_warning_gps); } -// One receiver fixed, SYS_HAS_NUM_GNSS not configured → no failsafe. +// One receiver, SYS_HAS_NUM_GNSS not configured → no failsafe. TEST_F(GnssRedundancyChecksTest, SingleGpsNoFailsafe) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); EXPECT_FALSE(_failsafe_flags.gnss_lost); } -// Two receivers at expected lever-arm separation → no divergence. -TEST_F(GnssRedundancyChecksTest, TwoGpsAgreeingNoFlags) +// Receivers that agree → no flags. +TEST_F(GnssRedundancyChecksTest, AgreeingNoFlags) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(AGREEING_LAT, BASE_LON)); - runCheck(); + setRequiredGnss(2); + runSustained(AGREEING, AGREEING); EXPECT_FALSE(_failsafe_flags.gnss_lost); EXPECT_FALSE(_health_warning_gps); } -// Two receivers at the same position while a 0.70m lever arm is configured → 0.70m deviation. -// Far outside the gate from the reported RTK accuracy (≈0.085m) but within the floored gate (≈2.12m), -// so it is tolerated even once the hysteresis time has elapsed. -TEST_F(GnssRedundancyChecksTest, TwoGpsTooCloseToleratedByAccuracyFloor) +// A disagreement far outside the reported RTK accuracy but within the limits is tolerated. +TEST_F(GnssRedundancyChecksTest, WithinLimitsTolerated) { - int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required); - - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(BASE_LAT, BASE_LON)); - runCheck(); - - px4_usleep(HYSTERESIS_WAIT_US); - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(BASE_LAT, BASE_LON)); - runCheck(); + setRequiredGnss(2); + runSustained(WITHIN_LIMIT, WITHIN_LIMIT); EXPECT_FALSE(_failsafe_flags.gnss_lost); EXPECT_FALSE(_health_warning_gps); } -// Two receivers too far apart → hysteresis timer starts but has not elapsed on first call. -TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarNotYetSustained) +// In flight, a divergence starts the hysteresis timer but isn't reported yet. +TEST_F(GnssRedundancyChecksTest, InFlightNotYetSustained) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(DIVERGING_FAR_LAT, BASE_LON)); - runCheck(); + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(true); EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); } -// Sustained divergence with SYS_HAS_NUM_GNSS = 2 → gnss_lost, but COM_GNSSLOSS_ACT = Warning does not block arming. -TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarSustainedSetsGnssLost) +// In flight, a sustained divergence with SYS_HAS_NUM_GNSS = 2 → gnss_lost, but COM_GNSSLOSS_ACT = Warning doesn't block +// arming. +TEST_F(GnssRedundancyChecksTest, InFlightSustainedSetsGnssLost) { - runSustainedDivergence(); + setRequiredGnss(2); + runSustained(DIVERGING, 0.f, true); EXPECT_TRUE(_failsafe_flags.gnss_lost); EXPECT_TRUE(_health_warning_gps); EXPECT_TRUE(_can_arm_all_modes); } -// Sustained divergence with COM_GNSSLOSS_ACT = Return → gnss_lost and arming blocked. -TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingFarReturnBlocksArming) +// Without SYS_HAS_NUM_GNSS = 2 a divergence is a warning only. +TEST_F(GnssRedundancyChecksTest, DivergenceWarnsWithoutRequiredReceivers) { setGnssLossAct(1); // Return - runSustainedDivergence(); + runSustained(DIVERGING, 0.f, true); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); +} + +// In flight, a divergence that recovers before the hysteresis time → no flag. +TEST_F(GnssRedundancyChecksTest, InFlightShortGlitchIgnored) +{ + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(true); + + publishBoth(AGREEING); + runCheck(true); + + px4_usleep(HYSTERESIS_WAIT_US); + publishBoth(AGREEING); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// Before arming, a divergence blocks arming at once. +TEST_F(GnssRedundancyChecksTest, BeforeArmingImmediate) +{ + setRequiredGnss(2); + setGnssLossAct(4); // Warning, block arming + publishBoth(DIVERGING); + runCheck(); EXPECT_TRUE(_failsafe_flags.gnss_lost); EXPECT_TRUE(_health_warning_gps); EXPECT_FALSE(_can_arm_all_modes); } -// After divergence the receivers recover → hysteresis resets, no flag. -TEST_F(GnssRedundancyChecksTest, TwoGpsDivergingClearsOnRecovery) +// A sustained divergence with COM_GNSSLOSS_ACT = Return → gnss_lost and arming blocked. +TEST_F(GnssRedundancyChecksTest, ReturnBlocksArming) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(DIVERGING_FAR_LAT, BASE_LON)); + setRequiredGnss(2); + setGnssLossAct(1); // Return + runSustained(DIVERGING); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); + EXPECT_FALSE(_can_arm_all_modes); +} + +// Before arming, a divergence clears only after the receivers agree continuously for the hysteresis time, and a new +// disagreement restarts the wait. +TEST_F(GnssRedundancyChecksTest, BeforeArmingClearsAfterContinuousAgreement) +{ + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(); + ASSERT_TRUE(_failsafe_flags.gnss_lost); + + publishBoth(AGREEING); + runCheck(); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + + px4_usleep(1500000); + publishBoth(DIVERGING); + runCheck(); + publishBoth(AGREEING); runCheck(); - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(AGREEING_LAT, BASE_LON)); + px4_usleep(1500000); + publishBoth(AGREEING); + runCheck(); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + + px4_usleep(700000); + publishBoth(AGREEING); runCheck(); EXPECT_FALSE(_failsafe_flags.gnss_lost); EXPECT_FALSE(_health_warning_gps); } -// A receiver that fails its checks is not counted → no divergence check triggered. -TEST_F(GnssRedundancyChecksTest, FixTypeBelow3NotCounted) +// A vertical disagreement above COM_GNSS_V_MAX diverges, even when the receivers agree horizontally. +TEST_F(GnssRedundancyChecksTest, VerticalDivergence) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON, 0.02f, 6)); - publishGnss(1, makeGnss(DIVERGING_FAR_LAT, BASE_LON, 0.02f, 2)); + setRequiredGnss(2); + runSustained(0.f, DIVERGING); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); +} + +// The reported accuracy doesn't move the limits. +TEST_F(GnssRedundancyChecksTest, ReportedAccuracyKeepsTheLimits) +{ + setRequiredGnss(2); + runSustained(6.f, 0.f, false, 5.f); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); +} + +// The limits are configurable. +TEST_F(GnssRedundancyChecksTest, LimitsConfigurable) +{ + setRequiredGnss(2); + setFloatParam("COM_GNSS_H_MAX", 1.f); + runSustained(WITHIN_LIMIT); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + + setFloatParam("COM_GNSS_H_MAX", 15.f); + setFloatParam("COM_GNSS_V_MAX", 1.f); + runSustained(DIVERGING, 0.5f); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + + runSustained(0.f, WITHIN_LIMIT); + EXPECT_TRUE(_failsafe_flags.gnss_lost); +} + +// A disagreement the sensors module can't compute, with samples too far apart in time, doesn't clear a pre-arm +// divergence. +TEST_F(GnssRedundancyChecksTest, UnknownDisagreementKeepsADivergence) +{ + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(); + ASSERT_TRUE(_failsafe_flags.gnss_lost); + + publishBoth(NAN, NAN); + runCheck(); + EXPECT_TRUE(_failsafe_flags.gnss_lost); + EXPECT_TRUE(_health_warning_gps); +} + +// In flight, an unknown disagreement doesn't restart the hysteresis time of a divergence. +TEST_F(GnssRedundancyChecksTest, InFlightUnknownDisagreementKeepsTheTimer) +{ + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(true); + + publishBoth(NAN, NAN); + runCheck(true); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + + px4_usleep(HYSTERESIS_WAIT_US); + publishBoth(DIVERGING); + runCheck(true); + EXPECT_TRUE(_failsafe_flags.gnss_lost); +} + +// Without a healthy pair to compare, a pre-arm divergence clears at once. +TEST_F(GnssRedundancyChecksTest, NoPairClearsADivergence) +{ + setRequiredGnss(2); + publishBoth(DIVERGING); + runCheck(); + ASSERT_TRUE(_failsafe_flags.gnss_lost); + + _status.device_id_selected = 0; + publishBoth(DIVERGING); runCheck(); EXPECT_FALSE(_failsafe_flags.gnss_lost); EXPECT_FALSE(_health_warning_gps); } -// SYS_HAS_NUM_GNSS = 2, COM_GNSSLOSS_ACT = Return, only one receiver fixed → gnss_lost and arming blocked. +// Without a selected receiver among the healthy ones there is nothing to compare with. +TEST_F(GnssRedundancyChecksTest, NoSelectedReceiverNotCompared) +{ + setRequiredGnss(2); + _status.device_id_selected = 0; + runSustained(DIVERGING); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// A receiver that fails its checks is neither counted nor compared. +TEST_F(GnssRedundancyChecksTest, UnhealthyReceiverNotCompared) +{ + _status.inconsistency_horizontal[1] = DIVERGING; + publishGnss(0); + publishGnss(1, RTK_ACCURACY, RTK_ACCURACY, 2); + runCheck(); + EXPECT_FALSE(_failsafe_flags.gnss_lost); + EXPECT_FALSE(_health_warning_gps); +} + +// SYS_HAS_NUM_GNSS = 2, COM_GNSSLOSS_ACT = Return, only one receiver healthy → gnss_lost and arming blocked. TEST_F(GnssRedundancyChecksTest, BelowRequiredSetsGnssLost) { - int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + setRequiredGnss(2); setGnssLossAct(1); // Return - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); EXPECT_TRUE(_failsafe_flags.gnss_lost); EXPECT_TRUE(_health_warning_gps); @@ -274,10 +418,10 @@ TEST_F(GnssRedundancyChecksTest, BelowRequiredSetsGnssLost) // COM_GNSSLOSS_ACT = "Warning, block arming" blocks arming without any in-flight action (gnss_lost is still set for the warning). TEST_F(GnssRedundancyChecksTest, BelowRequiredBlockArmingBlocksArming) { - int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + setRequiredGnss(2); setGnssLossAct(4); // Warning, block arming - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); EXPECT_TRUE(_failsafe_flags.gnss_lost); EXPECT_TRUE(_health_warning_gps); @@ -287,26 +431,25 @@ TEST_F(GnssRedundancyChecksTest, BelowRequiredBlockArmingBlocksArming) // COM_GNSSLOSS_ACT = Warning reports the issue but never blocks arming. TEST_F(GnssRedundancyChecksTest, BelowRequiredWarningDoesNotBlockArming) { - int required = 2; param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + setRequiredGnss(2); setGnssLossAct(0); // Warning - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); EXPECT_TRUE(_failsafe_flags.gnss_lost); EXPECT_TRUE(_health_warning_gps); EXPECT_TRUE(_can_arm_all_modes); } -// After seeing two fixed receivers, losing one emits a health warning +// After seeing two healthy receivers, losing one emits a health warning // even when SYS_HAS_NUM_GNSS is not set (dropped_below_peak path). TEST_F(GnssRedundancyChecksTest, DroppedBelowPeakSetsHealthWarning) { - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); - publishGnss(1, makeGnss(AGREEING_LAT, BASE_LON)); + publishBoth(AGREEING); runCheck(); EXPECT_FALSE(_health_warning_gps); // both present, no warning - // GPS1 disappears. + // GNSS1 disappears. sensor_gnss_s gone{}; _gnss1_pub.publish(gone); // device_id = 0 → treated as absent runCheck(); @@ -317,10 +460,9 @@ TEST_F(GnssRedundancyChecksTest, DroppedBelowPeakSetsHealthWarning) // A receiver counts only while the status names it and says it passes its checks. TEST_F(GnssRedundancyChecksTest, StatusOfAnotherReceiverDoesNotCount) { - int required = 1; - param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + setRequiredGnss(1); - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); EXPECT_FALSE(_failsafe_flags.gnss_lost); @@ -333,16 +475,20 @@ TEST_F(GnssRedundancyChecksTest, StatusOfAnotherReceiverDoesNotCount) // A status older than 1 s doesn't count, however fresh the receiver's own data is. TEST_F(GnssRedundancyChecksTest, StaleStatusDoesNotCount) { - int required = 1; - param_set(param_find("SYS_HAS_NUM_GNSS"), &required); + setRequiredGnss(1); - publishGnss(0, makeGnss(BASE_LAT, BASE_LON)); + publishGnss(0); runCheck(); ASSERT_FALSE(_failsafe_flags.gnss_lost); _status.timestamp = hrt_absolute_time() - 2000000; // 2 s _status_pub.publish(_status); - _gnss0_pub.publish(makeGnss(BASE_LAT, BASE_LON)); + + sensor_gnss_s gnss{}; + gnss.timestamp = hrt_absolute_time(); + gnss.device_id = 1; + gnss.fix_type = 6; + _gnss0_pub.publish(gnss); runCheck(); EXPECT_TRUE(_failsafe_flags.gnss_lost); } diff --git a/src/modules/commander/commander_params.yaml b/src/modules/commander/commander_params.yaml index 8015d6f4c3e..7f1a1c6a07e 100644 --- a/src/modules/commander/commander_params.yaml +++ b/src/modules/commander/commander_params.yaml @@ -125,9 +125,8 @@ parameters: long: |- Action the system takes when a GNSS failure is detected during flight. Triggers when the active GNSS count drops below SYS_HAS_NUM_GNSS (disabled - 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. + when SYS_HAS_NUM_GNSS=0), or when two receivers disagree by more than + COM_GNSS_H_MAX or COM_GNSS_V_MAX and SYS_HAS_NUM_GNSS=2 (otherwise warning only). 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 @@ -138,6 +137,38 @@ parameters: 3: Terminate 4: Warning, block arming default: 0 + COM_GNSS_H_MAX: + description: + short: Maximum horizontal disagreement between GNSS receivers + long: |- + Two receivers diverge when their horizontal positions disagree by more than + this, after their antenna positions. The default suits standalone receivers. + Detecting swapped antennas needs a limit below twice the distance between them. + It also needs a heading that doesn't come from these two receivers. + Until the heading is aligned, a limit that low also reports a divergence. + Before arming a divergence is reported at once and clears after 2 s of + agreement. In flight it has to persist for 2 s. See COM_GNSSLOSS_ACT for the action. + type: float + default: 5.0 + unit: m + min: 0.1 + max: 20.0 + decimal: 1 + increment: 0.1 + COM_GNSS_V_MAX: + description: + short: Maximum vertical disagreement between GNSS receivers + long: |- + Two receivers diverge when their heights disagree by more than this, after + their antenna positions. The default suits standalone receivers. + Same timing and action as COM_GNSS_H_MAX. + type: float + default: 10.0 + unit: m + min: 0.1 + max: 20.0 + decimal: 1 + increment: 0.1 COM_ARM_SWISBTN: description: short: Arm switch is a momentary button diff --git a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt index 5c1a29c3e94..752d04b0034 100644 --- a/src/modules/sensors/vehicle_gps_position/CMakeLists.txt +++ b/src/modules/sensors/vehicle_gps_position/CMakeLists.txt @@ -35,6 +35,7 @@ px4_add_library(vehicle_gps_position VehicleGPSPosition.cpp VehicleGPSPosition.hpp GnssHeadingBaseline.hpp + GnssInconsistency.hpp GnssSelector.cpp GnssSelector.hpp PpsTimeSync.cpp @@ -49,3 +50,4 @@ target_link_libraries(vehicle_gps_position px4_add_unit_gtest(SRC GnssSelectorTest.cpp LINKLIBS vehicle_gps_position) px4_add_unit_gtest(SRC GnssHeadingBaselineTest.cpp) +px4_add_unit_gtest(SRC GnssInconsistencyTest.cpp LINKLIBS geo) diff --git a/src/modules/sensors/vehicle_gps_position/GnssInconsistency.hpp b/src/modules/sensors/vehicle_gps_position/GnssInconsistency.hpp new file mode 100644 index 00000000000..cbea4b8d759 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/GnssInconsistency.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#pragma once + +#include +#include +#include +#include + +namespace sensors +{ +namespace gnss_inconsistency +{ + +// Samples further apart are not compared: moving one along its velocity for longer adds more error than an RTK +// receiver's accuracy, and a receiver that far behind has missed samples +static constexpr float kMaxSampleTimeDifference = 0.5f; // s + +struct Inconsistency { + float horizontal{NAN}; // m + float vertical{NAN}; // m +}; + +/** + * How far another receiver is from where the reference receiver and the antenna positions put it. + * + * The other receiver's position is moved along its velocity to the time of the reference sample, as the receivers + * don't sample at the same time. The antenna baseline is rotated into NED with the attitude, so swapped or misplaced + * antennas show. Until the heading is aligned the yaw is arbitrary, and the lever arm can show as up to twice the + * baseline. + * + * @param antenna antenna position of the reference receiver, body frame (m) + * @param other_antenna antenna position of the other receiver, body frame (m) + * @param attitude rotation from body to NED, identity when unknown + * @return NAN horizontally without a horizontal position, vertically without a 3D fix, or both when the samples are too + * far apart in time + */ +inline Inconsistency inconsistency(const sensor_gnss_s &reference, const matrix::Vector3f &antenna, + const sensor_gnss_s &other, const matrix::Vector3f &other_antenna, + const matrix::Quatf &attitude) +{ + Inconsistency result{}; + + if ((reference.fix_type < sensor_gnss_s::FIX_TYPE_2D) || (other.fix_type < sensor_gnss_s::FIX_TYPE_2D)) { + return result; + } + + const float dt = 1e-6f * static_cast(static_cast(reference.timestamp_sample) + - static_cast(other.timestamp_sample)); + + if (fabsf(dt) > kMaxSampleTimeDifference) { + return result; + } + + matrix::Vector3f measured{}; + get_vector_to_next_waypoint(reference.latitude, reference.longitude, other.latitude, other.longitude, + &measured(0), &measured(1)); + measured(2) = -static_cast(other.altitude_msl - reference.altitude_msl); + + if (other.vel_ned_valid) { + measured += matrix::Vector3f(other.vel_north, other.vel_east, other.vel_down) * dt; + } + + const matrix::Vector3f error = measured - attitude.rotateVector(other_antenna - antenna); + result.horizontal = matrix::Vector2f(error(0), error(1)).norm(); + + if ((reference.fix_type >= sensor_gnss_s::FIX_TYPE_3D) && (other.fix_type >= sensor_gnss_s::FIX_TYPE_3D)) { + result.vertical = fabsf(error(2)); + } + + return result; +} + +} // namespace gnss_inconsistency +} // namespace sensors diff --git a/src/modules/sensors/vehicle_gps_position/GnssInconsistencyTest.cpp b/src/modules/sensors/vehicle_gps_position/GnssInconsistencyTest.cpp new file mode 100644 index 00000000000..6a8158c0213 --- /dev/null +++ b/src/modules/sensors/vehicle_gps_position/GnssInconsistencyTest.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + +#include + +#include +#include + +#include "GnssInconsistency.hpp" + +using namespace sensors::gnss_inconsistency; +using matrix::Eulerf; +using matrix::Quatf; +using matrix::Vector3f; + +static constexpr double kLatitude = 47.397742; +static constexpr double kLongitude = 8.545594; +static constexpr double kAltitude = 488.0; + +static constexpr float kTolerance = 1e-3f; + +// Antennas 0.7 m apart along the body x axis +static const Vector3f kFront(0.35f, 0.f, 0.f); +static const Vector3f kBack(-0.35f, 0.f, 0.f); + +static const Quatf kLevelNorth{}; + +// A receiver with an RTK fixed solution, the given distance north, east and down of the reference point +static sensor_gnss_s receiverAt(float north, float east, float down = 0.f, uint64_t timestamp_sample = 1'000'000) +{ + sensor_gnss_s gnss{}; + gnss.timestamp_sample = timestamp_sample; + gnss.fix_type = sensor_gnss_s::FIX_TYPE_RTK_FIXED; + add_vector_to_global_position(kLatitude, kLongitude, north, east, &gnss.latitude, &gnss.longitude); + gnss.altitude_msl = kAltitude - static_cast(down); + return gnss; +} + +static Inconsistency compare(const sensor_gnss_s &other, const Vector3f &antenna = Vector3f(), + const Vector3f &other_antenna = Vector3f(), const Quatf &attitude = kLevelNorth) +{ + return inconsistency(receiverAt(0.f, 0.f), antenna, other, other_antenna, attitude); +} + +TEST(GnssInconsistency, ZeroForReceiversThatAgree) +{ + const Inconsistency result = compare(receiverAt(0.f, 0.f)); + EXPECT_NEAR(result.horizontal, 0.f, kTolerance); + EXPECT_NEAR(result.vertical, 0.f, kTolerance); +} + +TEST(GnssInconsistency, DistanceOfReceiversWithoutLeverArm) +{ + EXPECT_NEAR(compare(receiverAt(2.f, 0.f)).horizontal, 2.f, kTolerance); + EXPECT_NEAR(compare(receiverAt(-1.f, 1.f)).horizontal, sqrtf(2.f), kTolerance); + EXPECT_NEAR(compare(receiverAt(0.f, 0.f, -1.5f)).vertical, 1.5f, kTolerance); +} + +TEST(GnssInconsistency, LeverArmRotatedWithTheHeading) +{ + // Facing north the front antenna is north of the back one, facing east it is east of it + EXPECT_NEAR(compare(receiverAt(0.7f, 0.f), kBack, kFront).horizontal, 0.f, kTolerance); + EXPECT_NEAR(compare(receiverAt(0.f, 0.7f), kBack, kFront, Quatf(Eulerf(0.f, 0.f, M_PI_2_F))).horizontal, 0.f, + kTolerance); + EXPECT_NEAR(compare(receiverAt(-0.7f, 0.f), kBack, kFront, Quatf(Eulerf(0.f, 0.f, M_PI_F))).horizontal, 0.f, + kTolerance); +} + +TEST(GnssInconsistency, SwappedAntennas) +{ + // Facing north, the front receiver reports the back antenna's position: off by twice the baseline + EXPECT_NEAR(compare(receiverAt(-0.7f, 0.f), kBack, kFront).horizontal, 1.4f, kTolerance); +} + +TEST(GnssInconsistency, TiltApplies) +{ + // Pitched 30 degrees nose up, the front antenna is 0.61 m ahead of and 0.35 m above the back one + const Quatf pitched_up(Eulerf(0.f, math::radians(30.f), 0.f)); + const sensor_gnss_s front = receiverAt(0.7f * cosf(math::radians(30.f)), 0.f, -0.35f); + + const Inconsistency result = compare(front, kBack, kFront, pitched_up); + EXPECT_NEAR(result.horizontal, 0.f, kTolerance); + EXPECT_NEAR(result.vertical, 0.f, kTolerance); + + // Read as level, the same positions are 0.35 m off vertically + EXPECT_NEAR(compare(front, kBack, kFront).vertical, 0.35f, kTolerance); +} + +TEST(GnssInconsistency, AntennaHeightsApply) +{ + // The other antenna is mounted 1 m higher but reports 1.5 m lower: 2.5 m off + const Vector3f above(0.f, 0.f, -1.f); + EXPECT_NEAR(compare(receiverAt(0.f, 0.f, -1.f), Vector3f(), above).vertical, 0.f, kTolerance); + EXPECT_NEAR(compare(receiverAt(0.f, 0.f, 1.5f), Vector3f(), above).vertical, 2.5f, kTolerance); +} + +TEST(GnssInconsistency, OtherReceiverMovedToTheReferenceSampleTime) +{ + // At 20 m/s north and 1 m/s up, the other receiver's sample, 0.1 s older, lies 2 m behind and 0.1 m lower + sensor_gnss_s other = receiverAt(-2.f, 0.f, 0.1f, 900'000); + other.vel_ned_valid = true; + other.vel_north = 20.f; + other.vel_down = -1.f; + + Inconsistency result = compare(other); + EXPECT_NEAR(result.horizontal, 0.f, kTolerance); + EXPECT_NEAR(result.vertical, 0.f, kTolerance); + + // Without a velocity it can't be moved + other.vel_ned_valid = false; + result = compare(other); + EXPECT_NEAR(result.horizontal, 2.f, kTolerance); + EXPECT_NEAR(result.vertical, 0.1f, kTolerance); +} + +TEST(GnssInconsistency, UnknownForSamplesFarApart) +{ + // A receiver that missed its samples for a second is not compared, either way round + Inconsistency result = inconsistency(receiverAt(0.f, 0.f, 0.f, 2'000'000), Vector3f(), receiverAt(0.f, 0.f), + Vector3f(), kLevelNorth); + EXPECT_TRUE(std::isnan(result.horizontal)); + EXPECT_TRUE(std::isnan(result.vertical)); + + result = compare(receiverAt(0.f, 0.f, 0.f, 2'000'000)); + EXPECT_TRUE(std::isnan(result.horizontal)); + EXPECT_TRUE(std::isnan(result.vertical)); + + EXPECT_NEAR(compare(receiverAt(0.f, 0.f, 0.f, 1'400'000)).horizontal, 0.f, kTolerance); +} + +TEST(GnssInconsistency, UnknownWithoutAPosition) +{ + sensor_gnss_s no_fix = receiverAt(0.f, 0.f); + no_fix.fix_type = sensor_gnss_s::FIX_TYPE_NONE; + + EXPECT_TRUE(std::isnan(compare(no_fix).horizontal)); + EXPECT_TRUE(std::isnan(inconsistency(no_fix, Vector3f(), receiverAt(0.f, 0.f), Vector3f(), kLevelNorth).horizontal)); + + // A 2D fix has no height to compare + sensor_gnss_s fix_2d = receiverAt(0.f, 0.f); + fix_2d.fix_type = sensor_gnss_s::FIX_TYPE_2D; + EXPECT_NEAR(compare(fix_2d).horizontal, 0.f, kTolerance); + EXPECT_TRUE(std::isnan(compare(fix_2d).vertical)); +} diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 074481fd373..b884be5fb28 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -249,11 +249,7 @@ void VehicleGPSPosition::Run() vehicle_gnss_s gnss_output{}; gnss_output.receiver = _latest_sample[selected]; - const GpsParamSlot *slot = findParamSlot(gnss_output.receiver.device_id, selected); - - if (slot) { - slot->offset.copyTo(gnss_output.antenna_offset); - } + antennaOffset(selected).copyTo(gnss_output.antenna_offset); const uint64_t pps_timestamp = _pps_time_sync.correct_gnss_timestamp(gnss_output.receiver.timestamp, gnss_output.receiver.time_utc_usec); @@ -279,6 +275,7 @@ void VehicleGPSPosition::Run() _vehicle_gnss_pub.publish(gnss_output); _selected_device_id = gnss_output.receiver.device_id; + _published_instance = selected; } PublishStatus(); @@ -484,6 +481,25 @@ void VehicleGPSPosition::PublishStatus() const hrt_abstime now = hrt_absolute_time(); + bool publishing[GPS_MAX_RECEIVERS] {}; + + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { + publishing[i] = (_latest_sample[i].timestamp != 0) && (now < _latest_sample[i].timestamp + GnssSelector::GNSS_TIMEOUT_US); + } + + for (int i = 0; i < sensors_status_gnss_s::MAX_RECEIVERS; i++) { + status.inconsistency_horizontal[i] = NAN; + status.inconsistency_vertical[i] = NAN; + } + + const int selected = _published_instance; + const bool compare = (selected >= 0) && publishing[selected]; + + // Level and facing north until an attitude publishes + vehicle_attitude_s attitude{}; + const matrix::Quatf attitude_q = (compare && _vehicle_attitude_sub.copy(&attitude)) ? matrix::Quatf(attitude.q) + : matrix::Quatf(); + for (int i = 0; i < GPS_MAX_RECEIVERS; i++) { const GnssChecks &checks = _gnss_checks[i]; const sensor_gnss_s &sample = _latest_sample[i]; @@ -491,6 +507,15 @@ void VehicleGPSPosition::PublishStatus() status.device_ids[i] = sample.device_id; status.healthy[i] = !_gnss_selector.hasFailed(i, now); status.availability[i] = _gnss_selector.getAvailability(i); + + if (compare && publishing[i]) { + const gnss_inconsistency::Inconsistency inconsistency = (i == selected) ? gnss_inconsistency::Inconsistency{0.f, 0.f} + : gnss_inconsistency::inconsistency(_latest_sample[selected], antennaOffset(selected), sample, antennaOffset(i), + attitude_q); + status.inconsistency_horizontal[i] = inconsistency.horizontal; + status.inconsistency_vertical[i] = inconsistency.vertical; + } + status.failed_strict_checks[i] = checks.getStrictFailFlags(); status.failed_relaxed_checks[i] = checks.getRelaxedFailFlags(); status.strict[i] = checks.strict(); @@ -504,6 +529,12 @@ void VehicleGPSPosition::PublishStatus() _sensors_status_gnss_pub.publish(status); } +matrix::Vector3f VehicleGPSPosition::antennaOffset(int instance) const +{ + const GpsParamSlot *slot = findParamSlot(_latest_sample[instance].device_id, instance); + return slot ? slot->offset : matrix::Vector3f{}; +} + int VehicleGPSPosition::resolvePreferredInstance() const { const int gnss_prime = _param_sens_gnss_prime.get(); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp index 626ea77acce..ce19ffe1abd 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.hpp @@ -50,11 +50,13 @@ #include #include #include +#include #include #include #include #include "GnssHeadingBaseline.hpp" +#include "GnssInconsistency.hpp" #include "GnssSelector.hpp" #include "PpsTimeSync.hpp" @@ -102,6 +104,9 @@ private: // SENS_GNSSn_* slot for a receiver, by device_id or (when no IDs are configured) by sensor_gnss instance const GpsParamSlot *findParamSlot(uint32_t device_id, int instance) const; + // Antenna position of a receiver, body frame (m), 0 without a SENS_GNSSn_* slot + matrix::Vector3f antennaOffset(int instance) const; + // sensor_gnss instance of the receiver SENS_GNSS_PRIME designates, or of the moving base with -1; -1 for none int resolvePreferredInstance() const; @@ -140,6 +145,7 @@ private: }; uORB::Subscription _pps_capture_sub{ORB_ID(pps_capture)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; @@ -172,6 +178,7 @@ private: GnssChecks _gnss_checks[GPS_MAX_RECEIVERS] {}; sensor_gnss_s _latest_sample[GPS_MAX_RECEIVERS] {}; ///< timestamp_sample corrected, timestamp 0 until it publishes uint32_t _selected_device_id{0}; + int8_t _published_instance{-1}; ///< receiver of the last vehicle_gnss sample, -1 before the first one int8_t _preferred_instance{-1}; uint8_t _first_publication[GPS_MAX_RECEIVERS] {}; ///< 1 for the first receiver to publish, 2 for the next, 0 before uint8_t _receivers_published{0};