diff --git a/exercises/18_GPS_Field_QA/lib/field_qa/Config.h b/exercises/18_GPS_Field_QA/lib/field_qa/Config.h index 3546029..04232f9 100644 --- a/exercises/18_GPS_Field_QA/lib/field_qa/Config.h +++ b/exercises/18_GPS_Field_QA/lib/field_qa/Config.h @@ -50,6 +50,10 @@ #define LOG_AP_IP_OCTET 23 #endif +#ifndef GNSS_DIAG +#define GNSS_DIAG 0 +#endif + #define FIELD_QA_STR_INNER(x) #x #define FIELD_QA_STR(x) FIELD_QA_STR_INNER(x) diff --git a/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.cpp b/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.cpp index 2269bd4..4b37985 100644 --- a/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.cpp +++ b/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include "Config.h" @@ -57,6 +58,20 @@ static void copyTalker(const char* header, char* out) { out[2] = '\0'; } +static void gnssDiagf(const char* fmt, ...) { +#if GNSS_DIAG + char buf[192]; + va_list ap; + va_start(ap, fmt); + vsnprintf(buf, sizeof(buf), fmt, ap); + va_end(ap); + Serial.print("[GNSS] "); + Serial.println(buf); +#else + (void)fmt; +#endif +} + } // namespace void GnssManager::begin() { @@ -196,6 +211,13 @@ void GnssManager::parseGsa(char* fields[], int count) { if (count < 18) { return; } + const uint32_t now = millis(); + if ((uint32_t)(now - m_lastGsaMs) > 1500U) { + m_usedPrnCount = 0; + memset(m_usedPrns, 0, sizeof(m_usedPrns)); + } + m_lastGsaMs = now; + gnssDiagf("GSA %s", fields[0] ? fields[0] : "?"); const int dim = atoi(fields[2]); m_state.fixDimension = dim; if (count > 15 && fields[15] && fields[15][0]) { @@ -212,13 +234,10 @@ void GnssManager::parseGsa(char* fields[], int count) { } int satsUsed = 0; - m_usedPrnCount = 0; for (int i = 3; i <= 14 && i < count; ++i) { if (fields[i] && fields[i][0]) { ++satsUsed; - if (m_usedPrnCount < sizeof(m_usedPrns) / sizeof(m_usedPrns[0])) { - m_usedPrns[m_usedPrnCount++] = (uint8_t)atoi(fields[i]); - } + addUsedPrn((uint8_t)atoi(fields[i])); } } if (satsUsed > 0) { @@ -247,6 +266,36 @@ void GnssManager::clearSatelliteView() { m_state.maxSnr = 0; } +void GnssManager::clearTalkerSatellites(const char* talker) { + if (!talker || !talker[0]) { + return; + } + + size_t write = 0; + size_t removed = 0; + for (size_t read = 0; read < m_satCount; ++read) { + SatelliteInfo sat = m_satellites[read]; + if (sat.valid && strcmp(sat.talker, talker) == 0) { + ++removed; + continue; + } + if (write != read) { + m_satellites[write] = sat; + } + ++write; + } + for (size_t i = write; i < m_satCount; ++i) { + m_satellites[i] = SatelliteInfo{}; + } + if (removed > 0) { + gnssDiagf("clear talker=%s removed=%u remaining=%u", + talker, + (unsigned)removed, + (unsigned)write); + } + m_satCount = write; +} + void GnssManager::finalizeSatelliteStats() { uint32_t snrSum = 0; uint32_t snrCount = 0; @@ -282,6 +331,20 @@ void GnssManager::finalizeSatelliteStats() { m_state.meanSnr = snrCount > 0 ? ((float)snrSum / (float)snrCount) : -1.0f; } +void GnssManager::addUsedPrn(uint8_t prn) { + if (prn == 0) { + return; + } + for (size_t i = 0; i < m_usedPrnCount; ++i) { + if (m_usedPrns[i] == prn) { + return; + } + } + if (m_usedPrnCount < sizeof(m_usedPrns) / sizeof(m_usedPrns[0])) { + m_usedPrns[m_usedPrnCount++] = prn; + } +} + void GnssManager::parseGsv(char* fields[], int count) { if (count < 4) { return; @@ -290,7 +353,14 @@ void GnssManager::parseGsv(char* fields[], int count) { const int msgNum = atoi(fields[2]); const int satsInView = atoi(fields[3]); if (msgNum == 1) { - clearSatelliteView(); + char talker[3]; + copyTalker(fields[0], talker); + gnssDiagf("GSV %s %d/%d sats_in_view=%d", talker, msgNum, totalMsgs, satsInView); + clearTalkerSatellites(talker); + } else { + char talker[3]; + copyTalker(fields[0], talker); + gnssDiagf("GSV %s %d/%d sats_in_view=%d", talker, msgNum, totalMsgs, satsInView); } if (satsInView >= 0) { m_state.satsInView = satsInView; @@ -393,6 +463,9 @@ void GnssManager::processNmeaLine(char* line) { } m_sawSentence = true; m_state.sawSentence = true; + if (strstr(line, "GSV") || strstr(line, "GSA")) { + gnssDiagf("raw %s", line); + } char* star = strchr(line, '*'); if (star) { *star = '\0'; diff --git a/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.h b/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.h index 8b3d204..0c5a94f 100644 --- a/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.h +++ b/exercises/18_GPS_Field_QA/lib/field_qa/GnssManager.h @@ -30,7 +30,9 @@ class GnssManager { static bool parseUInt2(const char* s, uint8_t& out); static double parseNmeaCoord(const char* value, const char* hemi); void clearSatelliteView(); + void clearTalkerSatellites(const char* talker); void finalizeSatelliteStats(); + void addUsedPrn(uint8_t prn); bool prnUsedInSolution(uint8_t prn) const; HardwareSerial m_serial{1}; @@ -43,9 +45,10 @@ class GnssManager { GnssSample m_state; SatelliteInfo m_satellites[kMaxSatellites]; - uint8_t m_usedPrns[16] = {0}; + uint8_t m_usedPrns[kMaxSatellites] = {0}; size_t m_usedPrnCount = 0; size_t m_satCount = 0; + uint32_t m_lastGsaMs = 0; uint32_t m_lastGsvMs = 0; uint32_t m_lastFixMs = 0; uint32_t m_bootMs = 0; diff --git a/exercises/18_GPS_Field_QA/src/main.cpp b/exercises/18_GPS_Field_QA/src/main.cpp index 5c7b179..15a2c20 100644 --- a/exercises/18_GPS_Field_QA/src/main.cpp +++ b/exercises/18_GPS_Field_QA/src/main.cpp @@ -83,6 +83,42 @@ void IRAM_ATTR onPpsEdge() { String htmlEscape(const String& in); +void logGnssTalkerCounts(uint32_t sampleSeq, const SatelliteInfo* sats, size_t satCount) { +#if GNSS_DIAG + size_t gp = 0, ga = 0, gl = 0, gb = 0, gi = 0, gq = 0, gs = 0, gn = 0, other = 0; + for (size_t i = 0; i < satCount; ++i) { + if (!sats[i].valid) { + continue; + } + if (strcmp(sats[i].talker, "GP") == 0) ++gp; + else if (strcmp(sats[i].talker, "GA") == 0) ++ga; + else if (strcmp(sats[i].talker, "GL") == 0) ++gl; + else if (strcmp(sats[i].talker, "GB") == 0 || strcmp(sats[i].talker, "BD") == 0) ++gb; + else if (strcmp(sats[i].talker, "GI") == 0) ++gi; + else if (strcmp(sats[i].talker, "GQ") == 0) ++gq; + else if (strcmp(sats[i].talker, "GS") == 0) ++gs; + else if (strcmp(sats[i].talker, "GN") == 0) ++gn; + else ++other; + } + Serial.printf("[GNSS] flush sample=%lu satCount=%u GP=%u GA=%u GL=%u GB=%u GI=%u GQ=%u GS=%u GN=%u other=%u\n", + (unsigned long)sampleSeq, + (unsigned)satCount, + (unsigned)gp, + (unsigned)ga, + (unsigned)gl, + (unsigned)gb, + (unsigned)gi, + (unsigned)gq, + (unsigned)gs, + (unsigned)gn, + (unsigned)other); +#else + (void)sampleSeq; + (void)sats; + (void)satCount; +#endif +} + void recordEvent(const char* fmt, ...) { char msg[72]; va_list args; @@ -439,6 +475,7 @@ void sampleAndMaybeLog() { const uint32_t msSinceRunStart = millis() - g_runStartMs; SatelliteInfo sats[kMaxSatellites]; const size_t satCount = g_gnss.copySatellites(sats, kMaxSatellites); + logGnssTalkerCounts(sampleSeq, sats, satCount); if (!g_storage.appendSampleCsv(sample, sampleSeq, msSinceRunStart, g_runId, g_bootTimestampUtc)) { stopLoggingForStorageFailure("append_sample"); } else if (satCount > 0 &&