Skip to content

Commit

Permalink
Allow debugging of GPS vs location drift and GPS week handling (#276 / …
Browse files Browse the repository at this point in the history 
…#294)

* Allow debugging of GPS vs location drift
- store gps timestamp in gps record
- log gps timestamp and cpu time every minute to make derivations visible
- read week number also from NAV-SOL message.
- report week changes via statistic message
- use GPS time directly for csv if available
- wait for more precise data till time is set (80 vs 20 ms)
- gps logging, log each message only once
  • Loading branch information
@amandel
amandel authored Apr 17, 2022
1 parent 530ce1b commit 9ab7c9e
Show file tree
Hide file tree
Showing 5 changed files with 77 additions and 9 deletions.
46 changes: 40 additions & 6 deletions src/gps.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down Expand Up @@ -872,7 +881,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();
}
Expand All @@ -882,6 +891,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();
Expand Down Expand Up @@ -1027,11 +1045,25 @@ 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 ? " 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
&& 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();
Expand All @@ -1040,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;
Expand Down Expand Up @@ -1136,6 +1169,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);
Expand Down
4 changes: 4 additions & 0 deletions src/gps.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,8 @@ class Gps {
/* Collected informational messages as String. */
String getMessages() const;

String popMessage();

/* Clears the collected informational messages. */
void resetMessages();

Expand Down Expand Up @@ -486,6 +488,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. */
Expand Down
12 changes: 12 additions & 0 deletions src/gpsrecord.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down
5 changes: 4 additions & 1 deletion src/gpsrecord.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@

#include <Arduino.h>
#include <cstdint>
#include <ctime>

class Gps;

Expand Down Expand Up @@ -57,13 +56,16 @@ 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 */
void reset();

/* 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);

Expand All @@ -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 */
Expand Down
19 changes: 17 additions & 2 deletions src/writer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
* see <http://www.gnu.org/licenses/>.
*/

#include <utils/timeutils.h>
#include "writer.h"
#include "utils/file.h"

Expand Down Expand Up @@ -191,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;",
Expand Down Expand Up @@ -266,11 +275,17 @@ 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);
}
} 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 += ";";
Expand Down

0 comments on commit 9ab7c9e

Please sign in to comment.