280 lines
8.2 KiB
Arduino
280 lines
8.2 KiB
Arduino
/**
|
|
* @file GPSShield.ino
|
|
* @author Lewis He (lewishe@outlook.com)
|
|
* @license MIT
|
|
* @copyright Copyright (c) 2024 Shenzhen Xin Yuan Electronic Technology Co., Ltd
|
|
* @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
|
|
* @setting Arduino IDE : Tools -> USB CDC On Boot -> Enabled
|
|
*/
|
|
|
|
#ifndef SerialGPS
|
|
#define SerialGPS Serial1
|
|
#endif
|
|
|
|
#define BOARD_GPS_TX_PIN 43
|
|
#define BOARD_GPS_RX_PIN 44
|
|
|
|
#define BOARD_POWERON 10
|
|
|
|
#include <Arduino.h>
|
|
#include <TinyGPS++.h>
|
|
|
|
TinyGPSPlus gps;
|
|
static bool GPS_Recovery();
|
|
|
|
bool setupGPS()
|
|
{
|
|
// L76K GPS USE 9600 BAUDRATE
|
|
SerialGPS.begin(9600, SERIAL_8N1, BOARD_GPS_RX_PIN, BOARD_GPS_TX_PIN);
|
|
bool result = false;
|
|
uint32_t startTimeout ;
|
|
for (int i = 0; i < 3; ++i) {
|
|
SerialGPS.write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
|
|
delay(5);
|
|
// Get version information
|
|
startTimeout = millis() + 3000;
|
|
Serial.print("Try to init L76K . Wait stop .");
|
|
while (SerialGPS.available()) {
|
|
Serial.print(".");
|
|
SerialGPS.readString();
|
|
if (millis() > startTimeout) {
|
|
Serial.println("Wait L76K stop NMEA timeout!");
|
|
return false;
|
|
}
|
|
};
|
|
Serial.println();
|
|
SerialGPS.flush();
|
|
delay(200);
|
|
|
|
SerialGPS.write("$PCAS06,0*1B\r\n");
|
|
startTimeout = millis() + 500;
|
|
String ver = "";
|
|
while (!SerialGPS.available()) {
|
|
if (millis() > startTimeout) {
|
|
Serial.println("Get L76K timeout!");
|
|
return false;
|
|
}
|
|
}
|
|
SerialGPS.setTimeout(10);
|
|
ver = SerialGPS.readStringUntil('\n');
|
|
if (ver.startsWith("$GPTXT,01,01,02")) {
|
|
Serial.println("L76K GNSS init succeeded, using L76K GNSS Module\n");
|
|
result = true;
|
|
break;
|
|
}
|
|
delay(500);
|
|
}
|
|
// Initialize the L76K Chip, use GPS + GLONASS
|
|
SerialGPS.write("$PCAS04,5*1C\r\n");
|
|
delay(250);
|
|
SerialGPS.write("$PCAS03,1,1,1,1,1,1,1,1,1,1,,,0,0*02\r\n");
|
|
delay(250);
|
|
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
|
|
SerialGPS.write("$PCAS11,3*1E\r\n");
|
|
return result;
|
|
}
|
|
|
|
void setup()
|
|
{
|
|
Serial.begin(115200);
|
|
|
|
//!⚠️ The board peripheral power control pin needs to be set to HIGH when using the peripheral
|
|
pinMode(BOARD_POWERON, OUTPUT);
|
|
digitalWrite(BOARD_POWERON, HIGH);
|
|
|
|
Serial.println("This sketch only applies to boards carrying GPS shields, the default is T-Deck");
|
|
|
|
Serial.println("It does not support GPS function. Of course, you can use an external GPS module to connect to the Gover interface.");
|
|
|
|
|
|
// Arduino IDE : Tools -> USB CDC On Boot -> Enabled
|
|
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);
|
|
}
|
|
}
|
|
}
|
|
|
|
void loop()
|
|
{
|
|
|
|
|
|
while (Serial.available()) {
|
|
SerialGPS.write(Serial.read());
|
|
}
|
|
|
|
while (SerialGPS.available()) {
|
|
int c = SerialGPS.read();
|
|
// Serial.write(c);
|
|
if (gps.encode(c)) {
|
|
displayInfo();
|
|
}
|
|
}
|
|
|
|
if (millis() > 30000 && gps.charsProcessed() < 10) {
|
|
Serial.println(F("No GPS detected: check wiring."));
|
|
delay(1000);
|
|
}
|
|
|
|
delay(1);
|
|
}
|
|
|
|
|
|
void displayInfo()
|
|
{
|
|
Serial.print(F("Location: "));
|
|
if (gps.location.isValid()) {
|
|
Serial.print(gps.location.lat(), 6);
|
|
Serial.print(F(","));
|
|
Serial.print(gps.location.lng(), 6);
|
|
} else {
|
|
Serial.print(F("INVALID"));
|
|
}
|
|
|
|
Serial.print(F(" Date/Time: "));
|
|
if (gps.date.isValid()) {
|
|
Serial.print(gps.date.month());
|
|
Serial.print(F("/"));
|
|
Serial.print(gps.date.day());
|
|
Serial.print(F("/"));
|
|
Serial.print(gps.date.year());
|
|
} else {
|
|
Serial.print(F("INVALID"));
|
|
}
|
|
|
|
Serial.print(F(" "));
|
|
if (gps.time.isValid()) {
|
|
if (gps.time.hour() < 10) Serial.print(F("0"));
|
|
Serial.print(gps.time.hour());
|
|
Serial.print(F(":"));
|
|
if (gps.time.minute() < 10) Serial.print(F("0"));
|
|
Serial.print(gps.time.minute());
|
|
Serial.print(F(":"));
|
|
if (gps.time.second() < 10) Serial.print(F("0"));
|
|
Serial.print(gps.time.second());
|
|
Serial.print(F("."));
|
|
if (gps.time.centisecond() < 10) Serial.print(F("0"));
|
|
Serial.print(gps.time.centisecond());
|
|
} else {
|
|
Serial.print(F("INVALID"));
|
|
}
|
|
|
|
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;
|
|
}
|