Ken Van Hoeylandt
147 lines
5.7 KiB
C++
147 lines
5.7 KiB
C++
#include "Tactility/hal/gps/GpsDevice.h"
|
|
#include "Tactility/hal/gps/Ublox.h"
|
|
#include <Tactility/kernel/Kernel.h>
|
|
|
|
#include <tactility/device.h>
|
|
#include <tactility/drivers/uart_controller.h>
|
|
#include <tactility/log.h>
|
|
|
|
#include <cstring>
|
|
|
|
constexpr auto* TAG = "Gps";
|
|
|
|
#define GPS_UART_BUFFER_SIZE 256
|
|
|
|
using namespace tt;
|
|
using namespace tt::hal;
|
|
|
|
namespace tt::hal::gps {
|
|
|
|
/**
|
|
* From: https://github.com/meshtastic/firmware/blob/3b0232de1b6282eacfbff6e50b68fca7e67b8511/src/meshUtils.cpp#L40
|
|
*/
|
|
char* strnstr(const char* s, const char* find, size_t slen) {
|
|
char c;
|
|
if ((c = *find++) != '\0') {
|
|
char sc;
|
|
size_t len;
|
|
|
|
len = strlen(find);
|
|
do {
|
|
do {
|
|
if (slen-- < 1 || (sc = *s++) == '\0')
|
|
return (nullptr);
|
|
} while (sc != c);
|
|
if (len > slen)
|
|
return (nullptr);
|
|
} while (strncmp(s, find, len) != 0);
|
|
s--;
|
|
}
|
|
return ((char*)s);
|
|
}
|
|
|
|
/**
|
|
* From: https://github.com/meshtastic/firmware/blob/f81d3b045dd1b7e3ca7870af3da915ff4399ea98/src/gps/GPS.cpp
|
|
*/
|
|
GpsResponse getAck(::Device* uart, const char* message, uint32_t waitMillis) {
|
|
uint8_t buffer[768] = {0};
|
|
uint8_t b;
|
|
int bytesRead = 0;
|
|
uint32_t startTimeout = kernel::getMillis() + waitMillis;
|
|
#ifdef GPS_DEBUG
|
|
std::string debugmsg = "";
|
|
#endif
|
|
while (kernel::getMillis() < startTimeout) {
|
|
size_t available = 0;
|
|
uart_controller_get_available(uart, &available);
|
|
if (available > 0) {
|
|
uart_controller_read_byte(uart, &b, 1);
|
|
|
|
#ifdef GPS_DEBUG
|
|
debugmsg += vformat("%c", (b >= 32 && b <= 126) ? b : '.');
|
|
#endif
|
|
buffer[bytesRead] = b;
|
|
bytesRead++;
|
|
if ((bytesRead == 767) || (b == '\r')) {
|
|
if (strnstr((char*)buffer, message, bytesRead) != nullptr) {
|
|
#ifdef GPS_DEBUG
|
|
LOG_D(TAG, "Found: %s", message); // Log the found message
|
|
#endif
|
|
return GpsResponse::Ok;
|
|
} else {
|
|
bytesRead = 0;
|
|
#ifdef GPS_DEBUG
|
|
LOG_D(TAG, "%s", debugmsg.c_str());
|
|
#endif
|
|
}
|
|
}
|
|
}
|
|
}
|
|
return GpsResponse::None;
|
|
}
|
|
|
|
/**
|
|
* From: https://github.com/meshtastic/firmware/blob/f81d3b045dd1b7e3ca7870af3da915ff4399ea98/src/gps/GPS.cpp
|
|
*/
|
|
#define PROBE_SIMPLE(UART, CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...) \
|
|
do { \
|
|
LOG_I(TAG, "Probing for %s (%s)", CHIP, TOWRITE); \
|
|
uart_controller_flush_input(UART); \
|
|
uart_controller_write_bytes(UART, (const uint8_t*)(TOWRITE "\r\n"), strlen(TOWRITE "\r\n"), TIMEOUT); \
|
|
if (getAck(UART, RESPONSE, TIMEOUT) == GpsResponse::Ok) { \
|
|
LOG_I(TAG, "Probe detected %s %s", CHIP, #DRIVER); \
|
|
return DRIVER; \
|
|
} \
|
|
} while (0)
|
|
|
|
/**
|
|
* From: https://github.com/meshtastic/firmware/blob/f81d3b045dd1b7e3ca7870af3da915ff4399ea98/src/gps/GPS.cpp
|
|
*/
|
|
GpsModel probe(::Device* uart) {
|
|
// Close all NMEA sentences, valid for L76K, ATGM336H (and likely other AT6558 devices)
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n", 40, 500);
|
|
kernel::delayMillis(20);
|
|
|
|
// Close NMEA sequences on Ublox
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n", 29, 500);
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PUBX,40,GSV,0,0,0,0,0,0*59\r\n", 29, 500);
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n", 29, 500);
|
|
kernel::delayMillis(20);
|
|
|
|
// Unicore UFirebirdII Series: UC6580, UM620, UM621, UM670A, UM680A, or UM681A
|
|
PROBE_SIMPLE(uart, "UC6580", "$PDTINFO", "UC6580", GpsModel::UC6580, 500);
|
|
PROBE_SIMPLE(uart, "UM600", "$PDTINFO", "UM600", GpsModel::UC6580, 500);
|
|
PROBE_SIMPLE(uart, "ATGM336H", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM336H", GpsModel::ATGM336H, 500);
|
|
|
|
/* ATGM332D series (-11(GPS), -21(BDS), -31(GPS+BDS), -51(GPS+GLONASS), -71-0(GPS+BDS+GLONASS))
|
|
based on AT6558 */
|
|
PROBE_SIMPLE(uart, "ATGM332D", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM332D", GpsModel::ATGM336H, 500);
|
|
|
|
/* Airoha (Mediatek) AG3335A/M/S, A3352Q, Quectel L89 2.0, SimCom SIM65M */
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,2,0*3C\r\n", 17, 500); // GSA OFF to reduce volume
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,3,0*3D\r\n", 17, 500); // GSV OFF to reduce volume
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR513*3D\r\n", 13, 500); // save configuration
|
|
PROBE_SIMPLE(uart, "AG3335", "$PAIR021*39", "$PAIR021,AG3335", GpsModel::AG3335, 500);
|
|
PROBE_SIMPLE(uart, "AG3352", "$PAIR021*39", "$PAIR021,AG3352", GpsModel::AG3352, 500);
|
|
PROBE_SIMPLE(uart, "LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GpsModel::AG3352, 500);
|
|
|
|
PROBE_SIMPLE(uart, "L76K", "$PCAS06,0*1B", "$GPTXT,01,01,02,SW=", GpsModel::MTK, 500);
|
|
|
|
// Close all NMEA sentences, valid for L76B MTK platform (Waveshare Pico GPS)
|
|
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n", 51, 500);
|
|
kernel::delayMillis(20);
|
|
|
|
PROBE_SIMPLE(uart, "L76B", "$PMTK605*31", "Quectel-L76B", GpsModel::MTK_L76B, 500);
|
|
PROBE_SIMPLE(uart, "PA1616S", "$PMTK605*31", "1616S", GpsModel::MTK_PA1616S, 500);
|
|
|
|
auto ublox_result = ublox::probe(uart);
|
|
if (ublox_result != GpsModel::Unknown) {
|
|
return ublox_result;
|
|
} else {
|
|
LOG_W(TAG, "No GNSS Module");
|
|
return GpsModel::Unknown;
|
|
}
|
|
}
|
|
|
|
} // namespace tt::hal::gps
|