From c8512bdc64a24a762219400bf5bbc72ec76121ec Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 May 2026 15:57:09 +0200 Subject: [PATCH 1/2] test: add intermittent GNSS failure case test --- src/modules/ekf2/test/test_EKF_gps.cpp | 29 ++++++++++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index ebd024e36372..38b60d21c12a 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -283,3 +283,32 @@ TEST_F(EkfGpsTest, gnssJumpDetectionDRMode) EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); } + +TEST_F(EkfGpsTest, gnssIntermittentSaccFailureDisablesFusion) +{ + // GIVEN: EKF that fuses GPS on the ground after passing the initial checks + EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion()); + + // WHEN: speed accuracy fails most of the time but briefly passes 1 sample every 2s. + // Each good sample passes runInitialFixChecks() but run() still returns false because + // the last failure is too recent (min_health_time_us = 10s not satisfied). + // The fusion must therefore never actually fuse any data. + const float bad_sacc = 5.0f; // fails ekf2_req_sacc (default 1.0 m/s) + const float good_sacc = 0.2f; // passes ekf2_req_sacc + + for (int i = 0; i < 4; i++) { + gnssSample gps_data = _sensor_simulator._gps.getData(); + gps_data.sacc = bad_sacc; + _sensor_simulator._gps.setData(gps_data); + _sensor_simulator.runSeconds(1.8f); + + gps_data = _sensor_simulator._gps.getData(); + gps_data.sacc = good_sacc; + _sensor_simulator._gps.setData(gps_data); + _sensor_simulator.runSeconds(0.2f); // 1 GPS sample at 5 Hz + } + + // THEN: GNSS fusion must be disabled because the checks never truly pass + // and reset_timeout_max was exceeded since the last real pass. + EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion()); +} From 28f7bb5eb4dd15f2a150b00c6bb168cc0337e64c Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 May 2026 16:19:26 +0200 Subject: [PATCH 2/2] fix: only record GNSS check success time when passing hysteresis This fixes the case where the gnss checks is reporting false but the failure doesn't make the control logic time out. --- .../ekf2/EKF/aid_sources/gnss/gnss_checks.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp index e9077e8bddc3..647b69d8f906 100644 --- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp +++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp @@ -47,17 +47,16 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us) _time_last_fail_us = time_us; } - bool passed = false; - // Run strict checks while not flying yet if (!_control_status.flags.in_air) { _initial_checks_passed = false; } + _passed = false; + if (_initial_checks_passed) { if (runSimplifiedChecks(gnss)) { - _time_last_pass_us = time_us; - passed = isTimedOut(_time_last_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10)); + _passed = isTimedOut(_time_last_fail_us, time_us, math::max((uint64_t)1e6, (uint64_t)_params.min_health_time_us / 10)); } else { _time_last_fail_us = time_us; @@ -65,11 +64,9 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us) } else { if (runInitialFixChecks(gnss)) { - _time_last_pass_us = time_us; - if (isTimedOut(_time_last_fail_us, time_us, (uint64_t)_params.min_health_time_us)) { _initial_checks_passed = true; - passed = true; + _passed = true; } } else { @@ -80,8 +77,11 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us) lat_lon_prev.initReference(gnss.lat, gnss.lon, gnss.time_us); _alt_prev = gnss.alt; - _passed = passed; - return passed; + if (_passed) { + _time_last_pass_us = time_us; + } + + return _passed; } bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)