Update GPS example

Xinyuan-LilyGO · Sep 13, 2024 · 83ecab1 · 83ecab1
1 parent c3e03ef
commit 83ecab1
Showing 1 changed file with 113 additions and 5 deletions.
diff --git a/examples/GPSShield/GPSShield.ino b/examples/GPSShield/GPSShield.ino
@@ -6,7 +6,7 @@
  * @date      2024-03-29
  * @note      This sketch only applies to boards carrying GPS shields, the default is T-Deck
  *            It does not support GPS function. Of course, you can use an external GPS module to connect to the Gover interface.
- * 
+ *
  * @setting   Arduino IDE : Tools -> USB CDC On Boot -> Enabled
  * @setting   Arduino IDE : Tools -> USB CDC On Boot -> Enabled
  * @setting   Arduino IDE : Tools -> USB CDC On Boot -> Enabled
@@ -26,7 +26,7 @@
 #include <TinyGPS++.h>
 
 TinyGPSPlus gps;
-
+static bool GPS_Recovery();
 
 bool setupGPS()
 {
@@ -97,10 +97,17 @@ void setup()
     if (!setupGPS()) {
         // Set u-blox m10q gps baudrate 38400
         SerialGPS.begin(38400, SERIAL_8N1, BOARD_GPS_RX_PIN, BOARD_GPS_TX_PIN);
+        if (!GPS_Recovery()) {
+            SerialGPS.updateBaudRate(9600);
+            if (!GPS_Recovery()) {
+                while (1) {
+                    Serial.println("GPS Connect failed~!");
+                    delay(1000);
+                }
+            }
+            SerialGPS.updateBaudRate(38400);
+        }
     }
-
-    delay(2000);
-
 }
 
 void loop()
@@ -169,3 +176,104 @@ void displayInfo()
 
     Serial.println();
 }
+
+
+
+
+
+uint8_t buffer[256];
+
+int getAck(uint8_t *buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedID)
+{
+    uint16_t    ubxFrameCounter = 0;
+    bool        ubxFrame = 0;
+    uint32_t    startTime = millis();
+    uint16_t    needRead;
+
+    while (millis() - startTime < 800) {
+        while (SerialGPS.available()) {
+            int c = SerialGPS.read();
+            switch (ubxFrameCounter) {
+            case 0:
+                if (c == 0xB5) {
+                    ubxFrameCounter++;
+                }
+                break;
+            case 1:
+                if (c == 0x62) {
+                    ubxFrameCounter++;
+                } else {
+                    ubxFrameCounter = 0;
+                }
+                break;
+            case 2:
+                if (c == requestedClass) {
+                    ubxFrameCounter++;
+                } else {
+                    ubxFrameCounter = 0;
+                }
+                break;
+            case 3:
+                if (c == requestedID) {
+                    ubxFrameCounter++;
+                } else {
+                    ubxFrameCounter = 0;
+                }
+                break;
+            case 4:
+                needRead = c;
+                ubxFrameCounter++;
+                break;
+            case 5:
+                needRead |=  (c << 8);
+                ubxFrameCounter++;
+                break;
+            case 6:
+                if (needRead >= size) {
+                    ubxFrameCounter = 0;
+                    break;
+                }
+                if (SerialGPS.readBytes(buffer, needRead) != needRead) {
+                    ubxFrameCounter = 0;
+                } else {
+                    return needRead;
+                }
+                break;
+
+            default:
+                break;
+            }
+        }
+    }
+    return 0;
+}
+
+static bool GPS_Recovery()
+{
+    uint8_t cfg_clear1[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x1C, 0xA2};
+    uint8_t cfg_clear2[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x1B, 0xA1};
+    uint8_t cfg_clear3[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x03, 0x1D, 0xB3};
+    SerialGPS.write(cfg_clear1, sizeof(cfg_clear1));
+
+    if (getAck(buffer, 256, 0x05, 0x01)) {
+        Serial.println("Get ack successes!");
+    }
+    SerialGPS.write(cfg_clear2, sizeof(cfg_clear2));
+    if (getAck(buffer, 256, 0x05, 0x01)) {
+        Serial.println("Get ack successes!");
+    }
+    SerialGPS.write(cfg_clear3, sizeof(cfg_clear3));
+    if (getAck(buffer, 256, 0x05, 0x01)) {
+        Serial.println("Get ack successes!");
+    }
+
+    // UBX-CFG-RATE, Size 8, 'Navigation/measurement rate settings'
+    uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};
+    SerialGPS.write(cfg_rate, sizeof(cfg_rate));
+    if (getAck(buffer, 256, 0x06, 0x08)) {
+        Serial.println("Get ack successes!");
+    } else {
+        return false;
+    }
+    return true;
+}