fix: stuck GNSS control logic on intermittent data

src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp

@@ -47,29 +47,26 @@ 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;
 		}
 
 	} 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)

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());
+}