Skip to content

Commit 9ab7c9e

Browse files
amandelamandel
authored
Allow debugging of GPS vs location drift and GPS week handling (#276 / #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
1 parent 530ce1b commit 9ab7c9e

File tree

5 files changed

+77
-9
lines changed

5 files changed

+77
-9
lines changed

‎src/gps.cpp‎

Lines changed: 40 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -576,6 +576,15 @@ String Gps::getMessages() const {
576576
return theGpsMessage;
577577
}
578578

579+
String Gps::popMessage() {
580+
String theGpsMessage = "";
581+
if (mMessages.size() > 0) {
582+
theGpsMessage = mMessages[0];
583+
mMessages.erase(mMessages.begin());
584+
}
585+
return theGpsMessage;
586+
}
587+
579588
String Gps::getMessage(uint16_t idx) const {
580589
String theGpsMessage = "";
581590
if (mMessages.size() > idx) {
@@ -872,7 +881,7 @@ void Gps::parseUbxMessage() {
872881
mGpsBuffer.navDop.iTow, mGpsBuffer.navDop.gDop, mGpsBuffer.navDop.pDop,
873882
mGpsBuffer.navDop.tDop, mGpsBuffer.navDop.vDop, mGpsBuffer.navDop.hDop,
874883
mGpsBuffer.navDop.nDop, mGpsBuffer.navDop.eDop);
875-
if (prepareGpsData(mGpsBuffer.navSol.iTow)) {
884+
if (prepareGpsData(mGpsBuffer.navDop.iTow)) {
876885
mIncomingGpsRecord.setHdop(mGpsBuffer.navDop.hDop);
877886
checkGpsDataState();
878887
}
@@ -882,6 +891,15 @@ void Gps::parseUbxMessage() {
882891
log_v("SOL: iTOW: %u, gpsFix: %d, flags: %02x, numSV: %d, pDop: %04d.",
883892
mGpsBuffer.navSol.iTow, mGpsBuffer.navSol.gpsFix, mGpsBuffer.navSol.flags,
884893
mGpsBuffer.navSol.numSv, mGpsBuffer.navSol.pDop);
894+
if (mGpsBuffer.navSol.flags & 4) { // WKNSET
895+
if (mLastGpsWeek != mGpsBuffer.navSol.week) {
896+
// debugging #294
897+
addStatisticsMessage(String("NAVSOL gps week changed: ")
898+
+ mLastGpsWeek + " -> " + mGpsBuffer.navSol.week
899+
+ " at " + TimeUtils::dateTimeToString());
900+
}
901+
mLastGpsWeek = mGpsBuffer.navSol.week;
902+
}
885903
if (prepareGpsData(mGpsBuffer.navSol.iTow)) {
886904
mIncomingGpsRecord.setInfo(mGpsBuffer.navSol.numSv, mGpsBuffer.navSol.gpsFix, mGpsBuffer.navSol.flags);
887905
checkGpsDataState();
@@ -1027,11 +1045,25 @@ void Gps::parseUbxMessage() {
10271045
}
10281046

10291047
void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uint32_t receivedMs, const uint32_t delayMs) {
1030-
log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x, tAcc %dns, delay %dms",
1031-
message.iTow, message.fTow, message.week, message.leapS, message.valid, message.tAcc, delayMs);
1048+
log_i("TIMEGPS: iTOW: %u, fTOW: %d, week %d, leapS: %d, valid: 0x%02x (%s%s%s), tAcc %uns, DATE: %s, delay %dms",
1049+
message.iTow, message.fTow, message.week, message.leapS, message.valid,
1050+
message.valid & 1 ? "TOW" : "",
1051+
message.valid & 2 ? " WEEK" : "",
1052+
message.valid & 4 ? " UTC" : "",
1053+
message.tAcc,
1054+
TimeUtils::dateTimeToString(TimeUtils::toTime(message.week, message.iTow / 1000)).c_str(),
1055+
delayMs);
1056+
if (message.valid & 2) {
1057+
if (mLastGpsWeek != message.week) {
1058+
// debugging #294
1059+
addStatisticsMessage(String("TIMEGPS gps week changed: ")
1060+
+ mLastGpsWeek + " -> " + message.week);
1061+
}
1062+
mLastGpsWeek = message.week;
1063+
}
10321064
if ((message.valid & 0x03) == 0x03 // WEEK && TOW
1033-
&& delayMs < 80
1034-
&& message.tAcc < (80 * 1000 * 1000 /* 80ms */)
1065+
&& delayMs < 20
1066+
&& message.tAcc < (20 * 1000 * 1000 /* 20ms */)
10351067
&& (mLastTimeTimeSet == 0
10361068
|| (mLastTimeTimeSet + (2 * 60 * 1000 /* 2 minutes */)) < receivedMs)) {
10371069
String oldTime = TimeUtils::dateTimeToString();
@@ -1040,7 +1072,8 @@ void Gps::handleUbxNavTimeGps(const GpsBuffer::UbxNavTimeGps &message, const uin
10401072
if (oldTime != newTime) {
10411073
log_i("TIMEGPS set: %s -> %s", oldTime.c_str(), newTime.c_str());
10421074
addStatisticsMessage(String("TIMEGPS set: ")
1043-
+ oldTime + " -> " + newTime + " " + String(delayMs) + "ms.");
1075+
+ oldTime + " -> " + newTime + " delay " + String(delayMs)
1076+
+ "ms. tAcc:" + String(message.tAcc) + "ns");
10441077
}
10451078
if (mLastTimeTimeSet == 0) {
10461079
mLastTimeTimeSet = receivedMs;
@@ -1136,6 +1169,7 @@ bool Gps::prepareGpsData(uint32_t tow) {
11361169
// fine already prepared
11371170
} else if (mIncomingGpsRecord.mCollectTow == 0) {
11381171
mIncomingGpsRecord.setTow(tow);
1172+
mIncomingGpsRecord.setWeek(mLastGpsWeek);
11391173
} else if ((int32_t) (mIncomingGpsRecord.mCollectTow - tow) > 0) {
11401174
log_e("Data already published: %d",
11411175
mCurrentGpsRecord.mCollectTow);

‎src/gps.h‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -68,6 +68,8 @@ class Gps {
6868
/* Collected informational messages as String. */
6969
String getMessages() const;
7070

71+
String popMessage();
72+
7173
/* Clears the collected informational messages. */
7274
void resetMessages();
7375

@@ -486,6 +488,8 @@ class Gps {
486488
/* last time, when the ESP clock was adjusted to the GPR UTC time,
487489
* in millis ticker. */
488490
uint32_t mLastTimeTimeSet = 0;
491+
/* GPS week as received with time. */
492+
uint32_t mLastGpsWeek = 0;
489493
/* Number of bytes sent for alp request. */
490494
uint32_t mAlpBytesSent = 0;
491495
/* If true a outdated gps cfg was detected. */

‎src/gpsrecord.cpp‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -46,6 +46,10 @@ void GpsRecord::setTow(uint32_t tow) {
4646
mCollectTow = tow;
4747
}
4848

49+
void GpsRecord::setWeek(uint32_t week) {
50+
mCollectWeek = week;
51+
}
52+
4953
void GpsRecord::setPosition(int32_t lon, int32_t lat, int32_t height) {
5054
mLongitude = lon;
5155
mLatitude = lat;
@@ -75,6 +79,14 @@ bool GpsRecord::isAllSet() const {
7579
return mPositionSet && mVelocitySet && mInfoSet && mHdopSet;
7680
}
7781

82+
uint32_t GpsRecord::getTow() const {
83+
return mCollectTow;
84+
}
85+
86+
uint32_t GpsRecord::getWeek() const {
87+
return mCollectWeek;
88+
}
89+
7890
String GpsRecord::getAltitudeMetersString() const {
7991
// 3 digits is problematic, 2 are enough any way.
8092
return toScaledString((mHeight + 5) / 10, 2);

‎src/gpsrecord.h‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,6 @@
2727

2828
#include <Arduino.h>
2929
#include <cstdint>
30-
#include <ctime>
3130

3231
class Gps;
3332

@@ -57,13 +56,16 @@ class GpsRecord {
5756
uint8_t getSatellitesUsed() const;
5857
uint8_t getFixStatusFlags() const;
5958
GPS_FIX getFixStatus() const;
59+
uint32_t getTow() const;
60+
uint32_t getWeek() const;
6061

6162
protected:
6263
/* Clear all collected data */
6364
void reset();
6465

6566
/* Store tow and related date time data. */
6667
void setTow(uint32_t tow);
68+
void setWeek(uint32_t tow);
6769

6870
void setPosition(int32_t lon, int32_t lat, int32_t height);
6971

@@ -80,6 +82,7 @@ class GpsRecord {
8082
* merge records together.
8183
*/
8284
uint32_t mCollectTow = 0;
85+
uint32_t mCollectWeek = 0; // mainly debug
8386
/* deg, scale 1e-7 */
8487
int32_t mLongitude;
8588
/* deg, scale 1e-7 */

‎src/writer.cpp‎

Lines changed: 17 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@
2121
* see <http://www.gnu.org/licenses/>.
2222
*/
2323

24+
#include <utils/timeutils.h>
2425
#include "writer.h"
2526
#include "utils/file.h"
2627

@@ -191,8 +192,16 @@ bool CSVFileWriter::append(DataSet &set) {
191192
return true;
192193
}
193194

195+
time_t theTime;
196+
if (set.gpsRecord.getTow() != 0 && set.gpsRecord.getWeek() != 0) {
197+
theTime = TimeUtils::toTime(set.gpsRecord.getWeek(), set.gpsRecord.getTow() / 1000);
198+
// TODO: Force adjust filename if week changes and set.gpsRecord.getTow() is not small via mFinalFileName = false;
199+
} else {
200+
theTime = set.time;
201+
}
194202
tm time;
195-
localtime_r(&(set.time), &time);
203+
localtime_r(&(theTime), &time);
204+
// localtime_r(&(set.time), &time);
196205
char date[32];
197206
snprintf(date, sizeof(date),
198207
"%02d.%02d.%04d;%02d:%02d:%02d;%u;",
@@ -266,11 +275,17 @@ bool CSVFileWriter::append(DataSet &set) {
266275
csv += "DEV: Right interrupt adjusted : ";
267276
csv += sensorManager->getNumberOfInterruptAdjustments(RIGHT_SENSOR_ID);
268277
} else if (time.tm_sec >= 20 && time.tm_sec < 40) {
269-
String msg = gps.getMessage(time.tm_sec - 20);
278+
String msg = gps.popMessage();
270279
if (!msg.isEmpty()) {
271280
csv += "DEV: GPS: ";
272281
csv += ObsUtils::encodeForCsvField(msg);
273282
}
283+
} else if (time.tm_sec == 40) {
284+
csv += "DBG GPS Time: " +
285+
TimeUtils::dateTimeToString(TimeUtils::toTime(set.gpsRecord.getWeek(), set.gpsRecord.getTow() / 1000));
286+
} else if (time.tm_sec == 41) {
287+
csv += "DBG CPU Time: " +
288+
TimeUtils::dateTimeToString();
274289
}
275290
// #endif
276291
csv += ";";

0 commit comments

Comments
 (0)