Merge pull request #861 from a-f-G-U-C/NMEA-GPS-fixes

fixes from PR #851, #858 ported to NMEA GPS

src/gps/NMEAGPS.cpp

@@ -2,6 +2,12 @@
 #include "NMEAGPS.h"
 #include "RTC.h"
 
+#include <TinyGPS++.h>
+
+// GPS solutions older than this will be rejected - see TinyGPSDatum::age()
+#define GPS_SOL_EXPIRY_MS 300   // in millis
+#define NMEA_MSG_GXGSA "GNGSA"  // GSA message (GPGSA, GNGSA etc)
+
 static int32_t toDegInt(RawDegrees d)
 {
     int32_t degMult = 10000000; // 1e7
@@ -21,6 +27,17 @@ bool NMEAGPS::setupGPS()
     pinMode(PIN_GPS_PPS, INPUT);
 #endif
 
+// Currently disabled per issue #525 (TinyGPS++ crash bug)
+// when fixed upstream, can be un-disabled to enable 3D FixType and PDOP
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+    // see NMEAGPS.h
+    gsafixtype.begin(reader, NMEA_MSG_GXGSA, 2);
+    gsapdop.begin(reader, NMEA_MSG_GXGSA, 15);
+    DEBUG_MSG("Using " NMEA_MSG_GXGSA " for 3DFIX and PDOP\n");
+#else
+    DEBUG_MSG("GxGSA NOT available\n");
+#endif
+
     return true;
 }
 
@@ -64,47 +81,117 @@ The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of s
  */
 bool NMEAGPS::lookForLocation()
 {
-    bool foundLocation = false;
+    // By default, TinyGPS++ does not parse GPGSA lines, which give us 
+    //   the 2D/3D fixType (see NMEAGPS.h)
+    // At a minimum, use the fixQuality indicator in GPGGA (FIXME?)
+    fixQual = reader.fixQuality();
 
-    // uint8_t fixtype = reader.fixQuality();
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
-    // hasValidLocation = ((fixtype >= 1) && (fixtype <= 5));
+    fixType = atoi(gsafixtype.value());  // will set to zero if no data
+    DEBUG_MSG("FIX QUAL=%d, TYPE=%d\n", fixQual, fixType);
+#endif
 
+    // check if GPS has an acceptable lock
+    if (! hasLock())
+        return false;
+
+    // check if a complete GPS solution set is available for reading
+    //   tinyGPSDatum::age() also includes isValid() test
+    // FIXME
+    if (! ((reader.location.age() < GPS_SOL_EXPIRY_MS) &&
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+            (gsafixtype.age() < GPS_SOL_EXPIRY_MS) &&
+#endif
+            (reader.time.age() < GPS_SOL_EXPIRY_MS) &&
+            (reader.date.age() < GPS_SOL_EXPIRY_MS)))
+    {
+        // DEBUG_MSG("SOME data is TOO OLD\n");
+        return false;
+    }
+
+    // Is this a new point or are we re-reading the previous one?
+    if (! reader.location.isUpdated())
+        return false;
+
+    // Start reading the data
+    auto loc = reader.location.value();
+
+    // Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
+    // Bail out EARLY to avoid overwriting previous good data (like #857)
+    if(toDegInt(loc.lat) == 0) {
+        DEBUG_MSG("Ignoring bogus NMEA position\n");
+        return false;
+    }
+
+    // Dilution of precision (an accuracy metric) is reported in 10^2 units, so we need to scale down when we use it
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+    dop = TinyGPSPlus::parseDecimal(gsapdop.value());
+#else
+    // FIXME! naive PDOP emulation (assumes VDOP==HDOP)
+    // correct formula is PDOP = SQRT(HDOP^2 + VDOP^2)
+    dop = 1.41 * reader.hdop.value();
+#endif
+
+    // Discard incomplete or erroneous readings
+    if (dop == 0)
+        return false;
+
+    latitude = toDegInt(loc.lat);
+    longitude = toDegInt(loc.lng);
+
+    geoidal_height = reader.geoidHeight.meters();
+#ifdef GPS_ALTITUDE_HAE
+    altitude = reader.altitude.meters() + geoidal_height;
+#else
+    altitude = reader.altitude.meters();
+#endif
+
+    // positional timestamp
+    struct tm t;
+    t.tm_sec = reader.time.second();
+    t.tm_min = reader.time.minute();
+    t.tm_hour = reader.time.hour();
+    t.tm_mday = reader.date.day();
+    t.tm_mon = reader.date.month() - 1;
+    t.tm_year = reader.date.year() - 1900;
+    t.tm_isdst = false;
+    pos_timestamp = mktime(&t);
+
+    // Nice to have, if available
     if (reader.satellites.isUpdated()) {
         setNumSatellites(reader.satellites.value());
     }
 
-    // Diminution of precision (an accuracy metric) is reported in 10^2 units, so we need to scale down when we use it
+    if (reader.course.isUpdated() && reader.course.isValid()) {
-    if (reader.hdop.isUpdated()) {
-        dop = reader.hdop.value();
-    }
-    if (reader.course.isUpdated()) {
         heading = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
     }
 
-    if (reader.altitude.isUpdated())
+/*
-        altitude = reader.altitude.meters();
+    // REDUNDANT?
+    // expect gps pos lat=37.520825, lon=-122.309162, alt=158
+    DEBUG_MSG("new NMEA GPS pos lat=%f, lon=%f, alt=%d, dop=%g, heading=%f\n",
+              latitude * 1e-7, longitude * 1e-7, altitude, dop * 1e-2,
+              heading * 1e-5);
+*/
+    return true;
+}
 
-    if (reader.location.isUpdated()) {
 
-        auto loc = reader.location.value();
+bool NMEAGPS::hasLock()
-        latitude = toDegInt(loc.lat);
+{
-        longitude = toDegInt(loc.lng);
+    // Using GPGGA fix quality indicator
-
+    if (fixQual >= 1 && fixQual <= 5) {
-        // Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
-        if(longitude == 0)
+        // Use GPGSA fix type 2D/3D (better) if available
-            DEBUG_MSG("Ignoring bogus NMEA position\n");
+        if (fixType == 3 || fixType == 0)  // zero means "no data received"
-        else {
+#endif
-            foundLocation = true;
+            return true;
-
-            // expect gps pos lat=37.520825, lon=-122.309162, alt=158
-            DEBUG_MSG("new NMEA GPS pos lat=%f, lon=%f, alt=%d, hdop=%g, heading=%f\n", latitude * 1e-7, longitude * 1e-7, altitude,
-                    dop * 1e-2, heading * 1e-5);
-        }
     }
 
-    return foundLocation;
+    return false;
 }
 
+
 bool NMEAGPS::whileIdle()
 {
     bool isValid = false;

src/gps/NMEAGPS.h

@@ -12,6 +12,15 @@
 class NMEAGPS : public GPS
 {
     TinyGPSPlus reader;
+    uint8_t fixQual = 0;  // fix quality from GPGGA
+
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+    // (20210908) TinyGps++ can only read the GPGSA "FIX TYPE" field
+    // via optional feature "custom fields", currently disabled (bug #525)
+    TinyGPSCustom gsafixtype;  // custom extract fix type from GPGSA
+    TinyGPSCustom gsapdop;     // custom extract PDOP from GPGSA
+    uint8_t fixType = 0;  // fix type from GPGSA
+#endif
 
   public:
     virtual bool setupGPS();
@@ -38,4 +47,6 @@ class NMEAGPS : public GPS
      * @return true if we've acquired a new location
      */
     virtual bool lookForLocation();
+
+    virtual bool hasLock();
 };