From 83ecab13fa1f58de0ea1e2233a886ad298277e9a Mon Sep 17 00:00:00 2001 From: lewisxhe Date: Fri, 13 Sep 2024 14:27:20 +0800 Subject: [PATCH] Update GPS example --- examples/GPSShield/GPSShield.ino | 118 +++++++++++++++++++++++++++++-- 1 file changed, 113 insertions(+), 5 deletions(-) diff --git a/examples/GPSShield/GPSShield.ino b/examples/GPSShield/GPSShield.ino index 6aa5c41..26c77e5 100644 --- 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 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; +}