LG290P : add isBlocking checks and retries for enterConfigMode

Author: PaulZC

Firmware/RTK_Everywhere/GNSS_LG290P.h

@@ -176,7 +176,7 @@ class GNSS_LG290P : GNSS
     //   Returns true if successfully started and false upon failure
     bool enableRTCMTest();
 
-    bool enterConfigMode();
+    bool enterConfigMode(unsigned long waitForSemaphoreTimeout_millis);
 
     bool exitConfigMode();
 

Firmware/RTK_Everywhere/GNSS_LG290P.ino

@@ -224,24 +224,32 @@ bool GNSS_LG290P::configureOnce()
 
     bool response = true;
 
-    response &= enterConfigMode();
+    uint8_t retries = 4;
+
+    while ((retries > 0) && (!enterConfigMode(500)))
+    {
+        retries--;
+        systemPrintf("configureOnce: Enter config mode failed. %d retries remaining\r\n", retries);
+    }
+
+    response &= (retries > 0);
     if (settings.debugGnss && response == false)
-        systemPrintln("ConfigureOnce: Enter config mode failed");
+        systemPrintln("configureOnce: Enter config mode failed");
 
     response &= setDataBaudRate(settings.dataPortBaud);   // LG290P UART1 is connected to CH342 (Port B)
     response &= _lg290p->setPortBaudrate(2, 115200 * 4);  // LG290P UART2 is connected to the ESP32 UART1
     response &= setRadioBaudRate(settings.radioPortBaud); // LG290P UART3 is connected to the locking JST connector
     if (settings.debugGnss && response == false)
-        systemPrintln("ConfigureOnce: setBauds failed");
+        systemPrintln("configureOnce: setBauds failed");
 
     // Enable PPS signal with a width of 200ms
     response &= _lg290p->setPPS(200, false, true); // duration time ms, alwaysOutput, polarity
     if (settings.debugGnss && response == false)
-        systemPrintln("ConfigureOnce: setPPS failed");
+        systemPrintln("configureOnce: setPPS failed");
 
     response &= setConstellations();
     if (settings.debugGnss && response == false)
-        systemPrintln("ConfigureOnce: setConstellations failed");
+        systemPrintln("configureOnce: setConstellations failed");
 
     // We do not set Rover or fix rate here because fix rate only applies in rover mode.
 
@@ -291,9 +299,17 @@ bool GNSS_LG290P::configureRover()
 
     serialGNSS->flush(); // Remove any incoming characters
 
-    response &= enterConfigMode();
+    uint8_t retries = 4;
+
+    while ((retries > 0) && (!enterConfigMode(500)))
+    {
+        retries--;
+        systemPrintf("configureRover: Enter config mode failed. %d retries remaining\r\n", retries);
+    }
+
+    response &= (retries > 0);
     if (settings.debugGnss && response == false)
-        systemPrintln("Rover: Enter config mode failed");
+        systemPrintln("configureRover: Enter config mode failed");
 
     // We must force receiver into Rover mode so that we can set fix rate
     int currentMode = getMode();
@@ -304,21 +320,21 @@ bool GNSS_LG290P::configureRover()
         // fail because NMEA is not present.
         _lg290p->setModeRover(); // Wait for save and reset
         if (settings.debugGnss && response == false)
-            systemPrintln("Rover: Set mode rover failed");
+            systemPrintln("configureRover: Set mode rover failed");
     }
 
     // Set the fix rate. Default on LG290P is 10Hz so set accordingly.
     response &= setRate(settings.measurementRateMs / 1000.0); // May require save/reset
     if (settings.debugGnss && response == false)
-        systemPrintln("Rover: Set rate failed");
+        systemPrintln("configureRover: Set rate failed");
 
     response &= enableRTCMRover();
     if (settings.debugGnss && response == false)
-        systemPrintln("Rover: Enable RTCM failed");
+        systemPrintln("configureRover: Enable RTCM failed");
 
     response &= enableNMEA();
     if (settings.debugGnss && response == false)
-        systemPrintln("Rover: Enable NMEA failed");
+        systemPrintln("configureRover: Enable NMEA failed");
 
     response &= exitConfigMode(); // We must exit config before we save otherwise we will save with NMEA/RTCM off
 
@@ -371,9 +387,17 @@ bool GNSS_LG290P::configureBase()
 
     serialGNSS->flush(); // Remove any incoming characters
 
-    response &= enterConfigMode();
+    uint8_t retries = 4;
+
+    while ((retries > 0) && (!enterConfigMode(500)))
+    {
+        retries--;
+        systemPrintf("configureBase: Enter config mode failed. %d retries remaining\r\n", retries);
+    }
+
+    response &= (retries > 0);
     if (settings.debugGnss && response == false)
-        systemPrintln("Base: Enter config mode failed");
+        systemPrintln("configureBase: Enter config mode failed");
 
     // "When set to Base Station mode, the receiver will automatically disable NMEA message output and enable RTCM MSM4
     // and RTCM3-1005 message output."
@@ -391,7 +415,7 @@ bool GNSS_LG290P::configureBase()
         // fail because NMEA is not present.
         _lg290p->setModeBase(false); // Don't save and reset
         if (settings.debugGnss && response == false)
-            systemPrintln("Base: Set mode base failed");
+            systemPrintln("configureBase: Set mode base failed");
 
         // Device should now have survey mode disabled
     }
@@ -405,16 +429,16 @@ bool GNSS_LG290P::configureBase()
         // fail because NMEA is not present.
         disableSurveyIn(false); // Don't save and reset
         if (settings.debugGnss && response == false)
-            systemPrintln("Base: disable survey in failed");
+            systemPrintln("configureBase: disable survey in failed");
     }
 
     response &= enableRTCMBase(); // Set RTCM messages
     if (settings.debugGnss && response == false)
-        systemPrintln("Base: Enable RTCM failed");
+        systemPrintln("configureBase: Enable RTCM failed");
 
     response &= enableNMEA(); // Set NMEA messages
     if (settings.debugGnss && response == false)
-        systemPrintln("Base: Enable NMEA failed");
+        systemPrintln("configureBase: Enable NMEA failed");
 
     response &= exitConfigMode(); // We must exit config before we save otherwise we will save with NMEA/RTCM off
 
@@ -464,11 +488,20 @@ bool GNSS_LG290P::configureNtpMode()
 //----------------------------------------
 // Disable NMEA and RTCM on UART2 to reduce the serial traffic
 //----------------------------------------
-bool GNSS_LG290P::enterConfigMode()
+bool GNSS_LG290P::enterConfigMode(unsigned long waitForSemaphoreTimeout_millis)
 {
     if (online.gnss)
+    {
+        unsigned long start = millis();
+        bool isBlocking;
+        do { // Wait for up to waitForSemaphoreTimeout for library to stop blocking
+            isBlocking = _lg290p->isBlocking();
+        } while (isBlocking && (millis() < (start + waitForSemaphoreTimeout_millis)));
+
+        // This will fail if the library is still blocking, but it is worth a punt...
         return (_lg290p->sendOkCommand("$PQTMCFGPROT",
                                        ",W,1,2,00000000,00000000")); // Disable NMEA and RTCM on the LG290P UART2
+    }
     return (false);
 }