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