From eb27e744f78a0bce1d2be1774d9acffaf63082a7 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, 5 Sep 2021 15:10:06 +0000 Subject: [PATCH 1/2] add positional timestamp and geoidal separation Relevant to issues #842 and #843 (timestamp) and historical issues #392 and #359 (timestamp, geoidal) --- src/gps/GPS.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 63d594863..b337703b3 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -45,6 +45,9 @@ class GPS : private concurrency::OSThread // 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") {} virtual ~GPS(); From 1c06c2af9f74df4054a27c2b85972ffc4059afb3 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, 5 Sep 2021 17:11:04 +0000 Subject: [PATCH 2/2] read lat/lon/alt into temp vars instead of global Instead of reading the GPS solution directly into global variables and risking a bad-over-good overwrite (issue #857), read it into temporary vars and only update global vars after validation. Also updates positional timestamp variable and prepares (non-breaking) for HAE altitude support (issue #359) --- src/gps/UBloxGPS.cpp | 103 ++++++++++++++++++++++++++++++++----------- 1 file changed, 77 insertions(+), 26 deletions(-) diff --git a/src/gps/UBloxGPS.cpp b/src/gps/UBloxGPS.cpp index 2da2a21a8..21ed5af3c 100644 --- a/src/gps/UBloxGPS.cpp +++ b/src/gps/UBloxGPS.cpp @@ -8,6 +8,8 @@ // if gps_update_interval below this value, do not powercycle the GPS #define UBLOX_POWEROFF_THRESHOLD 90 +#define PDOP_INVALID 9999 + extern RadioConfig radioConfig; UBloxGPS::UBloxGPS() {} @@ -116,19 +118,18 @@ bool UBloxGPS::factoryReset() /** Idle processing while GPS is looking for lock */ 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 // the order here is important, because we only check for has latitude when reading - ublox.getSIV(maxWait()); - ublox.getPDOP(maxWait()); - ublox.getP(maxWait()); - // Update fixtype - if (ublox.moduleQueried.fixType) { - fixType = ublox.getFixType(0); - // DEBUG_MSG("GPS fix type %d, numSats %d\n", fixType, numSatellites); - } + //ublox.getSIV(maxWait()); // redundant with getPDOP below + ublox.getPDOP(maxWait()); // will trigger getSOL on NEO6, getP on others + ublox.getP(maxWait()); // will trigger getPosLLH on NEO6, getP on others + + // the fixType flag will be checked and updated in lookForLocation() } /** @@ -169,28 +170,78 @@ bool UBloxGPS::lookForLocation() { bool foundLocation = false; - if (ublox.moduleQueried.SIV) - setNumSatellites(ublox.getSIV(0)); + // catch fixType changes here, instead of whileActive() + if (ublox.moduleQueried.fixType) { + fixType = ublox.getFixType(); + } - if (ublox.moduleQueried.pDOP) - dop = ublox.getPDOP(0); // PDOP (an accuracy metric) is reported in 10^2 units so we have to scale down when we use it + // check if GPS has an acceptable lock + if (! hasLock()) { + return false; + } - // we only notify if position has changed due to a new fix - if ((fixType >= 3 && fixType <= 4)) { - if (ublox.moduleQueried.latitude) // rd fixes only - { - latitude = ublox.getLatitude(0); - longitude = ublox.getLongitude(0); - altitude = ublox.getAltitudeMSL(0) / 1000; // in mm convert to meters + // check if a complete GPS solution set is available for reading + // (some of these, like lat/lon are redundant and can be removed) + if ( ! (ublox.moduleQueried.latitude && + ublox.moduleQueried.longitude && + ublox.moduleQueried.altitude && + ublox.moduleQueried.pDOP && + ublox.moduleQueried.gpsiTOW)) + { + // Not ready? No problem! We'll try again later. + return false; + } - // Note: heading is only currently implmented in the ublox for the 8m chipset - therefore - // don't read it here - it will generate an ignored getPVT command on the 6ms - // heading = ublox.getHeading(0); + // read lat/lon/alt/dop data into temporary variables to avoid + // overwriting global variables with potentially invalid data + int32_t tmp_dop = ublox.getPDOP(0); // PDOP (an accuracy metric) is reported in 10^2 units so we have to scale down when we use it + int32_t tmp_lat = ublox.getLatitude(0); + int32_t tmp_lon = ublox.getLongitude(0); + int32_t tmp_alt_msl = ublox.getAltitudeMSL(0); + int32_t tmp_alt_hae = ublox.getAltitude(0); + // Note: heading is only currently implmented in the ublox for the 8m chipset - therefore + // don't read it here - it will generate an ignored getPVT command on the 6ms + // heading = ublox.getHeading(0); - // 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! - foundLocation = (latitude != 0) && (longitude != 0) && (latitude <= 900000000 && latitude >= -900000000); - } + // read positional timestamp + struct tm t; + t.tm_sec = ublox.getSecond(0); + t.tm_min = ublox.getMinute(0); + t.tm_hour = ublox.getHour(0); + t.tm_mday = ublox.getDay(0); + t.tm_mon = ublox.getMonth(0) - 1; + t.tm_year = ublox.getYear(0) - 1900; + t.tm_isdst = false; + + 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); + } + + // 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! + // FIXME - NULL ISLAND is a real location on Earth! + foundLocation = (tmp_lat != 0) && (tmp_lon != 0) && + (tmp_lat <= 900000000) && (tmp_lat >= -900000000) && + (tmp_dop < PDOP_INVALID); + + // 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; + } else { + DEBUG_MSG("Invalid location discarded\n"); } return foundLocation;