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};