From 33de00d319fe553373c313b146c900722692809e Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 20 May 2026 15:57:09 +0200 Subject: [PATCH] test(ekf2): 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 ebd024e3637..38b60d21c12 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()); +}