Merge pull request #4647 from fifieldt/gps-probe-macro

Simplify GNSS Probe code
meshtastic · Sep 7, 2024 · 56a2e40 · 56a2e40
2 parents c77b89d + ba28ffb
commit 56a2e40
Showing 1 changed file with 19 additions and 57 deletions.
diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp
@@ -1184,6 +1184,15 @@ int GPS::prepareDeepSleep(void *unused)
     return 0;
 }
 
+#define PROBE_SIMPLE(CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...)                                                              \
+    LOG_DEBUG("Trying " TOWRITE " (" CHIP ") ...\n");                                                                            \
+    clearBuffer();                                                                                                               \
+    _serial_gps->write(TOWRITE "\r\n");                                                                                          \
+    if (getACK(RESPONSE, TIMEOUT) == GNSS_RESPONSE_OK) {                                                                         \
+        LOG_INFO(CHIP " detected, using " #DRIVER " Module\n");                                                                  \
+        return DRIVER;                                                                                                           \
+    }
+
 GnssModel_t GPS::probe(int serialSpeed)
 {
 #if defined(ARCH_NRF52) || defined(ARCH_PORTDUINO) || defined(ARCH_RP2040) || defined(ARCH_STM32WL)
@@ -1198,11 +1207,7 @@ GnssModel_t GPS::probe(int serialSpeed)
 #ifdef GNSS_AIROHA
     return GNSS_MODEL_AG3335;
 #endif
-#ifdef GPS_DEBUG
-    for (int i = 0; i < 20; i++) {
-        getACK("$GP", 200);
-    }
-#endif
+
     memset(&info, 0, sizeof(struct uBloxGnssModelInfo));
     uint8_t buffer[768] = {0};
     delay(100);
@@ -1211,67 +1216,24 @@ GnssModel_t GPS::probe(int serialSpeed)
     _serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
     delay(20);
 
-    // get version information from Unicore UFirebirdII Series
-    // Works for: UC6580, UM620, UM621, UM670A, UM680A, or UM681A
-    _serial_gps->write("$PDTINFO\r\n");
-    delay(750);
-    if (getACK("UC6580", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("UC6580 detected, using UC6580 Module\n");
-        return GNSS_MODEL_UC6580;
-    }
-
-    clearBuffer();
-    _serial_gps->write("$PDTINFO\r\n");
-    delay(750);
-    if (getACK("UM600", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("UM600 detected, using UC6580 Module\n");
-        return GNSS_MODEL_UC6580;
-    }
-
-    // Get version information for ATGM336H
-    clearBuffer();
-    _serial_gps->write("$PCAS06,1*1A\r\n");
-    if (getACK("$GPTXT,01,01,02,HW=ATGM336H", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("ATGM336H GNSS init succeeded, using ATGM336H Module\n");
-        return GNSS_MODEL_ATGM336H;
-    }
-
+    // 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 */
-    clearBuffer();
-    _serial_gps->write("$PCAS06,1*1A\r\n");
-    if (getACK("$GPTXT,01,01,02,HW=ATGM332D", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("ATGM332D detected, using ATGM336H Module\n");
-        return GNSS_MODEL_ATGM336H;
-    }
+    PROBE_SIMPLE("ATGM332D", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM332D", GNSS_MODEL_ATGM336H, 500);
 
     /* Airoha (Mediatek) AG3335A/M/S, A3352Q, Quectel L89 2.0, SimCom SIM65M */
-    clearBuffer();
-    _serial_gps->write("PAIR020*38\r\n");
-    if (getACK("$PAIR020,AG3335", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("Aioha AG3335 detected, using AG3335 Module\n");
-        return GNSS_MODEL_AG3335;
-    }
+    PROBE_SIMPLE("AG3335", "PAIR020*38", "$PAIR020,AG3335", GNSS_MODEL_AG3335, 500);
 
-    // Get version information
-    clearBuffer();
-    _serial_gps->write("$PCAS06,0*1B\r\n");
-    if (getACK("$GPTXT,01,01,02,SW=", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("L76K GNSS init succeeded, using L76K GNSS Module\n");
-        return GNSS_MODEL_MTK;
-    }
+    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);
 
-    // Get version information
-    clearBuffer();
-    _serial_gps->write("$PMTK605*31\r\n");
-    if (getACK("Quectel-L76B", 500) == GNSS_RESPONSE_OK) {
-        LOG_INFO("L76B GNSS init succeeded, using L76B GNSS Module\n");
-        return GNSS_MODEL_MTK_L76B;
-    }
+    PROBE_SIMPLE("L76B", "$PMTK605*31", "Quectel-L76B", GNSS_MODEL_MTK_L76B, 500);
 
     uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00};
     UBXChecksum(cfg_rate, sizeof(cfg_rate));
@@ -1840,4 +1802,4 @@ void GPS::toggleGpsMode()
         enable();
     }
 }
-#endif // Exclude GPS
+#endif // Exclude GPS