From f6f9b9cd03fcc98a2c1ff67603d0421db49504b0 Mon Sep 17 00:00:00 2001 From: a-f-G-U-C <65810997+a-f-G-U-C@users.noreply.github.com> Date: Wed, 8 Sep 2021 14:02:13 +0000 Subject: [PATCH 1/2] fixes from PR #851, #858 ported to NMEA GPS apply fixes and upgrades from PR #851, #858 to NMEA GPS code --- src/gps/NMEAGPS.cpp | 142 +++++++++++++++++++++++++++++++++++--------- src/gps/NMEAGPS.h | 11 ++++ 2 files changed, 125 insertions(+), 28 deletions(-) diff --git a/src/gps/NMEAGPS.cpp b/src/gps/NMEAGPS.cpp index db7c328e4..cc287c2d6 100644 --- a/src/gps/NMEAGPS.cpp +++ b/src/gps/NMEAGPS.cpp @@ -2,6 +2,12 @@ #include "NMEAGPS.h" #include "RTC.h" +#include + +// 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; } @@ -48,7 +65,6 @@ The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of s t.tm_year = d.year() - 1900; t.tm_isdst = false; DEBUG_MSG("NMEA GPS time %d\n", t.tm_sec); - perhapsSetRTC(RTCQualityGPS, t); return true; @@ -64,47 +80,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(); - // hasValidLocation = ((fixtype >= 1) && (fixtype <= 5)); +#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS + 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.hdop.isUpdated()) { - dop = reader.hdop.value(); - } - if (reader.course.isUpdated()) { + if (reader.course.isUpdated() && reader.course.isValid()) { 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(); - latitude = toDegInt(loc.lat); - longitude = toDegInt(loc.lng); - - // Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus - if(longitude == 0) - DEBUG_MSG("Ignoring bogus NMEA position\n"); - else { - foundLocation = 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); - } +bool NMEAGPS::hasLock() +{ + // Using GPGGA fix quality indicator + if (fixQual >= 1 && fixQual <= 5) { +#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS + // Use GPGSA fix type 2D/3D (better) if available + if (fixType == 3 || fixType == 0) // zero means "no data received" +#endif + return true; } - return foundLocation; + return false; } + bool NMEAGPS::whileIdle() { bool isValid = false; @@ -112,7 +198,7 @@ bool NMEAGPS::whileIdle() // First consume any chars that have piled up at the receiver while (_serial_gps->available() > 0) { int c = _serial_gps->read(); - // DEBUG_MSG("%c", c); + DEBUG_MSG("%c", c); isValid |= reader.encode(c); } diff --git a/src/gps/NMEAGPS.h b/src/gps/NMEAGPS.h index 2fd29d408..bc5582508 100644 --- a/src/gps/NMEAGPS.h +++ b/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(); }; From de712ce41a518782571406058953bc6acfb80998 Mon Sep 17 00:00:00 2001 From: a-f-G-U-C <65810997+a-f-G-U-C@users.noreply.github.com> Date: Wed, 8 Sep 2021 14:26:21 +0000 Subject: [PATCH 2/2] disable debug code --- src/gps/NMEAGPS.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/gps/NMEAGPS.cpp b/src/gps/NMEAGPS.cpp index cc287c2d6..dcb24d617 100644 --- a/src/gps/NMEAGPS.cpp +++ b/src/gps/NMEAGPS.cpp @@ -65,6 +65,7 @@ The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of s t.tm_year = d.year() - 1900; t.tm_isdst = false; DEBUG_MSG("NMEA GPS time %d\n", t.tm_sec); + perhapsSetRTC(RTCQualityGPS, t); return true; @@ -198,7 +199,7 @@ bool NMEAGPS::whileIdle() // First consume any chars that have piled up at the receiver while (_serial_gps->available() > 0) { int c = _serial_gps->read(); - DEBUG_MSG("%c", c); + // DEBUG_MSG("%c", c); isValid |= reader.encode(c); }