Merge pull request #896 from a-f-G-U-C/gps-use-pos-struct

use Position struct for GPS data

src/GPSStatus.h

@@ -99,6 +99,10 @@ class GPSStatus : public Status
 
     bool matches(const GPSStatus *newStatus) const
     {
+#if GPS_EXTRAVERBOSE
+        DEBUG_MSG("GPSStatus.match() new pos@%x to old pos@%x\n",
+                    newStatus->p.pos_timestamp, p.pos_timestamp);
+#endif
         return (newStatus->hasLock != hasLock ||
                 newStatus->isConnected != isConnected ||
                 newStatus->p.latitude_i != p.latitude_i ||
@@ -109,25 +113,33 @@ class GPSStatus : public Status
                 newStatus->p.ground_track != p.ground_track ||
                 newStatus->p.sats_in_view != p.sats_in_view);
     }
+
     int updateStatus(const GPSStatus *newStatus)
     {
         // Only update the status if values have actually changed
-        bool isDirty;
+        bool isDirty = matches(newStatus);
-        {
+
-            isDirty = matches(newStatus);
+        if (isDirty && p.pos_timestamp && 
+            (newStatus->p.pos_timestamp == p.pos_timestamp)) {
+            // We can NEVER be in two locations at the same time! (also PR #886)
+            DEBUG_MSG("BUG!! positional timestamp unchanged from prev solution\n");
+        }
+
         initialized = true;
         hasLock = newStatus->hasLock;
         isConnected = newStatus->isConnected;
 
         p = newStatus->p;
-        }
+
         if (isDirty) {
-            if (hasLock)
+            if (hasLock) {
-                DEBUG_MSG("New GPS pos lat=%f, lon=%f, alt=%d, pdop=%.2f, heading=%.2f, sats=%d\n", 
+                // In debug logs, identify position by @timestamp:stage (stage 3 = notify)
+                DEBUG_MSG("New GPS pos@%x:3 lat=%f, lon=%f, alt=%d, pdop=%.2f, track=%.2f, sats=%d\n", 
+                            p.pos_timestamp,
                             p.latitude_i * 1e-7, p.longitude_i * 1e-7,
                             p.altitude, p.PDOP * 1e-2, p.ground_track * 1e-5, 
                             p.sats_in_view);
-            else
+            } else
                 DEBUG_MSG("No GPS lock\n");
             onNewStatus.notifyObservers(this);
         }

src/gps/GPS.cpp

@@ -202,11 +202,13 @@ void GPS::publishUpdate()
     if (shouldPublish) {
         shouldPublish = false;
 
-        DEBUG_MSG("publishing GPS lock=%d\n", hasLock());
+        // In debug logs, identify position by @timestamp:stage (stage 2 = publish)
+        DEBUG_MSG("publishing pos@%x:2, hasVal=%d, GPSlock=%d\n", 
+                    p.pos_timestamp, hasValidLocation, hasLock());
 
         // Notify any status instances that are observing us
         const meshtastic::GPSStatus status =
-            meshtastic::GPSStatus(hasValidLocation, isConnected(), latitude, longitude, altitude, dop, heading, numSatellites);
+            meshtastic::GPSStatus(hasValidLocation, isConnected(), p);
         newStatus.notifyObservers(&status);
     }
 }
@@ -244,6 +246,7 @@ int32_t GPS::runOnce()
 
         bool gotLoc = lookForLocation();
         if (gotLoc && !hasValidLocation) { // declare that we have location ASAP
+            DEBUG_MSG("hasValidLocation RISING EDGE\n");
             hasValidLocation = true;
             shouldPublish = true;
         }
@@ -260,6 +263,10 @@ int32_t GPS::runOnce()
 
             if (tooLong) {
                 // we didn't get a location during this ack window, therefore declare loss of lock
+                if (hasValidLocation) {
+                    DEBUG_MSG("hasValidLocation FALLING EDGE (last read: %d)\n", gotLoc);
+                }
+                p = Position_init_default;
                 hasValidLocation = false;
             }
 

src/gps/GPS.h

@@ -17,6 +17,10 @@ class GPS : private concurrency::OSThread
   private:
     uint32_t lastWakeStartMsec = 0, lastSleepStartMsec = 0, lastWhileActiveMsec = 0;
 
+    /**
+     * hasValidLocation - indicates that the position variables contain a complete
+     *   GPS location, valid and fresh (< gps_update_interval + gps_attempt_time)
+     */
     bool hasValidLocation = false; // default to false, until we complete our first read
 
     bool isAwake = false; // true if we want a location right now
@@ -39,14 +43,7 @@ class GPS : private concurrency::OSThread
     /** If !0 we will attempt to connect to the GPS over I2C */
     static uint8_t i2cAddress;
 
-    int32_t latitude = 0, longitude = 0; // as an int mult by 1e-7 to get value as double
+    Position p = Position_init_default;
-    int32_t altitude = 0;
-    uint32_t dop = 0;     // Diminution of position; PDOP where possible (UBlox), HDOP otherwise (TinyGPS) in 10^2 units (needs
-                          // scaling before use)
-    uint32_t heading = 0; // Heading of motion, in degrees * 10^-5
-
-    int32_t geoidal_height = 0;  // geoidal separation, in meters!
-    time_t pos_timestamp = 0;  // positional timestamp from GPS solution
 
     GPS() : concurrency::OSThread("GPS") {}
 

src/gps/NMEAGPS.cpp

@@ -95,6 +95,17 @@ bool NMEAGPS::lookForLocation()
     if (! hasLock())
         return false;
 
+#ifdef GPS_EXTRAVERBOSE
+    DEBUG_MSG("AGE: LOC=%d FIX=%d DATE=%d TIME=%d\n", 
+                reader.location.age(), 
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+                gsafixtype.age(),
+#else
+                0,
+#endif
+                reader.date.age(), reader.time.age());
+#endif  // GPS_EXTRAVERBOSE
+
     // check if a complete GPS solution set is available for reading
     //   tinyGPSDatum::age() also includes isValid() test
     // FIXME
@@ -105,7 +116,7 @@ bool NMEAGPS::lookForLocation()
             (reader.time.age() < GPS_SOL_EXPIRY_MS) &&
             (reader.date.age() < GPS_SOL_EXPIRY_MS)))
     {
-        // DEBUG_MSG("SOME data is TOO OLD\n");
+        DEBUG_MSG("SOME data is TOO OLD\n");
         return false;
     }
 
@@ -113,7 +124,7 @@ bool NMEAGPS::lookForLocation()
     if (! reader.location.isUpdated())
         return false;
 
-    // Start reading the data
+    // We know the solution is fresh and valid, so just read the data
     auto loc = reader.location.value();
 
     // Some GPSes (Air530) seem to send a zero longitude when the current fix is bogus
@@ -123,27 +134,34 @@ bool NMEAGPS::lookForLocation()
         return false;
     }
 
+    p.location_source = Position_LocSource_LOCSRC_GPS_INTERNAL;
+
     // 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());
+    p.HDOP = reader.hdop.value();
+    p.PDOP = TinyGPSPlus::parseDecimal(gsapdop.value());
+    DEBUG_MSG("PDOP=%d, HDOP=%d\n", dop, reader.hdop.value());
 #else
     // FIXME! naive PDOP emulation (assumes VDOP==HDOP)
     // correct formula is PDOP = SQRT(HDOP^2 + VDOP^2)
-    dop = 1.41 * reader.hdop.value();
+    p.HDOP = reader.hdop.value();
+    p.PDOP = 1.41 * reader.hdop.value();
 #endif
 
     // Discard incomplete or erroneous readings
-    if (dop == 0)
+    if (reader.hdop.value() == 0)
         return false;
 
-    latitude = toDegInt(loc.lat);
+    p.latitude_i = toDegInt(loc.lat);
-    longitude = toDegInt(loc.lng);
+    p.longitude_i = toDegInt(loc.lng);
 
-    geoidal_height = reader.geoidHeight.meters();
+    p.alt_geoid_sep = reader.geoidHeight.meters();
-#ifdef GPS_ALTITUDE_HAE
+    p.altitude_hae = reader.altitude.meters() + p.alt_geoid_sep;
-    altitude = reader.altitude.meters() + geoidal_height;
+    p.altitude = reader.altitude.meters();
-#else
+
-    altitude = reader.altitude.meters();
+    p.fix_quality = fixQual;
+#ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS
+    p.fix_type = fixType;
 #endif
 
     // positional timestamp
@@ -155,16 +173,16 @@ bool NMEAGPS::lookForLocation()
     t.tm_mon = reader.date.month() - 1;
     t.tm_year = reader.date.year() - 1900;
     t.tm_isdst = false;
-    pos_timestamp = mktime(&t);
+    p.pos_timestamp = mktime(&t);
 
     // Nice to have, if available
     if (reader.satellites.isUpdated()) {
-        setNumSatellites(reader.satellites.value());
+        p.sats_in_view = reader.satellites.value();
     }
 
     if (reader.course.isUpdated() && reader.course.isValid()) {
         if (reader.course.value() < 36000) {  // sanity check
-            heading = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
+            p.ground_track = reader.course.value() * 1e3; // Scale the heading (in degrees * 10^-2) to match the expected degrees * 10^-5
         } else {
             DEBUG_MSG("BOGUS course.value() REJECTED: %d\n",
                         reader.course.value());

src/gps/UBloxGPS.cpp

@@ -10,6 +10,8 @@
 
 #define PDOP_INVALID 9999
 
+// #define UBX_MODE_NMEA
+
 extern RadioConfig radioConfig;
 
 UBloxGPS::UBloxGPS() {}
@@ -48,12 +50,22 @@ bool UBloxGPS::setupGPS()
         delay(500);
 
     if (isConnected()) {
+#ifdef UBX_MODE_NMEA
+        DEBUG_MSG("Connected to UBLOX GPS, downgrading to NMEA mode\n");
+        DEBUG_MSG("- GPS errors below are related and safe to ignore\n");
+#else
         DEBUG_MSG("Connected to UBLOX GPS successfully\n");
+#endif
 
         if (!setUBXMode())
             RECORD_CRITICALERROR(CriticalErrorCode_UBloxInitFailed); // Don't halt the boot if saving the config fails, but do report the bug
 
+#ifdef UBX_MODE_NMEA
+        return false;
+#else
         return true;
+#endif
+
     } else {
         return false;
     }
@@ -61,6 +73,17 @@ bool UBloxGPS::setupGPS()
 
 bool UBloxGPS::setUBXMode()
 {
+#ifdef UBX_MODE_NMEA
+    if (_serial_gps) {
+        ublox.setUART1Output(COM_TYPE_NMEA, 1000);
+    }
+    if (i2cAddress) {
+        ublox.setI2COutput(COM_TYPE_NMEA, 1000);
+    }
+
+    return false;  // pretend initialization failed to force NMEA mode
+#endif
+
     if (_serial_gps) {
         if (!ublox.setUART1Output(COM_TYPE_UBX, 1000)) // Use native API
             return false;
@@ -119,7 +142,6 @@ bool UBloxGPS::factoryReset()
 void UBloxGPS::whileActive()
 {
     ublox.flushPVT();  // reset ALL freshness flags first
-
     ublox.getT(maxWait()); // ask for new time data - hopefully ready when we come back
 
     // Ask for a new position fix - hopefully it will have results ready by next time
@@ -177,6 +199,7 @@ bool UBloxGPS::lookForLocation()
             ublox.moduleQueried.longitude &&
             ublox.moduleQueried.altitude &&
             ublox.moduleQueried.pDOP &&
+            ublox.moduleQueried.SIV &&
             ublox.moduleQueried.gpsDay)) 
     {
         // Not ready? No problem! We'll try again later.
@@ -188,6 +211,7 @@ bool UBloxGPS::lookForLocation()
     DEBUG_MSG("FixType=%d\n", fixType);
 #endif
 
+
     // check if GPS has an acceptable lock
     if (! hasLock()) {
         ublox.flushPVT();  // reset ALL freshness flags
@@ -217,11 +241,7 @@ bool UBloxGPS::lookForLocation()
 
     time_t tmp_ts = mktime(&t);
 
-    // SIV number is nice-to-have if it's available
+    // FIXME - can opportunistically attempt to set RTC from GPS timestamp?
-    if (ublox.moduleQueried.SIV) {
-        uint16_t gSIV = ublox.getSIV(0);
-        setNumSatellites(gSIV);
-    }
 
     // bogus lat lon is reported as 0 or 0 (can be bogus just for one)
     // Also: apparently when the GPS is initially reporting lock it can output a bogus latitude > 90 deg!
@@ -232,16 +252,18 @@ bool UBloxGPS::lookForLocation()
 
     // only if entire dataset is valid, update globals from temp vars
     if (foundLocation) {
-        longitude = tmp_lon;
+        p.location_source = Position_LocSource_LOCSRC_GPS_INTERNAL;
-        latitude = tmp_lat;
+        p.longitude_i = tmp_lon;
-#ifdef GPS_ALTITUDE_HAE
+        p.latitude_i = tmp_lat;
-        altitude = tmp_alt_hae / 1000;
+        p.altitude = tmp_alt_msl / 1000;
-#else
+        p.altitude_hae = tmp_alt_hae / 1000;
-        altitude = tmp_alt_msl / 1000;
+        p.alt_geoid_sep = (tmp_alt_hae - tmp_alt_msl) / 1000;
-#endif
+        p.pos_timestamp = tmp_ts;
-        geoidal_height = (tmp_alt_hae - tmp_alt_msl) / 1000;
+        p.PDOP = tmp_dop;
-        pos_timestamp = tmp_ts;
+        p.fix_type = fixType;
-        dop = tmp_dop;
+        p.sats_in_view = ublox.getSIV(0);
+        // In debug logs, identify position by @timestamp:stage (stage 1 = birth)
+        DEBUG_MSG("lookForLocation() new pos@%x:1\n", tmp_ts);
     } else {
         // INVALID solution - should never happen
         DEBUG_MSG("Invalid location lat/lon/hae/dop %d/%d/%d/%d - discarded\n",

src/mesh/MeshService.cpp

@@ -213,27 +213,32 @@ int MeshService::onGPSChanged(const meshtastic::GPSStatus *newStatus)
 {
     // Update our local node info with our position (even if we don't decide to update anyone else)
     NodeInfo *node = refreshMyNodeInfo();
-    Position pos = node->position;
+    Position pos = Position_init_default;
 
     if (newStatus->getHasLock()) {
-        if (gps->altitude != 0)
+        // load data from GPS object, will add timestamp + battery further down
-            pos.altitude = gps->altitude;
+        pos = gps->p;
-        pos.latitude_i = gps->latitude;
-        pos.longitude_i = gps->longitude;
     } else {
         // The GPS has lost lock, if we are fixed position we should just keep using
         // the old position
+#if GPS_EXTRAVERBOSE
+        DEBUG_MSG("onGPSchanged() - lost validLocation\n");
+#endif
         if (radioConfig.preferences.fixed_position) {
             DEBUG_MSG("WARNING: Using fixed position\n");
-        } else {
+            pos = node->position;
-            // throw away old position
-            pos.latitude_i = 0;
-            pos.longitude_i = 0;
-            pos.altitude = 0;
         }
     }
 
-    DEBUG_MSG("got gps notify time=%u, lat=%d, bat=%d\n", pos.time, pos.latitude_i, pos.battery_level);
+    // Finally add a fresh timestamp and battery level reading
+    // I KNOW this is redundant with refreshMyNodeInfo() above, but these are
+    //   inexpensive nonblocking calls and can be refactored in due course
+    pos.time = getValidTime(RTCQualityGPS);
+    pos.battery_level = powerStatus->getBatteryChargePercent();
+
+    // In debug logs, identify position by @timestamp:stage (stage 4 = nodeDB)
+    DEBUG_MSG("onGPSChanged() pos@%x:4, time=%u, lat=%d, bat=%d\n", 
+                pos.pos_timestamp, pos.time, pos.latitude_i, pos.battery_level);
 
     // Update our current position in the local DB
     nodeDB.updatePosition(nodeDB.getNodeNum(), pos, RX_SRC_LOCAL);