From 457ce90541204b4ef7f89b3317f6fb2a46b187cb Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Fri, 23 May 2025 09:35:26 +0200
Subject: [PATCH] ekf2: run simplified GNSS checks after initial fix

This prevents stopping GNSS fusion on slightly degraded solution when
tight checks are set
---
 .../ekf2/EKF/aid_sources/gnss/gnss_checks.cpp | 30 ++++++++++++++++++-
 .../ekf2/EKF/aid_sources/gnss/gnss_checks.hpp |  1 +
 src/modules/ekf2/test/test_EKF_gps.cpp        | 11 +++++--
 3 files changed, 39 insertions(+), 3 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 6d41f482d5..fc26914693 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.cpp
@@ -50,7 +50,7 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us)
 	bool passed = false;
 
 	if (_initial_checks_passed) {
-		if (runInitialFixChecks(gnss)) {
+		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));
 
@@ -79,6 +79,34 @@ bool GnssChecks::run(const gnssSample &gnss, uint64_t time_us)
 	return passed;
 }
 
+bool GnssChecks::runSimplifiedChecks(const gnssSample &gnss)
+{
+	_check_fail_status.flags.fix = (gnss.fix_type < 3);
+
+	// Check the reported horizontal and vertical position accuracy
+	_check_fail_status.flags.hacc = (gnss.hacc > 50.f);
+	_check_fail_status.flags.vacc = (gnss.vacc > 50.f);
+
+	// Check the reported speed accuracy
+	_check_fail_status.flags.sacc = (gnss.sacc > 10.f);
+
+	_check_fail_status.flags.spoofed = gnss.spoofed;
+
+	bool passed = true;
+
+	if (
+		_check_fail_status.flags.fix ||
+		(_check_fail_status.flags.hacc    && isCheckEnabled(GnssChecksMask::kHacc)) ||
+		(_check_fail_status.flags.vacc    && isCheckEnabled(GnssChecksMask::kVacc)) ||
+		(_check_fail_status.flags.sacc    && isCheckEnabled(GnssChecksMask::kSacc)) ||
+		(_check_fail_status.flags.spoofed && isCheckEnabled(GnssChecksMask::kSpoofed))
+	) {
+		passed = false;
+	}
+
+	return passed;
+}
+
 bool GnssChecks::runInitialFixChecks(const gnssSample &gnss)
 {
 	// Check the fix type
diff --git a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
index af64678271..a8fdb39b90 100644
--- a/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
+++ b/src/modules/ekf2/EKF/aid_sources/gnss/gnss_checks.hpp
@@ -112,6 +112,7 @@ private:
 
 	bool isCheckEnabled(GnssChecksMask check) { return (_params.check_mask & static_cast<int32_t>(check)); }
 
+	bool runSimplifiedChecks(const gnssSample &gnss);
 	bool runInitialFixChecks(const gnssSample &gnss);
 	void runOnGroundGnssChecks(const gnssSample &gnss);
 
diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp
index f6aae937df..cc738a3f6f 100644
--- a/src/modules/ekf2/test/test_EKF_gps.cpp
+++ b/src/modules/ekf2/test/test_EKF_gps.cpp
@@ -84,12 +84,19 @@ TEST_F(EkfGpsTest, gpsTimeout)
 	// WHEN: the number of satellites drops below the minimum
 	_sensor_simulator._gps.setNumberOfSatellites(3);
 
+	// THEN: the GNSS fusion does not stop because other metrics are good enough
+	_sensor_simulator.runSeconds(8);
+	EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
+
+	// WHEN: the fix type drops
+	_sensor_simulator._gps.setFixType(0);
+
 	// THEN: the GNSS fusion stops after some time
 	_sensor_simulator.runSeconds(8);
 	EXPECT_FALSE(_ekf_wrapper.isIntendingGpsFusion());
 
-	// BUT WHEN: the number of satellites is good again
-	_sensor_simulator._gps.setNumberOfSatellites(16);
+	// BUT WHEN: the fix type is good again
+	_sensor_simulator._gps.setFixType(3);
 
 	// THEN: the GNSS fusion restarts
 	_sensor_simulator.runSeconds(6);