235 lines
6.3 KiB
C++
235 lines
6.3 KiB
C++
#if (HAS_GPS)
|
|
|
|
#include "globals.h"
|
|
#include "gpsread.h"
|
|
|
|
// Local logging tag
|
|
static const char TAG[] = __FILE__;
|
|
|
|
TinyGPSPlus gps;
|
|
TaskHandle_t GpsTask;
|
|
HardwareSerial GPS_Serial(1); // use UART #1
|
|
|
|
// Ublox UBX packet data
|
|
|
|
// UBX CFG-PRT packet
|
|
byte CFG_PRT[] = {
|
|
0xB5, // sync char 1
|
|
0x62, // sync char 2
|
|
0x06, // class
|
|
0x00, // id
|
|
0x14, // length
|
|
0x00, // .
|
|
0x01, // portID (UART 1)
|
|
0x00, // reserved
|
|
0x00, // txReady
|
|
0x00, // .
|
|
0b11010000, // UART mode: 8N1
|
|
0b00001000, // .
|
|
0x00, // .
|
|
0x00, // .
|
|
(byte)GPS_BAUDRATE, // baudrate
|
|
(byte)(GPS_BAUDRATE >> 8), // .
|
|
(byte)(GPS_BAUDRATE >> 16), // .
|
|
(byte)(GPS_BAUDRATE >> 24), // .
|
|
0b00000011, // input protocols: NMEA + UBX
|
|
0b00000000, // .
|
|
0b00000010, // output protocols: NMEA
|
|
0x00000000, // .
|
|
0x00, // reserved
|
|
0x00, // .
|
|
0x00, // .
|
|
0x00 // .
|
|
};
|
|
|
|
// Array of two bytes for CFG-MSG packets payload.
|
|
byte CFG_MSG_CID[][2] = {{0xF0, 0x01}, {0xF0, 0x02}, {0xF0, 0x03}, {0xF0, 0x05},
|
|
{0xF0, 0x06}, {0xF0, 0x07}, {0xF0, 0x08}, {0xF0, 0x09},
|
|
{0xF0, 0x0A}, {0xF0, 0x0E}, {0xF1, 0x00}, {0xF1, 0x03},
|
|
{0xF1, 0x04}, {0xF1, 0x05}, {0xF1, 0x06}};
|
|
|
|
// UBX CFG-MSG packet
|
|
byte CFG_MSG[] = {
|
|
0xB5, // sync char 1
|
|
0x62, // sync char 2
|
|
0x06, // class
|
|
0x01, // id
|
|
0x03, // length
|
|
0x00, // .
|
|
0x00, // payload (first byte from messages array element)
|
|
0x00, // payload (second byte from messages array element)
|
|
0x00 // payload (zero to disable message)
|
|
};
|
|
|
|
// UBX CFG-CFG packet
|
|
byte CFG_CFG[] = {
|
|
0xB5, // sync char 1
|
|
0x62, // sync char 2
|
|
0x06, // class
|
|
0x09, // id
|
|
0x0D, // length
|
|
0x00, // .
|
|
0b00011111, // clearmask
|
|
0b00000110, // .
|
|
0x00, // .
|
|
0x00, // .
|
|
0x00, // savemask
|
|
0x00, // .
|
|
0x00, // .
|
|
0x00, // .
|
|
0b00011111, // loadmask
|
|
0b00000110, // .
|
|
0x00, // .
|
|
0x00, // .
|
|
0b00010001 // devicemask
|
|
};
|
|
|
|
// helper functions to send UBX commands to ublox gps chip
|
|
|
|
void sendPacket(byte *packet, byte len) {
|
|
|
|
uint8_t CK_A = 0;
|
|
uint8_t CK_B = 0;
|
|
|
|
for (int i = 0; i < len; i++)
|
|
GPS_Serial.write(packet[i]);
|
|
|
|
// calculate and send Fletcher checksum
|
|
for (int i = 2; i < len; i++) {
|
|
CK_A += packet[i];
|
|
CK_B += CK_A;
|
|
}
|
|
GPS_Serial.write(CK_A);
|
|
GPS_Serial.write(CK_B);
|
|
}
|
|
|
|
void restoreDefaults() { sendPacket(CFG_CFG, sizeof(CFG_CFG)); }
|
|
void changeBaudrate() { sendPacket(CFG_PRT, sizeof(CFG_PRT)); }
|
|
|
|
void disableNmea() {
|
|
|
|
// tinygps++ processes only $GPGGA/$GNGGA and $GPRMC/$GNRMC
|
|
// thus, we disable all other NMEA messages
|
|
|
|
byte packetSize = sizeof(CFG_MSG);
|
|
|
|
// Offset to the place where payload starts.
|
|
byte payloadOffset = 6;
|
|
|
|
// Iterate over the messages array.
|
|
for (byte i = 0; i < sizeof(CFG_MSG_CID) / sizeof(*CFG_MSG_CID); i++) {
|
|
// Copy two bytes of payload to the packet buffer.
|
|
for (byte j = 0; j < sizeof(*CFG_MSG_CID); j++) {
|
|
CFG_MSG[payloadOffset + j] = CFG_MSG_CID[i][j];
|
|
}
|
|
sendPacket(CFG_MSG, packetSize);
|
|
}
|
|
}
|
|
|
|
// initialize and configure GPS
|
|
int gps_init(void) {
|
|
|
|
ESP_LOGI(TAG, "Opening serial GPS");
|
|
|
|
GPS_Serial.begin(GPS_SERIAL);
|
|
|
|
restoreDefaults();
|
|
delay(100);
|
|
|
|
changeBaudrate();
|
|
delay(100);
|
|
GPS_Serial.flush();
|
|
GPS_Serial.updateBaudRate(GPS_BAUDRATE);
|
|
|
|
disableNmea();
|
|
|
|
return 1;
|
|
|
|
} // gps_init()
|
|
|
|
// store current GPS location data in struct
|
|
void gps_storelocation(gpsStatus_t *gps_store) {
|
|
if (gps.location.isUpdated() && gps.location.isValid() &&
|
|
(gps.location.age() < 1500)) {
|
|
gps_store->latitude = (int32_t)(gps.location.lat() * 1e6);
|
|
gps_store->longitude = (int32_t)(gps.location.lng() * 1e6);
|
|
gps_store->satellites = (uint8_t)gps.satellites.value();
|
|
gps_store->hdop = (uint16_t)gps.hdop.value();
|
|
gps_store->altitude = (int16_t)gps.altitude.meters();
|
|
}
|
|
}
|
|
|
|
bool gps_hasfix() {
|
|
// adapted from source:
|
|
// https://github.com/hottimuc/Lora-TTNMapper-T-Beam/blob/master/fromV08/gps.cpp
|
|
return (gps.location.isValid() && gps.location.age() < 4000 &&
|
|
gps.hdop.isValid() && gps.hdop.value() <= 600 &&
|
|
gps.hdop.age() < 4000 && gps.altitude.isValid() &&
|
|
gps.altitude.age() < 4000);
|
|
}
|
|
|
|
// function to poll UTC time from GPS NMEA data; note: this is costly
|
|
time_t get_gpstime(uint16_t *msec = 0) {
|
|
|
|
const uint16_t txDelay =
|
|
70U * 1000 / (GPS_BAUDRATE / 9); // serial tx of 70 NMEA chars
|
|
|
|
// did we get a current date & time?
|
|
if (gps.time.age() < 1000) {
|
|
|
|
// convert tinygps time format to struct tm format
|
|
struct tm gps_tm = {0};
|
|
gps_tm.tm_sec = gps.time.second();
|
|
gps_tm.tm_min = gps.time.minute();
|
|
gps_tm.tm_hour = gps.time.hour();
|
|
gps_tm.tm_mday = gps.date.day();
|
|
gps_tm.tm_mon = gps.date.month() - 1; // 1-12 -> 0-11
|
|
gps_tm.tm_year = gps.date.year() - 1900; // 2000+ -> years since 1900
|
|
|
|
// convert UTC tm to time_t epoch
|
|
gps_tm.tm_isdst = 0; // UTC has no DST
|
|
time_t t = mkgmtime(&gps_tm);
|
|
|
|
#ifdef GPS_INT
|
|
// if we have a recent GPS PPS pulse, sync on top of next second
|
|
uint16_t ppsDiff = millis() - lastPPS;
|
|
if (ppsDiff < 1000)
|
|
*msec = ppsDiff;
|
|
else {
|
|
ESP_LOGD(TAG, "no PPS from GPS");
|
|
return 0;
|
|
}
|
|
#else
|
|
// best guess for sync on top of next second
|
|
*msec = gps.time.centisecond() * 10U + txDelay;
|
|
#endif
|
|
|
|
return t;
|
|
}
|
|
|
|
ESP_LOGD(TAG, "no valid GPS time");
|
|
return 0;
|
|
|
|
} // get_gpstime()
|
|
|
|
// GPS serial feed FreeRTos Task
|
|
void gps_loop(void *pvParameters) {
|
|
|
|
_ASSERT((uint32_t)pvParameters == 1); // FreeRTOS check
|
|
|
|
// feed GPS decoder with serial NMEA data from GPS device
|
|
while (1) {
|
|
while (cfg.payloadmask & GPS_DATA) {
|
|
|
|
while (GPS_Serial.available())
|
|
gps.encode(GPS_Serial.read());
|
|
|
|
delay(5);
|
|
}
|
|
delay(1000);
|
|
} // infinite while loop
|
|
|
|
} // gps_loop()
|
|
|
|
#endif // HAS_GPS
|