Merge TactilityHeadless into Tactility (#263)
There currently is no practical use to have TactilityHeadless as a subproject. I'm merging it with the Tactility project.
This commit is contained in:
Ken Van Hoeylandt
committed by
GitHub
GitHub
parent
d0ca3b16f8
commit
d72852a6e2
@@ -0,0 +1,51 @@
|
||||
#include "Tactility/hal/gps/GpsConfiguration.h"
|
||||
#include "Tactility/service/gps/GpsService.h"
|
||||
|
||||
#include <Tactility/TactilityCore.h>
|
||||
#include <Tactility/file/ObjectFile.h>
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
const char* toString(GpsModel model) {
|
||||
using enum GpsModel;
|
||||
switch (model) {
|
||||
case AG3335:
|
||||
return TT_STRINGIFY(AG3335);
|
||||
case AG3352:
|
||||
return TT_STRINGIFY(AG3352);
|
||||
case ATGM336H:
|
||||
return TT_STRINGIFY(ATGM336H);
|
||||
case LS20031:
|
||||
return TT_STRINGIFY(LS20031);
|
||||
case MTK:
|
||||
return TT_STRINGIFY(MTK);
|
||||
case MTK_L76B:
|
||||
return TT_STRINGIFY(MTK_L76B);
|
||||
case MTK_PA1616S:
|
||||
return TT_STRINGIFY(MTK_PA1616S);
|
||||
case UBLOX6:
|
||||
return TT_STRINGIFY(UBLOX6);
|
||||
case UBLOX7:
|
||||
return TT_STRINGIFY(UBLOX7);
|
||||
case UBLOX8:
|
||||
return TT_STRINGIFY(UBLOX8);
|
||||
case UBLOX9:
|
||||
return TT_STRINGIFY(UBLOX9);
|
||||
case UBLOX10:
|
||||
return TT_STRINGIFY(UBLOX10);
|
||||
case UC6580:
|
||||
return TT_STRINGIFY(UC6580);
|
||||
default:
|
||||
return TT_STRINGIFY(Unknown);
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> getModels() {
|
||||
std::vector<std::string> result;
|
||||
for (GpsModel model = GpsModel::Unknown; model <= GpsModel::UC6580; ++(int&)model) {
|
||||
result.push_back(toString(model));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,189 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/hal/gps/GpsInit.h"
|
||||
#include "Tactility/hal/gps/Probe.h"
|
||||
#include "Tactility/hal/uart/Uart.h"
|
||||
#include <cstring>
|
||||
#include <minmea.h>
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
constexpr uint32_t GPS_UART_BUFFER_SIZE = 256;
|
||||
constexpr const char* TAG = "GpsDevice";
|
||||
|
||||
int32_t GpsDevice::threadMainStatic(void* parameter) {
|
||||
auto* gps_device = (GpsDevice*)parameter;
|
||||
return gps_device->threadMain();
|
||||
}
|
||||
|
||||
int32_t GpsDevice::threadMain() {
|
||||
uint8_t buffer[GPS_UART_BUFFER_SIZE];
|
||||
|
||||
auto uart = uart::open(configuration.uartName);
|
||||
if (uart == nullptr) {
|
||||
TT_LOG_E(TAG, "Failed to open UART %s", configuration.uartName);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!uart->start()) {
|
||||
TT_LOG_E(TAG, "Failed to start UART %s", configuration.uartName);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!uart->setBaudRate((int)configuration.baudRate)) {
|
||||
TT_LOG_E(TAG, "Failed to set baud rate to %lu for UART %s", configuration.baudRate, configuration.uartName);
|
||||
return -1;
|
||||
}
|
||||
|
||||
GpsModel model = configuration.model;
|
||||
if (model == GpsModel::Unknown) {
|
||||
model = probe(*uart);
|
||||
if (model == GpsModel::Unknown) {
|
||||
TT_LOG_E(TAG, "Probe failed");
|
||||
setState(State::Error);
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
mutex.lock();
|
||||
this->model = model;
|
||||
mutex.unlock();
|
||||
|
||||
if (!init(*uart, model)) {
|
||||
TT_LOG_E(TAG, "Init failed");
|
||||
setState(State::Error);
|
||||
return -1;
|
||||
}
|
||||
|
||||
setState(State::On);
|
||||
|
||||
// Reference: https://gpsd.gitlab.io/gpsd/NMEA.html
|
||||
while (!isThreadInterrupted()) {
|
||||
size_t bytes_read = uart->readUntil(reinterpret_cast<std::byte*>(buffer), GPS_UART_BUFFER_SIZE, '\n', 100 / portTICK_PERIOD_MS);
|
||||
|
||||
// Thread might've been interrupted in the meanwhile
|
||||
if (isThreadInterrupted()) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (bytes_read > 0U) {
|
||||
|
||||
TT_LOG_I(TAG, "[%ul] %s", bytes_read, buffer);
|
||||
|
||||
switch (minmea_sentence_id((char*)buffer, false)) {
|
||||
case MINMEA_SENTENCE_RMC:
|
||||
minmea_sentence_rmc rmc_frame;
|
||||
if (minmea_parse_rmc(&rmc_frame, (char*)buffer)) {
|
||||
mutex.lock();
|
||||
for (auto& subscription : rmcSubscriptions) {
|
||||
(*subscription.onData)(getId(), rmc_frame);
|
||||
}
|
||||
mutex.unlock();
|
||||
TT_LOG_D(TAG, "RMC %f lat, %f lon, %f m/s", minmea_tocoord(&rmc_frame.latitude), minmea_tocoord(&rmc_frame.longitude), minmea_tofloat(&rmc_frame.speed));
|
||||
} else {
|
||||
TT_LOG_W(TAG, "RMC parse error: %s", buffer);
|
||||
}
|
||||
break;
|
||||
case MINMEA_SENTENCE_GGA:
|
||||
minmea_sentence_gga gga_frame;
|
||||
if (minmea_parse_gga(&gga_frame, (char*)buffer)) {
|
||||
mutex.lock();
|
||||
for (auto& subscription : ggaSubscriptions) {
|
||||
(*subscription.onData)(getId(), gga_frame);
|
||||
}
|
||||
mutex.unlock();
|
||||
TT_LOG_D(TAG, "GGA %f lat, %f lon", minmea_tocoord(&gga_frame.latitude), minmea_tocoord(&gga_frame.longitude));
|
||||
} else {
|
||||
TT_LOG_W(TAG, "GGA parse error: %s", buffer);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (uart->isStarted() && !uart->stop()) {
|
||||
TT_LOG_W(TAG, "Failed to stop UART %s", configuration.uartName);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool GpsDevice::start() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (thread != nullptr && thread->getState() != Thread::State::Stopped) {
|
||||
TT_LOG_W(TAG, "Already started");
|
||||
return true;
|
||||
}
|
||||
|
||||
threadInterrupted = false;
|
||||
|
||||
TT_LOG_I(TAG, "Starting thread");
|
||||
setState(State::PendingOn);
|
||||
|
||||
thread = std::make_unique<Thread>(
|
||||
"gps",
|
||||
4096,
|
||||
threadMainStatic,
|
||||
this
|
||||
);
|
||||
thread->setPriority(tt::Thread::Priority::High);
|
||||
thread->start();
|
||||
|
||||
TT_LOG_I(TAG, "Starting finished");
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsDevice::stop() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
setState(State::PendingOff);
|
||||
|
||||
if (thread != nullptr) {
|
||||
threadInterrupted = true;
|
||||
|
||||
// Detach thread, it will auto-delete when leaving the current scope
|
||||
auto old_thread = std::move(thread);
|
||||
|
||||
if (old_thread->getState() != Thread::State::Stopped) {
|
||||
// Unlock so thread can lock
|
||||
lock.unlock();
|
||||
// Wait for thread to finish
|
||||
old_thread->join();
|
||||
// Re-lock to continue logic below
|
||||
lock.lock();
|
||||
}
|
||||
}
|
||||
|
||||
setState(State::Off);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsDevice::isThreadInterrupted() const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
return threadInterrupted;
|
||||
}
|
||||
|
||||
GpsModel GpsDevice::getModel() const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
return model; // Make copy because of thread safety
|
||||
}
|
||||
|
||||
GpsDevice::State GpsDevice::getState() const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
return state; // Make copy because of thread safety
|
||||
}
|
||||
|
||||
void GpsDevice::setState(State newState) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
state = newState;
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,288 @@
|
||||
#include "Tactility/hal/gps/Cas.h"
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/hal/gps/Ublox.h"
|
||||
#include <cstring>
|
||||
|
||||
#define TAG "gps"
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
bool initMtk(uart::Uart& uart);
|
||||
bool initMtkL76b(uart::Uart& uart);
|
||||
bool initMtkPa1616s(uart::Uart& uart);
|
||||
bool initAtgm336h(uart::Uart& uart);
|
||||
bool initUc6580(uart::Uart& uart);
|
||||
bool initAg33xx(uart::Uart& uart);
|
||||
|
||||
// region CAS
|
||||
|
||||
// Calculate the checksum for a CAS packet
|
||||
static void CASChecksum(uint8_t *message, size_t length)
|
||||
{
|
||||
uint32_t cksum = ((uint32_t)message[5] << 24); // Message ID
|
||||
cksum += ((uint32_t)message[4]) << 16; // Class
|
||||
cksum += message[2]; // Payload Len
|
||||
|
||||
// Iterate over the payload as a series of uint32_t's and
|
||||
// accumulate the cksum
|
||||
for (size_t i = 0; i < (length - 10) / 4; i++) {
|
||||
uint32_t pl = 0;
|
||||
memcpy(&pl, (message + 6) + (i * sizeof(uint32_t)), sizeof(uint32_t)); // avoid pointer dereference
|
||||
cksum += pl;
|
||||
}
|
||||
|
||||
// Place the checksum values in the message
|
||||
message[length - 4] = (cksum & 0xFF);
|
||||
message[length - 3] = (cksum & (0xFF << 8)) >> 8;
|
||||
message[length - 2] = (cksum & (0xFF << 16)) >> 16;
|
||||
message[length - 1] = (cksum & (0xFF << 24)) >> 24;
|
||||
}
|
||||
|
||||
// Function to create a CAS packet for editing in memory
|
||||
static uint8_t makeCASPacket(uint8_t* buffer, uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
|
||||
{
|
||||
// General CAS structure
|
||||
// | H1 | H2 | payload_len | cls | msg | Payload ... | Checksum |
|
||||
// Size: | 1 | 1 | 2 | 1 | 1 | payload_len | 4 |
|
||||
// Pos: | 0 | 1 | 2 | 3 | 4 | 5 | 6 | 7 ... | 6 + payload_len ... |
|
||||
// |------|------|-------------|------|------|------|--------------|---------------------------|
|
||||
// | 0xBA | 0xCE | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX | 0xXX ... | 0xXX | 0xXX | 0xXX | 0xXX |
|
||||
|
||||
// Construct the CAS packet
|
||||
buffer[0] = 0xBA; // header 1 (0xBA)
|
||||
buffer[1] = 0xCE; // header 2 (0xCE)
|
||||
buffer[2] = payload_size; // length 1
|
||||
buffer[3] = 0; // length 2
|
||||
buffer[4] = class_id; // class
|
||||
buffer[5] = msg_id; // id
|
||||
|
||||
buffer[6 + payload_size] = 0x00; // Checksum
|
||||
buffer[7 + payload_size] = 0x00;
|
||||
buffer[8 + payload_size] = 0x00;
|
||||
buffer[9 + payload_size] = 0x00;
|
||||
|
||||
for (int i = 0; i < payload_size; i++) {
|
||||
buffer[6 + i] = msg[i];
|
||||
}
|
||||
CASChecksum(buffer, (payload_size + 10));
|
||||
|
||||
return (payload_size + 10);
|
||||
}
|
||||
|
||||
GpsResponse getACKCas(uart::Uart& uart, uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
|
||||
{
|
||||
uint32_t startTime = kernel::getMillis();
|
||||
uint8_t buffer[CAS_ACK_NACK_MSG_SIZE] = {0};
|
||||
uint8_t bufferPos = 0;
|
||||
|
||||
// CAS-ACK-(N)ACK structure
|
||||
// | H1 | H2 | Payload Len | cls | msg | Payload | Checksum (4) |
|
||||
// | | | | | | Cls | Msg | Reserved | |
|
||||
// |------|------|-------------|------|------|------|------|-------------|---------------------------|
|
||||
// ACK-NACK| 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x00 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |
|
||||
// ACK-ACK | 0xBA | 0xCE | 0x04 | 0x00 | 0x05 | 0x01 | 0xXX | 0xXX | 0x00 | 0x00 | 0xXX | 0xXX | 0xXX | 0xXX |
|
||||
|
||||
while (kernel::getTicks() - startTime < waitMillis) {
|
||||
if (uart.available()) {
|
||||
uart.readByte(&buffer[bufferPos++]);
|
||||
|
||||
// keep looking at the first two bytes of buffer until
|
||||
// we have found the CAS frame header (0xBA, 0xCE), if not
|
||||
// keep reading bytes until we find a frame header or we run
|
||||
// out of time.
|
||||
if ((bufferPos == 2) && !(buffer[0] == 0xBA && buffer[1] == 0xCE)) {
|
||||
buffer[0] = buffer[1];
|
||||
buffer[1] = 0;
|
||||
bufferPos = 1;
|
||||
}
|
||||
}
|
||||
|
||||
// we have read all the bytes required for the Ack/Nack (14-bytes)
|
||||
// and we must have found a frame to get this far
|
||||
if (bufferPos == sizeof(buffer) - 1) {
|
||||
uint8_t msg_cls = buffer[4]; // message class should be 0x05
|
||||
uint8_t msg_msg_id = buffer[5]; // message id should be 0x00 or 0x01
|
||||
uint8_t payload_cls = buffer[6]; // payload class id
|
||||
uint8_t payload_msg = buffer[7]; // payload message id
|
||||
|
||||
// Check for an ACK-ACK for the specified class and message id
|
||||
if ((msg_cls == 0x05) && (msg_msg_id == 0x01) && payload_cls == class_id && payload_msg == msg_id) {
|
||||
#ifdef GPS_DEBUG
|
||||
LOG_INFO("Got ACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
|
||||
#endif
|
||||
return GpsResponse::Ok;
|
||||
}
|
||||
|
||||
// Check for an ACK-NACK for the specified class and message id
|
||||
if ((msg_cls == 0x05) && (msg_msg_id == 0x00) && payload_cls == class_id && payload_msg == msg_id) {
|
||||
#ifdef GPS_DEBUG
|
||||
LOG_WARN("Got NACK for class %02X message %02X in %dms", class_id, msg_id, millis() - startTime);
|
||||
#endif
|
||||
return GpsResponse::NotAck;
|
||||
}
|
||||
|
||||
// This isn't the frame we are looking for, clear the buffer
|
||||
// and try again until we run out of time.
|
||||
memset(buffer, 0x0, sizeof(buffer));
|
||||
bufferPos = 0;
|
||||
}
|
||||
}
|
||||
return GpsResponse::None;
|
||||
}
|
||||
|
||||
// endregion
|
||||
|
||||
bool init(uart::Uart& uart, GpsModel type) {
|
||||
switch (type) {
|
||||
case GpsModel::Unknown:
|
||||
tt_crash();
|
||||
case GpsModel::AG3335:
|
||||
case GpsModel::AG3352:
|
||||
return initAg33xx(uart);
|
||||
case GpsModel::ATGM336H:
|
||||
return initAtgm336h(uart);
|
||||
case GpsModel::LS20031:
|
||||
return true;
|
||||
case GpsModel::MTK:
|
||||
return initMtk(uart);
|
||||
case GpsModel::MTK_L76B:
|
||||
return initMtkL76b(uart);
|
||||
case GpsModel::MTK_PA1616S:
|
||||
return initMtkPa1616s(uart);
|
||||
case GpsModel::UBLOX6:
|
||||
case GpsModel::UBLOX7:
|
||||
case GpsModel::UBLOX8:
|
||||
case GpsModel::UBLOX9:
|
||||
case GpsModel::UBLOX10:
|
||||
return ublox::init(uart, type);
|
||||
case GpsModel::UC6580:
|
||||
return initUc6580(uart);
|
||||
}
|
||||
|
||||
TT_LOG_I(TAG, "Init not implemented %d", static_cast<int>(type));
|
||||
return false;
|
||||
}
|
||||
|
||||
bool initAg33xx(uart::Uart& uart) {
|
||||
uart.writeString("$PAIR066,1,0,1,0,0,1*3B\r\n"); // Enable GPS+GALILEO+NAVIC
|
||||
|
||||
// Configure NMEA (sentences will output once per fix)
|
||||
uart.writeString("$PAIR062,0,1*3F\r\n"); // GGA ON
|
||||
uart.writeString("$PAIR062,1,0*3F\r\n"); // GLL OFF
|
||||
uart.writeString("$PAIR062,2,0*3C\r\n"); // GSA OFF
|
||||
uart.writeString("$PAIR062,3,0*3D\r\n"); // GSV OFF
|
||||
uart.writeString("$PAIR062,4,1*3B\r\n"); // RMC ON
|
||||
uart.writeString("$PAIR062,5,0*3B\r\n"); // VTG OFF
|
||||
uart.writeString("$PAIR062,6,0*38\r\n"); // ZDA ON
|
||||
|
||||
kernel::delayMillis(250);
|
||||
uart.writeString("$PAIR513*3D\r\n"); // save configuration
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initUc6580(uart::Uart& uart) {
|
||||
// The Unicore UC6580 can use a lot of sat systems, enable it to
|
||||
// use GPS L1 & L5 + BDS B1I & B2a + GLONASS L1 + GALILEO E1 & E5a + SBAS + QZSS
|
||||
// This will reset the receiver, so wait a bit afterwards
|
||||
// The paranoid will wait for the OK*04 confirmation response after each command.
|
||||
uart.writeString("$CFGSYS,h35155\r\n");
|
||||
kernel::delayMillis(750);
|
||||
// Must be done after the CFGSYS command
|
||||
// Turn off GSV messages, we don't really care about which and where the sats are, maybe someday.
|
||||
uart.writeString("$CFGMSG,0,3,0\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Turn off GSA messages, TinyGPS++ doesn't use this message.
|
||||
uart.writeString("$CFGMSG,0,2,0\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Turn off NOTICE __TXT messages, these may provide Unicore some info but we don't care.
|
||||
uart.writeString("$CFGMSG,6,0,0\r\n");
|
||||
kernel::delayMillis(250);
|
||||
uart.writeString("$CFGMSG,6,1,0\r\n");
|
||||
kernel::delayMillis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initAtgm336h(uart::Uart& uart) {
|
||||
uint8_t buffer[256];
|
||||
|
||||
// Set the intial configuration of the device - these _should_ work for most AT6558 devices
|
||||
int msglen = makeCASPacket(buffer, 0x06, 0x07, sizeof(_message_CAS_CFG_NAVX_CONF), _message_CAS_CFG_NAVX_CONF);
|
||||
uart.writeBytes(buffer, msglen);
|
||||
if (getACKCas(uart, 0x06, 0x07, 250) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "ATGM336H: Could not set Config");
|
||||
}
|
||||
|
||||
// Set the update frequence to 1Hz
|
||||
msglen = makeCASPacket(buffer, 0x06, 0x04, sizeof(_message_CAS_CFG_RATE_1HZ), _message_CAS_CFG_RATE_1HZ);
|
||||
uart.writeBytes(buffer, msglen);
|
||||
if (getACKCas(uart, 0x06, 0x04, 250) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "ATGM336H: Could not set Update Frequency");
|
||||
}
|
||||
|
||||
// Set the NEMA output messages
|
||||
// Ask for only RMC and GGA
|
||||
uint8_t fields[] = {CAS_NEMA_RMC, CAS_NEMA_GGA};
|
||||
for (unsigned int i = 0; i < sizeof(fields); i++) {
|
||||
// Construct a CAS-CFG-MSG packet
|
||||
uint8_t cas_cfg_msg_packet[] = {0x4e, fields[i], 0x01, 0x00};
|
||||
msglen = makeCASPacket(buffer, 0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet);
|
||||
uart.writeBytes(buffer, msglen);
|
||||
if (getACKCas(uart, 0x06, 0x01, 250) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "ATGM336H: Could not enable NMEA MSG: %d", fields[i]);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initMtkPa1616s(uart::Uart& uart) {
|
||||
// PA1616S is used in some GPS breakout boards from Adafruit
|
||||
// PA1616S does not have GLONASS capability. PA1616D does, but is not implemented here.
|
||||
uart.writeString("$PMTK353,1,0,0,0,0*2A\r\n");
|
||||
// Above command will reset the GPS and takes longer before it will accept new commands
|
||||
kernel::delayMillis(1000);
|
||||
// Only ask for RMC and GGA (GNRMC and GNGGA)
|
||||
uart.writeString("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Enable SBAS / WAAS
|
||||
uart.writeString("$PMTK301,2*2E\r\n");
|
||||
kernel::delayMillis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initMtkL76b(uart::Uart& uart) {
|
||||
// Waveshare Pico-GPS hat uses the L76B with 9600 baud
|
||||
// Initialize the L76B Chip, use GPS + GLONASS
|
||||
// See note in L76_Series_GNSS_Protocol_Specification, chapter 3.29
|
||||
uart.writeString("$PMTK353,1,1,0,0,0*2B\r\n");
|
||||
// Above command will reset the GPS and takes longer before it will accept new commands
|
||||
kernel::delayMillis(1000);
|
||||
// only ask for RMC and GGA (GNRMC and GNGGA)
|
||||
// See note in L76_Series_GNSS_Protocol_Specification, chapter 2.1
|
||||
uart.writeString("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Enable SBAS
|
||||
uart.writeString("$PMTK301,2*2E\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Enable PPS for 2D/3D fix only
|
||||
uart.writeString("$PMTK285,3,100*3F\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
|
||||
uart.writeString("$PMTK886,1*29\r\n");
|
||||
kernel::delayMillis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initMtk(uart::Uart& uart) {
|
||||
// Initialize the L76K Chip, use GPS + GLONASS + BEIDOU
|
||||
uart.writeString("$PCAS04,7*1E\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// only ask for RMC and GGA
|
||||
uart.writeString("$PCAS03,1,0,0,0,1,0,0,0,0,0,,,0,0*02\r\n");
|
||||
kernel::delayMillis(250);
|
||||
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
|
||||
uart.writeString("$PCAS11,3*1E\r\n");
|
||||
kernel::delayMillis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,140 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/hal/gps/Ublox.h"
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/hal/uart/Uart.h>
|
||||
#include <Tactility/kernel/Kernel.h>
|
||||
#include <cstring>
|
||||
|
||||
#define 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(uart::Uart& 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) {
|
||||
if (uart.available()) {
|
||||
uart.readByte(&b);
|
||||
|
||||
#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_DEBUG("Found: %s", message); // Log the found message
|
||||
#endif
|
||||
return GpsResponse::Ok;
|
||||
} else {
|
||||
bytesRead = 0;
|
||||
#ifdef GPS_DEBUG
|
||||
LOG_DEBUG(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 { \
|
||||
TT_LOG_I(TAG, "Probing for %s (%s)", CHIP, TOWRITE); \
|
||||
UART.flushInput(); \
|
||||
UART.writeString(TOWRITE "\r\n", TIMEOUT); \
|
||||
if (getAck(UART, RESPONSE, TIMEOUT) == GpsResponse::Ok) { \
|
||||
TT_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(uart::Uart& uart) {
|
||||
// Close all NMEA sentences, valid for L76K, ATGM336H (and likely other AT6558 devices)
|
||||
uart.writeString("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
|
||||
kernel::delayMillis(20);
|
||||
|
||||
// Close NMEA sequences on Ublox
|
||||
uart.writeString("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n");
|
||||
uart.writeString("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n");
|
||||
uart.writeString("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n");
|
||||
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.writeString("$PAIR062,2,0*3C\r\n"); // GSA OFF to reduce volume
|
||||
uart.writeString("$PAIR062,3,0*3D\r\n"); // GSV OFF to reduce volume
|
||||
uart.writeString("$PAIR513*3D\r\n"); // 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.writeString("$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n");
|
||||
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 {
|
||||
TT_LOG_W(TAG, "No GNSS Module (baudrate %lu)", uart.getBaudRate());
|
||||
return GpsModel::Unknown;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,114 @@
|
||||
#include "Tactility/hal/gps/Satellites.h"
|
||||
|
||||
#include <algorithm>
|
||||
|
||||
#define TAG "satellites"
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
constexpr inline bool hasTimeElapsed(TickType_t now, TickType_t timeInThePast, TickType_t expireTimeInTicks) {
|
||||
return (TickType_t)(now - timeInThePast) >= expireTimeInTicks;
|
||||
}
|
||||
|
||||
SatelliteStorage::SatelliteRecord* SatelliteStorage::findRecord(int number) {
|
||||
auto result = records | std::views::filter([number](auto& record) {
|
||||
return record.inUse && record.data.nr == number;
|
||||
});
|
||||
|
||||
if (!result.empty()) {
|
||||
return &result.front();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
SatelliteStorage::SatelliteRecord* SatelliteStorage::findUnusedRecord() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
auto result = records | std::views::filter([](auto& record) {
|
||||
return !record.inUse;
|
||||
});
|
||||
|
||||
if (!result.empty()) {
|
||||
auto* record = &result.front();
|
||||
record->inUse = true;
|
||||
TT_LOG_D(TAG, "Found unused record");
|
||||
return record;
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
SatelliteStorage::SatelliteRecord* SatelliteStorage::findRecordToRecycle() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
int candidate_index = -1;
|
||||
auto candidate_age = portMAX_DELAY;
|
||||
TickType_t expire_duration = kernel::secondsToTicks(recycleTimeSeconds);
|
||||
TickType_t now = kernel::getTicks();
|
||||
for (int i = 0; i < records.size(); ++i) {
|
||||
// First try to find a record that is "old enough"
|
||||
if (hasTimeElapsed(now, records[i].lastUpdated, expire_duration)) {
|
||||
TT_LOG_D(TAG, "! [%d] %lu < %lu", i, records[i].lastUpdated, expire_duration);
|
||||
candidate_index = i;
|
||||
break;
|
||||
}
|
||||
|
||||
// Otherwise keep finding the oldest record
|
||||
if (records[i].inUse && records[i].lastUpdated < candidate_age) {
|
||||
candidate_index = i;
|
||||
candidate_age = records[i].lastUpdated;
|
||||
TT_LOG_D(TAG, "? [%d] %lu < %lu", i, records[i].lastUpdated, candidate_age);
|
||||
}
|
||||
}
|
||||
|
||||
assert(candidate_index != -1);
|
||||
|
||||
TT_LOG_D(TAG, "Recycled record %d", candidate_index);
|
||||
|
||||
return &records[candidate_index];
|
||||
}
|
||||
|
||||
SatelliteStorage::SatelliteRecord* SatelliteStorage::findWithFallback(int number) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (auto* found_record = findRecord(number)) {
|
||||
return found_record;
|
||||
} else if (auto* unused_record = findUnusedRecord()) {
|
||||
return unused_record;
|
||||
} else {
|
||||
return findRecordToRecycle();
|
||||
}
|
||||
}
|
||||
|
||||
void SatelliteStorage::notify(const minmea_sat_info& data) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
auto* record = findWithFallback(data.nr);
|
||||
if (record != nullptr) {
|
||||
record->inUse = true;
|
||||
record->lastUpdated = kernel::getTicks();
|
||||
record->data = data;
|
||||
TT_LOG_D(TAG, "Updated satellite %d: elevation %d, azimuth %d, snr %d", record->data.nr, record->data.elevation, record->data.elevation, record->data.snr);
|
||||
}
|
||||
}
|
||||
|
||||
void SatelliteStorage::getRecords(const std::function<void(const minmea_sat_info&)>& onRecord) const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
TickType_t expire_duration = kernel::secondsToTicks(recentTimeSeconds);
|
||||
TickType_t now = kernel::getTicks();
|
||||
|
||||
for (auto& record: records) {
|
||||
if (record.inUse && !hasTimeElapsed(now, record.lastUpdated, expire_duration)) {
|
||||
onRecord(record.data);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,480 @@
|
||||
#include "Tactility/hal/gps/Ublox.h"
|
||||
#include "Tactility/hal/gps/UbloxMessages.h"
|
||||
#include "Tactility/hal/uart/Uart.h"
|
||||
#include <cstring>
|
||||
|
||||
#define TAG "ublox"
|
||||
|
||||
namespace tt::hal::gps::ublox {
|
||||
|
||||
bool initUblox6(uart::Uart& uart);
|
||||
bool initUblox789(uart::Uart& uart, GpsModel model);
|
||||
bool initUblox10(uart::Uart& uart);
|
||||
|
||||
#define SEND_UBX_PACKET(UART, BUFFER, TYPE, ID, DATA, ERRMSG, TIMEOUT) \
|
||||
do { \
|
||||
auto msglen = makePacket(TYPE, ID, DATA, sizeof(DATA), BUFFER); \
|
||||
UART.writeBytes(BUFFER, sizeof(BUFFER)); \
|
||||
if (getAck(UART, TYPE, ID, TIMEOUT) != GpsResponse::Ok) { \
|
||||
TT_LOG_I(TAG, "Sending packet failed: %s", #ERRMSG); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
void checksum(uint8_t* message, size_t length) {
|
||||
uint8_t CK_A = 0, CK_B = 0;
|
||||
|
||||
// Calculate the checksum, starting from the CLASS field (which is message[2])
|
||||
for (size_t i = 2; i < length - 2; i++) {
|
||||
CK_A = (CK_A + message[i]) & 0xFF;
|
||||
CK_B = (CK_B + CK_A) & 0xFF;
|
||||
}
|
||||
|
||||
// Place the calculated checksum values in the message
|
||||
message[length - 2] = CK_A;
|
||||
message[length - 1] = CK_B;
|
||||
}
|
||||
|
||||
uint8_t makePacket(uint8_t classId, uint8_t messageId, const uint8_t* payload, uint8_t payloadSize, uint8_t* bufferOut) {
|
||||
// Construct the UBX packet
|
||||
bufferOut[0] = 0xB5U; // header
|
||||
bufferOut[1] = 0x62U; // header
|
||||
bufferOut[2] = classId; // class
|
||||
bufferOut[3] = messageId; // id
|
||||
bufferOut[4] = payloadSize; // length
|
||||
bufferOut[5] = 0x00U;
|
||||
|
||||
bufferOut[6 + payloadSize] = 0x00U; // CK_A
|
||||
bufferOut[7 + payloadSize] = 0x00U; // CK_B
|
||||
|
||||
for (int i = 0; i < payloadSize; i++) {
|
||||
bufferOut[6 + i] = payload[i];
|
||||
}
|
||||
checksum(bufferOut, (payloadSize + 8U));
|
||||
return (payloadSize + 8U);
|
||||
}
|
||||
|
||||
GpsResponse getAck(uart::Uart& uart, uint8_t class_id, uint8_t msg_id, uint32_t waitMillis) {
|
||||
uint8_t b;
|
||||
uint8_t ack = 0;
|
||||
const uint8_t ackP[2] = {class_id, msg_id};
|
||||
uint8_t buf[10] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00};
|
||||
uint32_t startTime = kernel::getMillis();
|
||||
const char frame_errors[] = "More than 100 frame errors";
|
||||
int sCounter = 0;
|
||||
#ifdef GPS_DEBUG
|
||||
std::string debugmsg = "";
|
||||
#endif
|
||||
|
||||
for (int j = 2; j < 6; j++) {
|
||||
buf[8] += buf[j];
|
||||
buf[9] += buf[8];
|
||||
}
|
||||
|
||||
for (int j = 0; j < 2; j++) {
|
||||
buf[6 + j] = ackP[j];
|
||||
buf[8] += buf[6 + j];
|
||||
buf[9] += buf[8];
|
||||
}
|
||||
|
||||
while (kernel::getTicks() - startTime < waitMillis) {
|
||||
if (ack > 9) {
|
||||
#ifdef GPS_DEBUG
|
||||
TT_LOG_I(TAG, "Got ACK for class %02X message %02X in %lums", class_id, msg_id, kernel::getMillis() - startTime);
|
||||
#endif
|
||||
return GpsResponse::Ok; // ACK received
|
||||
}
|
||||
if (uart.available()) {
|
||||
uart.readByte(&b);
|
||||
if (b == frame_errors[sCounter]) {
|
||||
sCounter++;
|
||||
if (sCounter == 26) {
|
||||
#ifdef GPS_DEBUG
|
||||
|
||||
TT_LOG_I(TAG, "%s", debugmsg.c_str());
|
||||
#endif
|
||||
return GpsResponse::FrameErrors;
|
||||
}
|
||||
} else {
|
||||
sCounter = 0;
|
||||
}
|
||||
#ifdef GPS_DEBUG
|
||||
debugmsg += std::format("%02X", b);
|
||||
#endif
|
||||
if (b == buf[ack]) {
|
||||
ack++;
|
||||
} else {
|
||||
if (ack == 3 && b == 0x00) { // UBX-ACK-NAK message
|
||||
#ifdef GPS_DEBUG
|
||||
TT_LOG_I(TAG, "%s", debugmsg.c_str());
|
||||
#endif
|
||||
TT_LOG_W(TAG, "Got NAK for class %02X message %02X", class_id, msg_id);
|
||||
return GpsResponse::NotAck; // NAK received
|
||||
}
|
||||
ack = 0; // Reset the acknowledgement counter
|
||||
}
|
||||
}
|
||||
}
|
||||
#ifdef GPS_DEBUG
|
||||
TT_LOG_I(TAG, "%s", debugmsg.c_str());
|
||||
TT_LOG_W(TAG, "No response for class %02X message %02X", class_id, msg_id);
|
||||
#endif
|
||||
return GpsResponse::None; // No response received within timeout
|
||||
}
|
||||
|
||||
static int getAck(uart::Uart& uart, uint8_t* buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedId, TickType_t timeout) {
|
||||
uint16_t ubxFrameCounter = 0;
|
||||
uint32_t startTime = kernel::getTicks();
|
||||
uint16_t needRead = 0;
|
||||
|
||||
while (kernel::getTicks() - startTime < timeout) {
|
||||
while (uart.available()) {
|
||||
uint8_t c;
|
||||
uart.readByte(&c);
|
||||
|
||||
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: {
|
||||
// Payload length msb
|
||||
needRead |= (c << 8);
|
||||
ubxFrameCounter++;
|
||||
// Check for buffer overflow
|
||||
if (needRead >= size) {
|
||||
ubxFrameCounter = 0;
|
||||
break;
|
||||
}
|
||||
auto read_bytes = uart.readBytes(buffer, needRead, 250 / portTICK_PERIOD_MS);
|
||||
if (read_bytes != needRead) {
|
||||
ubxFrameCounter = 0;
|
||||
} else {
|
||||
// return payload length
|
||||
#ifdef GPS_DEBUG
|
||||
TT_LOG_I(TAG, "Got ACK for class %02X message %02X in %lums", requestedClass, requestedId, kernel::getMillis() - startTime);
|
||||
#endif
|
||||
return needRead;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
#define DETECTED_MESSAGE "%s detected, using %s Module"
|
||||
|
||||
static struct uBloxGnssModelInfo {
|
||||
char swVersion[30];
|
||||
char hwVersion[10];
|
||||
uint8_t extensionNo;
|
||||
char extension[10][30];
|
||||
uint8_t protocol_version;
|
||||
} ublox_info;
|
||||
|
||||
GpsModel probe(uart::Uart& uart) {
|
||||
TT_LOG_I(TAG, "Probing for U-blox");
|
||||
|
||||
uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x00, 0x00};
|
||||
checksum(cfg_rate, sizeof(cfg_rate));
|
||||
uart.flushInput();
|
||||
uart.writeBytes(cfg_rate, sizeof(cfg_rate));
|
||||
// Check that the returned response class and message ID are correct
|
||||
GpsResponse response = getAck(uart, 0x06, 0x08, 750);
|
||||
if (response == GpsResponse::None) {
|
||||
TT_LOG_W(TAG, "No GNSS Module (baudrate %lu)", uart.getBaudRate());
|
||||
return GpsModel::Unknown;
|
||||
} else if (response == GpsResponse::FrameErrors) {
|
||||
TT_LOG_W(TAG, "UBlox Frame Errors (baudrate %lu)", uart.getBaudRate());
|
||||
}
|
||||
|
||||
uint8_t buffer[256];
|
||||
memset(buffer, 0, sizeof(buffer));
|
||||
uint8_t _message_MONVER[8] = {
|
||||
0xB5, 0x62, // Sync message for UBX protocol
|
||||
0x0A, 0x04, // Message class and ID (UBX-MON-VER)
|
||||
0x00, 0x00, // Length of payload (we're asking for an answer, so no payload)
|
||||
0x00, 0x00 // Checksum
|
||||
};
|
||||
// Get Ublox gnss module hardware and software info
|
||||
checksum(_message_MONVER, sizeof(_message_MONVER));
|
||||
uart.flushInput();
|
||||
uart.writeBytes(_message_MONVER, sizeof(_message_MONVER));
|
||||
|
||||
uint16_t ack_response_len = getAck(uart, buffer, sizeof(buffer), 0x0A, 0x04, 1200);
|
||||
if (ack_response_len) {
|
||||
uint16_t position = 0;
|
||||
for (char& i: ublox_info.swVersion) {
|
||||
i = buffer[position];
|
||||
position++;
|
||||
}
|
||||
for (char& i: ublox_info.hwVersion) {
|
||||
i = buffer[position];
|
||||
position++;
|
||||
}
|
||||
|
||||
while (ack_response_len >= position + 30) {
|
||||
for (int i = 0; i < 30; i++) {
|
||||
ublox_info.extension[ublox_info.extensionNo][i] = buffer[position];
|
||||
position++;
|
||||
}
|
||||
ublox_info.extensionNo++;
|
||||
if (ublox_info.extensionNo > 9)
|
||||
break;
|
||||
}
|
||||
|
||||
TT_LOG_I(TAG, "Module Info : ");
|
||||
TT_LOG_I(TAG, "Soft version: %s", ublox_info.swVersion);
|
||||
TT_LOG_I(TAG, "Hard version: %s", ublox_info.hwVersion);
|
||||
TT_LOG_I(TAG, "Extensions:%d", ublox_info.extensionNo);
|
||||
for (int i = 0; i < ublox_info.extensionNo; i++) {
|
||||
TT_LOG_I(TAG, " %s", ublox_info.extension[i]);
|
||||
}
|
||||
|
||||
memset(buffer, 0, sizeof(buffer));
|
||||
|
||||
// tips: extensionNo field is 0 on some 6M GNSS modules
|
||||
for (int i = 0; i < ublox_info.extensionNo; ++i) {
|
||||
if (!strncmp(ublox_info.extension[i], "MOD=", 4)) {
|
||||
strncpy((char*)buffer, &(ublox_info.extension[i][4]), sizeof(buffer));
|
||||
} else if (!strncmp(ublox_info.extension[i], "PROTVER", 7)) {
|
||||
char* ptr = nullptr;
|
||||
memset(buffer, 0, sizeof(buffer));
|
||||
strncpy((char*)buffer, &(ublox_info.extension[i][8]), sizeof(buffer));
|
||||
TT_LOG_I(TAG, "Protocol Version:%s", (char*)buffer);
|
||||
if (strlen((char*)buffer)) {
|
||||
ublox_info.protocol_version = strtoul((char*)buffer, &ptr, 10);
|
||||
TT_LOG_I(TAG, "ProtVer=%d", ublox_info.protocol_version);
|
||||
} else {
|
||||
ublox_info.protocol_version = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (strncmp(ublox_info.hwVersion, "00040007", 8) == 0) {
|
||||
TT_LOG_I(TAG, DETECTED_MESSAGE, "U-blox 6", "6");
|
||||
return GpsModel::UBLOX6;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00070000", 8) == 0) {
|
||||
TT_LOG_I(TAG, DETECTED_MESSAGE, "U-blox 7", "7");
|
||||
return GpsModel::UBLOX7;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00080000", 8) == 0) {
|
||||
TT_LOG_I(TAG, DETECTED_MESSAGE, "U-blox 8", "8");
|
||||
return GpsModel::UBLOX8;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00190000", 8) == 0) {
|
||||
TT_LOG_I(TAG, DETECTED_MESSAGE, "U-blox 9", "9");
|
||||
return GpsModel::UBLOX9;
|
||||
} else if (strncmp(ublox_info.hwVersion, "000A0000", 8) == 0) {
|
||||
TT_LOG_I(TAG, DETECTED_MESSAGE, "U-blox 10", "10");
|
||||
return GpsModel::UBLOX10;
|
||||
}
|
||||
}
|
||||
|
||||
return GpsModel::Unknown;
|
||||
}
|
||||
|
||||
bool init(uart::Uart& uart, GpsModel model) {
|
||||
TT_LOG_I(TAG, "U-blox init");
|
||||
switch (model) {
|
||||
case GpsModel::UBLOX6:
|
||||
return initUblox6(uart);
|
||||
case GpsModel::UBLOX7:
|
||||
case GpsModel::UBLOX8:
|
||||
case GpsModel::UBLOX9:
|
||||
return initUblox789(uart, model);
|
||||
case GpsModel::UBLOX10:
|
||||
return initUblox10(uart);
|
||||
default:
|
||||
TT_LOG_E(TAG, "Unknown or unsupported U-blox model");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool initUblox10(uart::Uart& uart) {
|
||||
uint8_t buffer[256];
|
||||
kernel::delayMillis(1000);
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_RAM, "disable NMEA messages in M10 RAM", 300);
|
||||
kernel::delayMillis(750);
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_BBR, "disable NMEA messages in M10 BBR", 300);
|
||||
kernel::delayMillis(750);
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_RAM, "disable Info messages for M10 GPS RAM", 300);
|
||||
kernel::delayMillis(750);
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_BBR, "disable Info messages for M10 GPS BBR", 300);
|
||||
kernel::delayMillis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_PM_RAM, "enable powersave for M10 GPS RAM", 300);
|
||||
kernel::delayMillis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_PM_BBR, "enable powersave for M10 GPS BBR", 300);
|
||||
kernel::delayMillis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ITFM_RAM, "enable jam detection M10 GPS RAM", 300);
|
||||
kernel::delayMillis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ITFM_BBR, "enable jam detection M10 GPS BBR", 300);
|
||||
kernel::delayMillis(750);
|
||||
// Here is where the init commands should go to do further M10 initialization.
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_SBAS_RAM, "disable SBAS M10 GPS RAM", 300);
|
||||
kernel::delayMillis(750); // will cause a receiver restart so wait a bit
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_SBAS_BBR, "disable SBAS M10 GPS BBR", 300);
|
||||
kernel::delayMillis(750); // will cause a receiver restart so wait a bit
|
||||
|
||||
// Done with initialization
|
||||
|
||||
// Enable wanted NMEA messages in BBR layer so they will survive a periodic sleep
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ENABLE_NMEA_BBR, "enable messages for M10 GPS BBR", 300);
|
||||
kernel::delayMillis(750);
|
||||
// Enable wanted NMEA messages in RAM layer
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ENABLE_NMEA_RAM, "enable messages for M10 GPS RAM", 500);
|
||||
kernel::delayMillis(750);
|
||||
|
||||
// As the M10 has no flash, the best we can do to preserve the config is to set it in RAM and BBR.
|
||||
// BBR will survive a restart, and power off for a while, but modules with small backup
|
||||
// batteries or super caps will not retain the config for a long power off time.
|
||||
auto packet_size = makePacket(0x06, 0x09, _message_SAVE_10, sizeof(_message_SAVE_10), buffer);
|
||||
uart.writeBytes(buffer, packet_size);
|
||||
if (getAck(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
TT_LOG_I(TAG, "GNSS module configuration saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initUblox789(uart::Uart& uart, GpsModel model) {
|
||||
uint8_t buffer[256];
|
||||
if (model == GpsModel::UBLOX7) {
|
||||
TT_LOG_D(TAG, "Set GPS+SBAS");
|
||||
auto msglen = makePacket(0x06, 0x3e, _message_GNSS_7, sizeof(_message_GNSS_7), buffer);
|
||||
uart.writeBytes(buffer, msglen);
|
||||
} else { // 8,9
|
||||
auto msglen = makePacket(0x06, 0x3e, _message_GNSS_8, sizeof(_message_GNSS_8), buffer);
|
||||
uart.writeBytes(buffer, msglen);
|
||||
}
|
||||
|
||||
if (getAck(uart, 0x06, 0x3e, 800) == GpsResponse::NotAck) {
|
||||
// It's not critical if the module doesn't acknowledge this configuration.
|
||||
TT_LOG_D(TAG, "reconfigure GNSS - defaults maintained. Is this module GPS-only?");
|
||||
} else {
|
||||
if (model == GpsModel::UBLOX7) {
|
||||
TT_LOG_I(TAG, "GPS+SBAS configured");
|
||||
} else { // 8,9
|
||||
TT_LOG_I(TAG, "GPS+SBAS+GLONASS+Galileo configured");
|
||||
}
|
||||
// Documentation say, we need wait at least 0.5s after reconfiguration of GNSS module, before sending next
|
||||
// commands for the M8 it tends to be more. 1 sec should be enough
|
||||
kernel::delayMillis(1000);
|
||||
}
|
||||
|
||||
uart.flushInput();
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
|
||||
|
||||
if (model == GpsModel::UBLOX8) { // 8
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_8, "enable interference resistance", 500);
|
||||
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5_8, "configure NAVX5_8 settings", 500);
|
||||
} else { // 6,7,9
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
|
||||
}
|
||||
|
||||
// Turn off unwanted NMEA messages, set update rate
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
|
||||
|
||||
if (ublox_info.protocol_version >= 18) {
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x86, _message_PMS, "enable powersave for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
|
||||
// For M8 we want to enable NMEA version 4.10 so we can see the additional satellites.
|
||||
if (model == GpsModel::UBLOX8) {
|
||||
uart.flushInput();
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x17, _message_NMEA, "enable NMEA 4.10", 500);
|
||||
}
|
||||
} else {
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x11, _message_CFG_RXM_PSM, "enable powersave mode for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
}
|
||||
|
||||
auto packet_size = makePacket(0x06, 0x09, _message_SAVE, sizeof(_message_SAVE), buffer);
|
||||
uart.writeBytes(buffer, packet_size);
|
||||
if (getAck(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
TT_LOG_I(TAG, "GNSS module configuration saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initUblox6(uart::Uart& uart) {
|
||||
uint8_t buffer[256];
|
||||
|
||||
uart.flushInput();
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
|
||||
|
||||
// Turn off unwanted NMEA messages, set update rate
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
|
||||
|
||||
uart.flushInput();
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x11, _message_CFG_RXM_ECO, "enable powersave ECO mode for Neo-6", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_AID, "disable UBX-AID", 500);
|
||||
|
||||
auto packet_size = makePacket(0x06, 0x09, _message_SAVE, sizeof(_message_SAVE), buffer);
|
||||
uart.writeBytes(buffer, packet_size);
|
||||
if (getAck(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
TT_LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
TT_LOG_I(TAG, "GNSS module config saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps::ublox
|
||||
Reference in New Issue
Block a user