From 70b80e600d4c8fd05c6b662b1cf087fa31918649 Mon Sep 17 00:00:00 2001 From: a-f-G-U-C <65810997+a-f-G-U-C@users.noreply.github.com> Date: Sun, 24 Oct 2021 12:38:35 +0000 Subject: [PATCH 1/3] use Position struct for GPS data --- src/gps/GPS.cpp | 11 +++++++-- src/gps/GPS.h | 13 ++++------- src/gps/NMEAGPS.cpp | 48 +++++++++++++++++++++++++++------------ src/gps/UBloxGPS.cpp | 54 +++++++++++++++++++++++++++++++------------- 4 files changed, 85 insertions(+), 41 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 89f2d79b..c1e48034 100644 --- a/src/gps/GPS.cpp +++ b/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; } diff --git a/src/gps/GPS.h b/src/gps/GPS.h index b337703b..895d5607 100644 --- a/src/gps/GPS.h +++ b/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 - 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 + Position p = Position_init_default; GPS() : concurrency::OSThread("GPS") {} diff --git a/src/gps/NMEAGPS.cpp b/src/gps/NMEAGPS.cpp index c652fab6..d0d5e46e 100644 --- a/src/gps/NMEAGPS.cpp +++ b/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); - longitude = toDegInt(loc.lng); + p.latitude_i = toDegInt(loc.lat); + p.longitude_i = toDegInt(loc.lng); - geoidal_height = reader.geoidHeight.meters(); -#ifdef GPS_ALTITUDE_HAE - altitude = reader.altitude.meters() + geoidal_height; -#else - altitude = reader.altitude.meters(); + p.alt_geoid_sep = reader.geoidHeight.meters(); + p.altitude_hae = reader.altitude.meters() + p.alt_geoid_sep; + p.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()); diff --git a/src/gps/UBloxGPS.cpp b/src/gps/UBloxGPS.cpp index 9f8285bb..30b6a579 100644 --- a/src/gps/UBloxGPS.cpp +++ b/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 - if (ublox.moduleQueried.SIV) { - uint16_t gSIV = ublox.getSIV(0); - setNumSatellites(gSIV); - } + // FIXME - can opportunistically attempt to set RTC from GPS timestamp? // 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; - latitude = tmp_lat; -#ifdef GPS_ALTITUDE_HAE - altitude = tmp_alt_hae / 1000; -#else - altitude = tmp_alt_msl / 1000; -#endif - geoidal_height = (tmp_alt_hae - tmp_alt_msl) / 1000; - pos_timestamp = tmp_ts; - dop = tmp_dop; + p.location_source = Position_LocSource_LOCSRC_GPS_INTERNAL; + p.longitude_i = tmp_lon; + p.latitude_i = tmp_lat; + p.altitude = tmp_alt_msl / 1000; + p.altitude_hae = tmp_alt_hae / 1000; + p.alt_geoid_sep = (tmp_alt_hae - tmp_alt_msl) / 1000; + p.pos_timestamp = tmp_ts; + p.PDOP = tmp_dop; + p.fix_type = fixType; + 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", From f69c8dddad79c3da20f0d000eca12c9ff34ace61 Mon Sep 17 00:00:00 2001 From: a-f-G-U-C <65810997+a-f-G-U-C@users.noreply.github.com> Date: Sun, 24 Oct 2021 12:48:48 +0000 Subject: [PATCH 2/3] update MeshService to use Position struct --- src/mesh/MeshService.cpp | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/mesh/MeshService.cpp b/src/mesh/MeshService.cpp index b8d47ab8..2e27cfc4 100644 --- a/src/mesh/MeshService.cpp +++ b/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) - pos.altitude = gps->altitude; - pos.latitude_i = gps->latitude; - pos.longitude_i = gps->longitude; + // load data from GPS object, will add timestamp + battery further down + pos = gps->p; } 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 { - // throw away old position - pos.latitude_i = 0; - pos.longitude_i = 0; - pos.altitude = 0; + pos = node->position; } } - 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); From ef1d52ca04d42c7a993e6ab92196a0c6c83c2c0a Mon Sep 17 00:00:00 2001 From: a-f-G-U-C <65810997+a-f-G-U-C@users.noreply.github.com> Date: Sun, 24 Oct 2021 13:02:31 +0000 Subject: [PATCH 3/3] update log message, sanity check --- src/GPSStatus.h | 32 ++++++++++++++++++++++---------- 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/src/GPSStatus.h b/src/GPSStatus.h index 6023a6db..7399d7e2 100644 --- a/src/GPSStatus.h +++ b/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; - { - isDirty = matches(newStatus); - initialized = true; - hasLock = newStatus->hasLock; - isConnected = newStatus->isConnected; + bool isDirty = matches(newStatus); - p = newStatus->p; + 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) - DEBUG_MSG("New GPS pos lat=%f, lon=%f, alt=%d, pdop=%.2f, heading=%.2f, sats=%d\n", + if (hasLock) { + // 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); }