GPS guard: reject a fix the receiver reports as 2D (GSA)

docs/cli_commands.md

@@ -1078,7 +1078,7 @@ region save
 ---
 
 #### GPS anti-spoofing guard (When built with `GPS_GUARD`)
-With `GPS_GUARD`, a fix becomes the node's last fix (position, telemetry, adverts) only after it passes plausibility checks: at least `GPS_GUARD_MIN_SATS` satellites, HDOP ≤ `GPS_GUARD_MAX_HDOP10`/10, inside the optional build-time region (`GPS_GUARD_LAT_MIN/MAX`, `GPS_GUARD_LON_MIN/MAX`), within the fence around the node's configured `lat`/`lon` (if set), no jump faster than `GPS_GUARD_MAX_SPEED_KMH`, and `GPS_GUARD_STABLE_FIXES` consistent fixes in a row. An unverified position is never reported.
+With `GPS_GUARD`, a fix becomes the node's last fix (position, telemetry, adverts) only after it passes plausibility checks: at least `GPS_GUARD_MIN_SATS` satellites, HDOP ≤ `GPS_GUARD_MAX_HDOP10`/10, a 3D fix if the receiver reports the fix mode in GSA (`GPS_GUARD_REQUIRE_3D`, default on; status `rej 2d`), inside the optional build-time region (`GPS_GUARD_LAT_MIN/MAX`, `GPS_GUARD_LON_MIN/MAX`), within the fence around the node's configured `lat`/`lon` (if set), no jump faster than `GPS_GUARD_MAX_SPEED_KMH`, and `GPS_GUARD_STABLE_FIXES` consistent fixes in a row. An unverified position is never reported.
 
 GPS time is written to the clock only while such a fix is current, never earlier than the firmware build date, and, once the clock holds a plausible time (from the build date to 10 years after it), only within `GPS_GUARD_TIME_MAX_STEP` seconds for the first sync and within clock drift (`GPS_GUARD_TIME_DRIFT_BASE` s + `GPS_GUARD_TIME_DRIFT_PPM`) afterwards. Setting the clock manually (`time`, `clock sync`) resets that anchor. A clock outside that window (e.g. a bogus date in 2080) is corrected by the first trusted fix.
 

src/helpers/sensors/GpsGuard.h

@@ -4,7 +4,8 @@
 //
 // A location provider asks the guard before it stores a fix as its last fix and before it
 // writes GPS time to the clock:
-//  - acceptFix(): plausibility checks (satellites, HDOP, optional build-time region, distance
+//  - acceptFix(): plausibility checks (satellites, HDOP, 3D fix if the receiver reports the fix
+//    mode, optional build-time region, distance
 //    from a reference position, jump speed) and N consecutive consistent fixes;
 //  - acceptTime(): GPS time only while a trusted fix is current, not before this firmware
 //    was built, and once the clock is valid only small corrections (clock drift after the
@@ -25,6 +26,9 @@
 #ifndef GPS_GUARD_MAX_HDOP10
   #define GPS_GUARD_MAX_HDOP10      40      // HDOP 4.0
 #endif
+#ifndef GPS_GUARD_REQUIRE_3D
+  #define GPS_GUARD_REQUIRE_3D      1       // reject a fix the receiver reports as 2D (GSA); 0 = off
+#endif
 #ifndef GPS_GUARD_FENCE_M
   #define GPS_GUARD_FENCE_M         1000    // max distance from the reference position (0 = off)
 #endif
@@ -135,8 +139,10 @@ class GpsGuard {
   bool isTimeAlert() const { return _time_alert; }
 
   // Fix as reported by the receiver: lat/lon in micro-degrees, HDOP in tenths (255 = unknown),
-  // gps_time = UTC epoch of the fix. Returns true if the fix can be stored as trusted.
-  bool acceptFix(long lat_e6, long lon_e6, long sats, uint8_t hdop10, uint32_t gps_time) {
+  // gps_time = UTC epoch of the fix, fix_mode = receiver fix mode from GSA (1 = none, 2 = 2D,
+  // 3 = 3D, 0 = unknown: no recent GSA). Returns true if the fix can be stored as trusted.
+  bool acceptFix(long lat_e6, long lon_e6, long sats, uint8_t hdop10, uint32_t gps_time,
+                 uint8_t fix_mode = 0) {
     const double lat = lat_e6 / 1000000.0;
     const double lon = lon_e6 / 1000000.0;
     const unsigned long now = millis();
@@ -149,6 +155,11 @@ class GpsGuard {
     } else if (hdop10 == 255 || hdop10 > GPS_GUARD_MAX_HDOP10) {
       why = "hdop";
     }
+#if GPS_GUARD_REQUIRE_3D
+    else if (fix_mode == 1 || fix_mode == 2) {
+      why = "2d";
+    }
+#endif
 #if defined(GPS_GUARD_LAT_MIN) && defined(GPS_GUARD_LAT_MAX) && defined(GPS_GUARD_LON_MIN) && defined(GPS_GUARD_LON_MAX)
     else if (lat < GPS_GUARD_LAT_MIN || lat > GPS_GUARD_LAT_MAX || lon < GPS_GUARD_LON_MIN || lon > GPS_GUARD_LON_MAX) {
       why = "region";

src/helpers/sensors/MicroNMEALocationProvider.h

@@ -54,6 +54,26 @@ class MicroNMEALocationProvider : public LocationProvider {
     bool _nmea_seen = false;
 #ifdef GPS_GUARD
     GpsGuard _guard;
+    uint8_t _gsa_fix_mode = 0;      // fix mode from the last GSA: 1 = none, 2 = 2D, 3 = 3D
+    unsigned long _gsa_ms = 0;
+
+    static bool isGsa(const char* id) {   // GPGSA, GNGSA, BDGSA, ...
+        const size_t n = strlen(id);
+        return n >= 3 && strcmp(id + n - 3, "GSA") == 0;
+    }
+    // $--GSA,<mode A/M>,<fix 1/2/3>,...
+    void parseGsa(const char* s) {
+        s = strchr(s, ',');
+        if (s != NULL) s = strchr(s + 1, ',');
+        if (s != NULL && s[1] >= '1' && s[1] <= '3') {
+            _gsa_fix_mode = s[1] - '0';
+            _gsa_ms = millis();
+        }
+    }
+    // 0 (unknown) if the receiver does not send GSA, or sent none recently
+    uint8_t currentFixMode() const {
+        return (_gsa_fix_mode != 0 && (uint32_t)(millis() - _gsa_ms) <= 2000) ? _gsa_fix_mode : 0;
+    }
 #endif
 
     static uint32_t ageMs(unsigned long timestamp) {
@@ -94,7 +114,7 @@ class MicroNMEALocationProvider : public LocationProvider {
 #ifdef GPS_GUARD
         // only plausible, stable fixes become the last fix (position, telemetry, time sync)
         if (!_guard.acceptFix(nmea.getLatitude(), nmea.getLongitude(), nmea.getNumSatellites(),
-                              nmea.getHDOP(), getTimestamp())) return false;
+                              nmea.getHDOP(), getTimestamp(), currentFixMode())) return false;
 #endif
 
         LastFix fix;
@@ -336,6 +356,7 @@ class MicroNMEALocationProvider : public LocationProvider {
 
                     const char* messageId = nmea.getMessageID();
 #ifdef GPS_GUARD
+                    if (isGsa(messageId)) parseGsa(nmea.getSentence());
                     if (parsed && !nmea.isValid() &&
                         (strcmp(messageId, "GGA") == 0 || strcmp(messageId, "RMC") == 0)) {
                         _guard.noFix(nmea.getNumSatellites());
@@ -395,5 +416,10 @@ class MicroNMEALocationProvider : public LocationProvider {
             fix_age,
             (unsigned long)_begin_calls,
             (unsigned long)_stop_calls);
+#ifdef GPS_GUARD
+        // fix mode from GSA (1 none, 2 2D, 3 3D, 0 no recent GSA)
+        const size_t len = strlen(out);
+        if (len < out_size) snprintf(out + len, out_size - len, " fm:%u", currentFixMode());
+#endif
     }
 };