From 2391982c1d9cc96c8baa19e77cea95093a007779 Mon Sep 17 00:00:00 2001 From: Tom Fifield Date: Tue, 4 Mar 2025 23:47:06 +0800 Subject: [PATCH] Only call GPS Probe commands once per family (#6114) In the GPS probe code we write commands on the serial line and determine which GPS we have based on the result. GPS units in the same family sometimes use the same command, but return different results (eg AG3335 and AG3332 both use $PAIR021*39). Currently we run the command once per GPS. Instead we should run each command only once per family, record the result, and select the GNSS MODEL based on the result, which is what this patch does. Before the change, we put 12 commands on the serial bus. Now we only put 6. This should markedly improve the speed and reliability of GPS detection. Fixes https://github.com/meshtastic/firmware/issues/5193 Co-authored-by: Ben Meadors --- src/gps/GPS.cpp | 82 +++++++++++++++++++++++++++++++++++++++---------- src/gps/GPS.h | 7 +++++ 2 files changed, 73 insertions(+), 16 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 2989a59bd..7dcb77fcc 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -1,3 +1,7 @@ +#include // Include for strstr +#include +#include + #include "configuration.h" #if !MESHTASTIC_EXCLUDE_GPS #include "Default.h" @@ -1117,7 +1121,7 @@ int GPS::prepareDeepSleep(void *unused) } static const char *PROBE_MESSAGE = "Trying %s (%s)..."; -static const char *DETECTED_MESSAGE = "%s detected, using %s Module"; +static const char *DETECTED_MESSAGE = "%s detected"; #define PROBE_SIMPLE(CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...) \ do { \ @@ -1125,11 +1129,22 @@ static const char *DETECTED_MESSAGE = "%s detected, using %s Module"; clearBuffer(); \ _serial_gps->write(TOWRITE "\r\n"); \ if (getACK(RESPONSE, TIMEOUT) == GNSS_RESPONSE_OK) { \ - LOG_INFO(DETECTED_MESSAGE, CHIP, #DRIVER); \ + LOG_INFO(DETECTED_MESSAGE, CHIP); \ return DRIVER; \ } \ } while (0) +#define PROBE_FAMILY(FAMILY_NAME, COMMAND, RESPONSE_MAP, TIMEOUT) \ + do { \ + LOG_DEBUG(PROBE_MESSAGE, COMMAND, FAMILY_NAME); \ + clearBuffer(); \ + _serial_gps->write(COMMAND "\r\n"); \ + GnssModel_t detectedDriver = getProbeResponse(TIMEOUT, RESPONSE_MAP); \ + if (detectedDriver != GNSS_MODEL_UNKNOWN) { \ + return detectedDriver; \ + } \ + } while (0) + GnssModel_t GPS::probe(int serialSpeed) { #if defined(ARCH_NRF52) || defined(ARCH_PORTDUINO) || defined(ARCH_STM32WL) @@ -1160,31 +1175,34 @@ GnssModel_t GPS::probe(int serialSpeed) delay(20); // Unicore UFirebirdII Series: UC6580, UM620, UM621, UM670A, UM680A, or UM681A - PROBE_SIMPLE("UC6580", "$PDTINFO", "UC6580", GNSS_MODEL_UC6580, 500); - PROBE_SIMPLE("UM600", "$PDTINFO", "UM600", GNSS_MODEL_UC6580, 500); - PROBE_SIMPLE("ATGM336H", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM336H", GNSS_MODEL_ATGM336H, 500); - /* ATGM332D series (-11(GPS), -21(BDS), -31(GPS+BDS), -51(GPS+GLONASS), -71-0(GPS+BDS+GLONASS)) - based on AT6558 */ - PROBE_SIMPLE("ATGM332D", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM332D", GNSS_MODEL_ATGM336H, 500); + std::vector unicore = {{"UC6580", "UC6580", GNSS_MODEL_UC6580}, {"UM600", "UM600", GNSS_MODEL_UC6580}}; + PROBE_FAMILY("Unicore Family", "$PDTINFO", unicore, 500); + + std::vector atgm = { + {"ATGM336H", "$GPTXT,01,01,02,HW=ATGM336H", GNSS_MODEL_ATGM336H}, + /* ATGM332D series (-11(GPS), -21(BDS), -31(GPS+BDS), -51(GPS+GLONASS), -71-0(GPS+BDS+GLONASS)) based on AT6558 */ + {"ATGM332D", "$GPTXT,01,01,02,HW=ATGM332D", GNSS_MODEL_ATGM336H}}; + PROBE_FAMILY("ATGM33xx Family", "$PCAS06,1*1A", atgm, 500); /* Airoha (Mediatek) AG3335A/M/S, A3352Q, Quectel L89 2.0, SimCom SIM65M */ _serial_gps->write("$PAIR062,2,0*3C\r\n"); // GSA OFF to reduce volume _serial_gps->write("$PAIR062,3,0*3D\r\n"); // GSV OFF to reduce volume _serial_gps->write("$PAIR513*3D\r\n"); // save configuration - PROBE_SIMPLE("AG3335", "$PAIR021*39", "$PAIR021,AG3335", GNSS_MODEL_AG3335, 500); - PROBE_SIMPLE("AG3352", "$PAIR021*39", "$PAIR021,AG3352", GNSS_MODEL_AG3352, 500); - PROBE_SIMPLE("LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GNSS_MODEL_AG3352, 500); + std::vector airoha = {{"AG3335", "$PAIR021,AG3335", GNSS_MODEL_AG3335}, + {"AG3352", "$PAIR021,AG3352", GNSS_MODEL_AG3352}, + {"RYS3520", "$PAIR021,REYAX_RYS3520_V2", GNSS_MODEL_AG3352}}; + PROBE_FAMILY("Airoha Family", "$PAIR021*39", airoha, 1000); + PROBE_SIMPLE("LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GNSS_MODEL_AG3352, 500); PROBE_SIMPLE("L76K", "$PCAS06,0*1B", "$GPTXT,01,01,02,SW=", GNSS_MODEL_MTK, 500); // Close all NMEA sentences, valid for L76B MTK platform (Waveshare Pico GPS) _serial_gps->write("$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n"); delay(20); - - PROBE_SIMPLE("L76B", "$PMTK605*31", "Quectel-L76B", GNSS_MODEL_MTK_L76B, 500); - PROBE_SIMPLE("PA1616S", "$PMTK605*31", "1616S", GNSS_MODEL_MTK_PA1616S, 500); - - PROBE_SIMPLE("LS20031", "$PMTK605*31", "MC-1513", GNSS_MODEL_LS20031, 500); + std::vector mtk = {{"L76B", "Quectel-L76B", GNSS_MODEL_MTK_L76B}, + {"PA1616S", "1616S", GNSS_MODEL_MTK_PA1616S}, + {"LS20031", "MC-1513", GNSS_MODEL_LS20031}}; + PROBE_FAMILY("MTK Family", "$PMTK605*31", mtk, 500); uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00}; UBXChecksum(cfg_rate, sizeof(cfg_rate)); @@ -1281,6 +1299,38 @@ GnssModel_t GPS::probe(int serialSpeed) return GNSS_MODEL_UNKNOWN; } +GnssModel_t GPS::getProbeResponse(unsigned long timeout, const std::vector &responseMap) +{ + String response = ""; + unsigned long start = millis(); + while (millis() - start < timeout) { + if (_serial_gps->available()) { + response += (char)_serial_gps->read(); + + if (response.endsWith(",") || response.endsWith("\r\n")) { +#ifdef GPS_DEBUG + LOG_DEBUG(response.c_str()); +#endif + // check if we can see our chips + for (const auto &chipInfo : responseMap) { + if (strstr(response.c_str(), chipInfo.detectionString.c_str()) != nullptr) { + LOG_INFO("%s detected", chipInfo.chipName.c_str()); + return chipInfo.driver; + } + } + } + if (response.endsWith("\r\n")) { + response.trim(); + response = ""; // Reset the response string for the next potential message + } + } + } +#ifdef GPS_DEBUG + LOG_DEBUG(response.c_str()); +#endif + return GNSS_MODEL_UNKNOWN; // Return empty string on timeout +} + GPS *GPS::createGps() { int8_t _rx_gpio = config.position.rx_gpio; diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 01a4fe745..240cf66d2 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -48,6 +48,11 @@ enum GPSPowerState : uint8_t { GPS_OFF // Powered off indefinitely }; +struct ChipInfo { + String chipName; // The name of the chip (for logging) + String detectionString; // The string to match in the response + GnssModel_t driver; // The driver to use +}; /** * A gps class that only reads from the GPS periodically and keeps the gps powered down except when reading * @@ -230,6 +235,8 @@ class GPS : private concurrency::OSThread virtual int32_t runOnce() override; + GnssModel_t getProbeResponse(unsigned long timeout, const std::vector &responseMap); + // Get GNSS model GnssModel_t probe(int serialSpeed);