Coordinates and altitude captured along with build version. Sample log:

20260217_212053_z        set RTC to GPS using 1PPS pulse-per-second discipline  rtc-gps drift=+0 s; sats=20; lat=44.936488 lon=-123.021837; alt_m=59.1; hdop=0.7; utc_age_ms=659; pps_edges=805; fw_epoch=1771362221; fw_build_utc=20260217_210341_z
This commit is contained in:
John Poole 2026-02-17 13:24:28 -08:00
commit c324998ef0
3 changed files with 169 additions and 6 deletions

View file

@ -10,6 +10,7 @@ platform = espressif32
framework = arduino framework = arduino
board = esp32-s3-devkitc-1 board = esp32-s3-devkitc-1
monitor_speed = 115200 monitor_speed = 115200
extra_scripts = pre:scripts/set_build_epoch.py
lib_deps = lib_deps =
lewisxhe/XPowersLib@0.3.3 lewisxhe/XPowersLib@0.3.3
Wire Wire

View file

@ -0,0 +1,12 @@
import time
Import("env")
epoch = int(time.time())
utc_tag = time.strftime("%Y%m%d_%H%M%S_z", time.gmtime(epoch))
env.Append(
CPPDEFINES=[
("FW_BUILD_EPOCH", str(epoch)),
("FW_BUILD_UTC", '\\"%s\\"' % utc_tag),
]
)

View file

@ -34,6 +34,14 @@
#define FILE_APPEND FILE_WRITE #define FILE_APPEND FILE_WRITE
#endif #endif
#ifndef FW_BUILD_EPOCH
#define FW_BUILD_EPOCH 0
#endif
#ifndef FW_BUILD_UTC
#define FW_BUILD_UTC "unknown"
#endif
static const uint32_t kSerialDelayMs = 5000; static const uint32_t kSerialDelayMs = 5000;
static const uint32_t kLoopMsDiscipline = 60000; static const uint32_t kLoopMsDiscipline = 60000;
static const uint32_t kNoTimeDelayMs = 30000; static const uint32_t kNoTimeDelayMs = 30000;
@ -66,8 +74,14 @@ struct DateTime {
struct GpsState { struct GpsState {
bool sawAnySentence = false; bool sawAnySentence = false;
bool hasValidUtc = false; bool hasValidUtc = false;
bool hasValidPosition = false;
bool hasValidAltitude = false;
uint8_t satsUsed = 0; uint8_t satsUsed = 0;
uint8_t satsInView = 0; uint8_t satsInView = 0;
float hdop = -1.0f;
float altitudeM = 0.0f;
double latitudeDeg = 0.0;
double longitudeDeg = 0.0;
DateTime utc{}; DateTime utc{};
uint32_t lastUtcMs = 0; uint32_t lastUtcMs = 0;
}; };
@ -269,14 +283,76 @@ static bool parseUInt2(const char* s, uint8_t& out) {
return true; return true;
} }
static bool parseNmeaCoordToDecimal(const char* raw, const char* hemi, bool isLat, double& outDeg) {
if (!raw || !hemi || raw[0] == '\0' || hemi[0] == '\0') {
return false;
}
// NMEA uses ddmm.mmmm (lat) and dddmm.mmmm (lon), with leading zeros preserved.
// Parse from string slices so longitudes like 071xx.xxxx do not collapse to 7xx.xxxx.
int degDigits = isLat ? 2 : 3;
size_t n = strlen(raw);
if (n <= (size_t)degDigits + 2) {
return false;
}
for (int i = 0; i < degDigits; ++i) {
if (!isdigit((unsigned char)raw[i])) {
return false;
}
}
char degBuf[4] = {0};
memcpy(degBuf, raw, degDigits);
int deg = atoi(degBuf);
const char* minPtr = raw + degDigits;
double minutes = atof(minPtr);
if (minutes < 0.0 || minutes >= 60.0) {
return false;
}
double dec = (double)deg + (minutes / 60.0);
char h = (char)toupper((unsigned char)hemi[0]);
if (h == 'S' || h == 'W') {
dec = -dec;
} else if (h != 'N' && h != 'E') {
return false;
}
outDeg = dec;
return true;
}
static void parseGga(char* fields[], int count) { static void parseGga(char* fields[], int count) {
if (count <= 7) { if (count <= 7) {
return; return;
} }
const char* latRaw = (count > 2) ? fields[2] : nullptr;
const char* latHem = (count > 3) ? fields[3] : nullptr;
const char* lonRaw = (count > 4) ? fields[4] : nullptr;
const char* lonHem = (count > 5) ? fields[5] : nullptr;
int sats = atoi(fields[7]); int sats = atoi(fields[7]);
if (sats >= 0 && sats <= 255) { if (sats >= 0 && sats <= 255) {
g_gps.satsUsed = (uint8_t)sats; g_gps.satsUsed = (uint8_t)sats;
} }
if (count > 8 && fields[8] && fields[8][0] != '\0') {
g_gps.hdop = (float)atof(fields[8]);
}
if (count > 9 && fields[9] && fields[9][0] != '\0') {
g_gps.altitudeM = (float)atof(fields[9]);
g_gps.hasValidAltitude = true;
}
// Position fallback from GGA so we still log coordinates if RMC position is missing.
double lat = 0.0;
double lon = 0.0;
if (parseNmeaCoordToDecimal(latRaw, latHem, true, lat) &&
parseNmeaCoordToDecimal(lonRaw, lonHem, false, lon)) {
g_gps.latitudeDeg = lat;
g_gps.longitudeDeg = lon;
g_gps.hasValidPosition = true;
}
} }
static void parseGsv(char* fields[], int count) { static void parseGsv(char* fields[], int count) {
@ -296,6 +372,10 @@ static void parseRmc(char* fields[], int count) {
const char* utc = fields[1]; const char* utc = fields[1];
const char* status = fields[2]; const char* status = fields[2];
const char* latRaw = fields[3];
const char* latHem = fields[4];
const char* lonRaw = fields[5];
const char* lonHem = fields[6];
const char* date = fields[9]; const char* date = fields[9];
if (!status || status[0] != 'A') { if (!status || status[0] != 'A') {
@ -323,6 +403,15 @@ static void parseRmc(char* fields[], int count) {
g_gps.utc.year = (uint16_t)(2000U + yy); g_gps.utc.year = (uint16_t)(2000U + yy);
g_gps.hasValidUtc = true; g_gps.hasValidUtc = true;
g_gps.lastUtcMs = millis(); g_gps.lastUtcMs = millis();
double lat = 0.0;
double lon = 0.0;
if (parseNmeaCoordToDecimal(latRaw, latHem, true, lat) &&
parseNmeaCoordToDecimal(lonRaw, lonHem, false, lon)) {
g_gps.latitudeDeg = lat;
g_gps.longitudeDeg = lon;
g_gps.hasValidPosition = true;
}
} }
static void processNmeaLine(char* line) { static void processNmeaLine(char* line) {
@ -448,7 +537,14 @@ static bool ensureGpsLogPathReady() {
return true; return true;
} }
static bool appendDisciplineLog(const DateTime& gpsUtc, int64_t rtcMinusGpsSeconds) { static bool appendDisciplineLog(const DateTime& gpsUtc,
bool havePriorRtc,
int64_t rtcMinusGpsSeconds,
uint8_t sats,
uint32_t utcAgeMs,
uint32_t ppsEdges,
char* outTs,
size_t outTsLen) {
if (!ensureGpsLogPathReady()) { if (!ensureGpsLogPathReady()) {
logf("SD not mounted, skipping append to gps/discipline_rtc.log"); logf("SD not mounted, skipping append to gps/discipline_rtc.log");
return false; return false;
@ -462,13 +558,54 @@ static bool appendDisciplineLog(const DateTime& gpsUtc, int64_t rtcMinusGpsSecon
char ts[32]; char ts[32];
formatUtcCompact(gpsUtc, ts, sizeof(ts)); formatUtcCompact(gpsUtc, ts, sizeof(ts));
if (outTs && outTsLen > 0) {
snprintf(outTs, outTsLen, "%s", ts);
}
char line[220]; char drift[40];
if (havePriorRtc) {
snprintf(drift, sizeof(drift), "%+lld s", (long long)rtcMinusGpsSeconds);
} else {
snprintf(drift, sizeof(drift), "RTC_unset");
}
char pos[64];
if (g_gps.hasValidPosition) {
snprintf(pos, sizeof(pos), "lat=%.6f lon=%.6f", g_gps.latitudeDeg, g_gps.longitudeDeg);
} else {
snprintf(pos, sizeof(pos), "lat=NA lon=NA");
}
char hdop[16];
if (g_gps.hdop > 0.0f) {
snprintf(hdop, sizeof(hdop), "%.1f", g_gps.hdop);
} else {
snprintf(hdop, sizeof(hdop), "NA");
}
char alt[16];
if (g_gps.hasValidAltitude) {
snprintf(alt, sizeof(alt), "%.1f", g_gps.altitudeM);
} else {
snprintf(alt, sizeof(alt), "NA");
}
char line[320];
snprintf(line, snprintf(line,
sizeof(line), sizeof(line),
"%s\t set RTC to GPS using 1PPS pulse-per-second discipline\trtc-gps drift=%+lld s", "%s\t set RTC to GPS using 1PPS pulse-per-second discipline\t"
"rtc-gps drift=%s; sats=%u; %s; alt_m=%s; hdop=%s; utc_age_ms=%lu; pps_edges=%lu; "
"fw_epoch=%lu; fw_build_utc=%s",
ts, ts,
(long long)rtcMinusGpsSeconds); drift,
(unsigned)sats,
pos,
alt,
hdop,
(unsigned long)utcAgeMs,
(unsigned long)ppsEdges,
(unsigned long)FW_BUILD_EPOCH,
FW_BUILD_UTC);
size_t wrote = f.println(line); size_t wrote = f.println(line);
f.close(); f.close();
@ -553,18 +690,31 @@ static bool disciplineRtcToGps() {
driftSec = toEpochSeconds(priorRtc) - toEpochSeconds(target); driftSec = toEpochSeconds(priorRtc) - toEpochSeconds(target);
} }
(void)appendDisciplineLog(target, driftSec); uint8_t sats = bestSatelliteCount();
uint32_t utcAgeMs = (uint32_t)(millis() - g_gps.lastUtcMs);
uint32_t ppsEdges = g_ppsEdgeCount;
char tsCompact[32];
bool logOk = appendDisciplineLog(target,
havePriorRtc,
driftSec,
sats,
utcAgeMs,
ppsEdges,
tsCompact,
sizeof(tsCompact));
char utcLine[36]; char utcLine[36];
char driftLine[36]; char driftLine[36];
char logLine[36];
formatUtcHuman(target, utcLine, sizeof(utcLine)); formatUtcHuman(target, utcLine, sizeof(utcLine));
if (havePriorRtc) { if (havePriorRtc) {
snprintf(driftLine, sizeof(driftLine), "rtc-gps drift: %+lld s", (long long)driftSec); snprintf(driftLine, sizeof(driftLine), "rtc-gps drift: %+lld s", (long long)driftSec);
} else { } else {
snprintf(driftLine, sizeof(driftLine), "rtc-gps drift: RTC_unset"); snprintf(driftLine, sizeof(driftLine), "rtc-gps drift: RTC_unset");
} }
snprintf(logLine, sizeof(logLine), "Log:%s sats:%u", logOk ? "ok" : "fail", (unsigned)sats);
oledShowLines("RTC disciplined to GPS", utcLine, "Method: 1PPS", driftLine); oledShowLines("RTC disciplined to GPS", utcLine, driftLine, logLine, tsCompact);
logf("RTC disciplined to GPS with 1PPS. %s drift=%+llds lowV=%s", logf("RTC disciplined to GPS with 1PPS. %s drift=%+llds lowV=%s",
utcLine, utcLine,