From 1eebc62e19acbecaefa811f53c2bd6e41e73c20e Mon Sep 17 00:00:00 2001 From: amandel Date: Fri, 5 Nov 2021 17:18:38 +0100 Subject: [PATCH 1/3] Allow debugging of GPS vs location drift - store gps timestamp in gps record - log gps timestamp of record every minute to make derivations visible --- src/gps.cpp | 14 ++++++++++++-- src/gps.h | 2 ++ src/gpsrecord.cpp | 12 ++++++++++++ src/gpsrecord.h | 5 ++++- src/writer.cpp | 4 ++++ 5 files changed, 34 insertions(+), 3 deletions(-) diff --git a/src/gps.cpp b/src/gps.cpp index 48bf3639..7c5804ff 100644 --- a/src/gps.cpp +++ b/src/gps.cpp @@ -1027,8 +1027,17 @@ void Gps::parseUbxMessage() { } void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uint32_t receivedMs, const uint32_t delayMs) { - log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x, tAcc %dns, delay %dms", - message.iTow, message.fTow, message.week, message.leapS, message.valid, message.tAcc, delayMs); + log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x (%s%s%s) , tAcc %uns, DATE: %s, delay %dms", + message.iTow, message.fTow, message.week, message.leapS, message.valid, + message.valid & 1 ? "TOW" : "", + message.valid & 2 ? " WEEK " : "", + message.valid & 4 ? " GPS" : "", + message.tAcc, + TimeUtils::dateTimeToString(TimeUtils::toTime(message.week, message.iTow / 1000)).c_str(), + delayMs); + if (message.valid & 2) { + mLastGpsWeek = message.week; + } if ((message.valid & 0x03) == 0x03 // WEEK && TOW && delayMs < 80 && message.tAcc < (80 * 1000 * 1000 /* 80ms */) @@ -1136,6 +1145,7 @@ bool Gps::prepareGpsData(uint32_t tow) { // fine already prepared } else if (mIncomingGpsRecord.mCollectTow == 0) { mIncomingGpsRecord.setTow(tow); + mIncomingGpsRecord.setWeek(mLastGpsWeek); } else if ((int32_t) (mIncomingGpsRecord.mCollectTow - tow) > 0) { log_e("Data already published: %d", mCurrentGpsRecord.mCollectTow); diff --git a/src/gps.h b/src/gps.h index 310f44bb..5d6b5b0d 100644 --- a/src/gps.h +++ b/src/gps.h @@ -486,6 +486,8 @@ class Gps { /* last time, when the ESP clock was adjusted to the GPR UTC time, * in millis ticker. */ uint32_t mLastTimeTimeSet = 0; + /* GPS week as received with time. */ + uint32_t mLastGpsWeek = 0; /* Number of bytes sent for alp request. */ uint32_t mAlpBytesSent = 0; /* If true a outdated gps cfg was detected. */ diff --git a/src/gpsrecord.cpp b/src/gpsrecord.cpp index 1a4eba43..50dd6922 100644 --- a/src/gpsrecord.cpp +++ b/src/gpsrecord.cpp @@ -46,6 +46,10 @@ void GpsRecord::setTow(uint32_t tow) { mCollectTow = tow; } +void GpsRecord::setWeek(uint32_t week) { + mCollectWeek = week; +} + void GpsRecord::setPosition(int32_t lon, int32_t lat, int32_t height) { mLongitude = lon; mLatitude = lat; @@ -75,6 +79,14 @@ bool GpsRecord::isAllSet() const { return mPositionSet && mVelocitySet && mInfoSet && mHdopSet; } +uint32_t GpsRecord::getTow() const { + return mCollectTow; +} + +uint32_t GpsRecord::getWeek() const { + return mCollectWeek; +} + String GpsRecord::getAltitudeMetersString() const { // 3 digits is problematic, 2 are enough any way. return toScaledString((mHeight + 5) / 10, 2); diff --git a/src/gpsrecord.h b/src/gpsrecord.h index 0fd75961..0e299618 100644 --- a/src/gpsrecord.h +++ b/src/gpsrecord.h @@ -27,7 +27,6 @@ #include #include -#include class Gps; @@ -57,6 +56,8 @@ class GpsRecord { uint8_t getSatellitesUsed() const; uint8_t getFixStatusFlags() const; GPS_FIX getFixStatus() const; + uint32_t getTow() const; + uint32_t getWeek() const; protected: /* Clear all collected data */ @@ -64,6 +65,7 @@ class GpsRecord { /* Store tow and related date time data. */ void setTow(uint32_t tow); + void setWeek(uint32_t tow); void setPosition(int32_t lon, int32_t lat, int32_t height); @@ -80,6 +82,7 @@ class GpsRecord { * merge records together. */ uint32_t mCollectTow = 0; + uint32_t mCollectWeek = 0; // mainly debug /* deg, scale 1e-7 */ int32_t mLongitude; /* deg, scale 1e-7 */ diff --git a/src/writer.cpp b/src/writer.cpp index c47569cd..413ee6e8 100644 --- a/src/writer.cpp +++ b/src/writer.cpp @@ -21,6 +21,7 @@ * see . */ +#include #include "writer.h" #include "utils/file.h" @@ -271,6 +272,9 @@ bool CSVFileWriter::append(DataSet &set) { csv += "DEV: GPS: "; csv += ObsUtils::encodeForCsvField(msg); } + } else if (time.tm_sec == 40) { + csv += "DBG GPS Time: " + + TimeUtils::dateTimeToString(TimeUtils::toTime(set.gpsRecord.getWeek(), set.gpsRecord.getTow() / 1000)); } // #endif csv += ";"; From b686903bd279c2d1d88bf3580552aa5601f641da Mon Sep 17 00:00:00 2001 From: amandel Date: Sun, 17 Apr 2022 12:41:46 +0200 Subject: [PATCH 2/3] check for invalid reported gps week number - read week number also from NAV-SOL message. - report week changes via statistic message - use GPS time directly for csv if available - logging adjustments --- src/gps.cpp | 22 ++++++++++++++++++---- src/writer.cpp | 10 +++++++++- 2 files changed, 27 insertions(+), 5 deletions(-) diff --git a/src/gps.cpp b/src/gps.cpp index 7c5804ff..6fc9c1dc 100644 --- a/src/gps.cpp +++ b/src/gps.cpp @@ -872,7 +872,7 @@ void Gps::parseUbxMessage() { mGpsBuffer.navDop.iTow, mGpsBuffer.navDop.gDop, mGpsBuffer.navDop.pDop, mGpsBuffer.navDop.tDop, mGpsBuffer.navDop.vDop, mGpsBuffer.navDop.hDop, mGpsBuffer.navDop.nDop, mGpsBuffer.navDop.eDop); - if (prepareGpsData(mGpsBuffer.navSol.iTow)) { + if (prepareGpsData(mGpsBuffer.navDop.iTow)) { mIncomingGpsRecord.setHdop(mGpsBuffer.navDop.hDop); checkGpsDataState(); } @@ -882,6 +882,15 @@ void Gps::parseUbxMessage() { log_v("SOL: iTOW: %u, gpsFix: %d, flags: %02x, numSV: %d, pDop: %04d.", mGpsBuffer.navSol.iTow, mGpsBuffer.navSol.gpsFix, mGpsBuffer.navSol.flags, mGpsBuffer.navSol.numSv, mGpsBuffer.navSol.pDop); + if (mGpsBuffer.navSol.flags & 4) { // WKNSET + if (mLastGpsWeek != mGpsBuffer.navSol.week) { + // debugging #294 + addStatisticsMessage(String("NAVSOL gps week changed: ") + + mLastGpsWeek + " -> " + mGpsBuffer.navSol.week + + " at " + TimeUtils::dateTimeToString()); + } + mLastGpsWeek = mGpsBuffer.navSol.week; + } if (prepareGpsData(mGpsBuffer.navSol.iTow)) { mIncomingGpsRecord.setInfo(mGpsBuffer.navSol.numSv, mGpsBuffer.navSol.gpsFix, mGpsBuffer.navSol.flags); checkGpsDataState(); @@ -1027,15 +1036,20 @@ void Gps::parseUbxMessage() { } void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uint32_t receivedMs, const uint32_t delayMs) { - log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x (%s%s%s) , tAcc %uns, DATE: %s, delay %dms", + log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x (%s%s%s), tAcc %uns, DATE: %s, delay %dms", message.iTow, message.fTow, message.week, message.leapS, message.valid, message.valid & 1 ? "TOW" : "", - message.valid & 2 ? " WEEK " : "", - message.valid & 4 ? " GPS" : "", + message.valid & 2 ? " WEEK" : "", + message.valid & 4 ? " UTC" : "", message.tAcc, TimeUtils::dateTimeToString(TimeUtils::toTime(message.week, message.iTow / 1000)).c_str(), delayMs); if (message.valid & 2) { + if (mLastGpsWeek != message.week) { + // debugging #294 + addStatisticsMessage(String("TIMEGPS gps week changed: ") + + mLastGpsWeek + " -> " + message.week); + } mLastGpsWeek = message.week; } if ((message.valid & 0x03) == 0x03 // WEEK && TOW diff --git a/src/writer.cpp b/src/writer.cpp index 413ee6e8..b60ae4a5 100644 --- a/src/writer.cpp +++ b/src/writer.cpp @@ -192,8 +192,16 @@ bool CSVFileWriter::append(DataSet &set) { return true; } + time_t theTime; + if (set.gpsRecord.getTow() != 0 && set.gpsRecord.getWeek() != 0) { + theTime = TimeUtils::toTime(set.gpsRecord.getWeek(), set.gpsRecord.getTow() / 1000); + // TODO: Force adjust filename if week changes and set.gpsRecord.getTow() is not small via mFinalFileName = false; + } else { + theTime = set.time; + } tm time; - localtime_r(&(set.time), &time); + localtime_r(&(theTime), &time); + // localtime_r(&(set.time), &time); char date[32]; snprintf(date, sizeof(date), "%02d.%02d.%04d;%02d:%02d:%02d;%u;", From 618ceb605931ef3a2c52cc3624d7153b05b9fa9f Mon Sep 17 00:00:00 2001 From: amandel Date: Sun, 17 Apr 2022 19:25:49 +0200 Subject: [PATCH 3/3] GPS and logging tweaks - wait for more precise data till time is set (ms) - gps logging, log each message only once - log gps and cpu clock time --- src/gps.cpp | 16 +++++++++++++--- src/gps.h | 2 ++ src/writer.cpp | 5 ++++- 3 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/gps.cpp b/src/gps.cpp index 3c68060f..a5bb3981 100644 --- a/src/gps.cpp +++ b/src/gps.cpp @@ -576,6 +576,15 @@ String Gps::getMessages() const { return theGpsMessage; } +String Gps::popMessage() { + String theGpsMessage = ""; + if (mMessages.size() > 0) { + theGpsMessage = mMessages[0]; + mMessages.erase(mMessages.begin()); + } + return theGpsMessage; +} + String Gps::getMessage(uint16_t idx) const { String theGpsMessage = ""; if (mMessages.size() > idx) { @@ -1053,8 +1062,8 @@ void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uin mLastGpsWeek = message.week; } if ((message.valid & 0x03) == 0x03 // WEEK && TOW - && delayMs < 80 - && message.tAcc < (80 * 1000 * 1000 /* 80ms */) + && delayMs < 20 + && message.tAcc < (20 * 1000 * 1000 /* 20ms */) && (mLastTimeTimeSet == 0 || (mLastTimeTimeSet + (2 * 60 * 1000 /* 2 minutes */)) < receivedMs)) { String oldTime = TimeUtils::dateTimeToString(); @@ -1063,7 +1072,8 @@ void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uin if (oldTime != newTime) { log_i("TIMEGPS set: %s -> %s", oldTime.c_str(), newTime.c_str()); addStatisticsMessage(String("TIMEGPS set: ") - + oldTime + " -> " + newTime + " " + String(delayMs) + "ms."); + + oldTime + " -> " + newTime + " delay " + String(delayMs) + + "ms. tAcc:" + String(message.tAcc) + "ns"); } if (mLastTimeTimeSet == 0) { mLastTimeTimeSet = receivedMs; diff --git a/src/gps.h b/src/gps.h index 5d6b5b0d..2ddee3a9 100644 --- a/src/gps.h +++ b/src/gps.h @@ -68,6 +68,8 @@ class Gps { /* Collected informational messages as String. */ String getMessages() const; + String popMessage(); + /* Clears the collected informational messages. */ void resetMessages(); diff --git a/src/writer.cpp b/src/writer.cpp index b60ae4a5..8ff59a4f 100644 --- a/src/writer.cpp +++ b/src/writer.cpp @@ -275,7 +275,7 @@ bool CSVFileWriter::append(DataSet &set) { csv += "DEV: Right interrupt adjusted : "; csv += sensorManager->getNumberOfInterruptAdjustments(RIGHT_SENSOR_ID); } else if (time.tm_sec >= 20 && time.tm_sec < 40) { - String msg = gps.getMessage(time.tm_sec - 20); + String msg = gps.popMessage(); if (!msg.isEmpty()) { csv += "DEV: GPS: "; csv += ObsUtils::encodeForCsvField(msg); @@ -283,6 +283,9 @@ bool CSVFileWriter::append(DataSet &set) { } else if (time.tm_sec == 40) { csv += "DBG GPS Time: " + TimeUtils::dateTimeToString(TimeUtils::toTime(set.gpsRecord.getWeek(), set.gpsRecord.getTow() / 1000)); + } else if (time.tm_sec == 41) { + csv += "DBG CPU Time: " + + TimeUtils::dateTimeToString(); } // #endif csv += ";";