GPS implementation (#216)
Implemented basic GPS support: - GPS HAL - GPS Service - GPS Settings app
This commit is contained in:
Ken Van Hoeylandt
committed by
GitHub
GitHub
parent
2345ba6d13
commit
14e459e50f
@@ -0,0 +1,152 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include <minmea.h>
|
||||
#include <cstring>
|
||||
|
||||
#define TAG "gps"
|
||||
#define GPS_UART_BUFFER_SIZE 256
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
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];
|
||||
|
||||
if (!uart::setBaudRate(configuration.uartPort, (int)configuration.baudRate)) {
|
||||
TT_LOG_E(TAG, "Failed to set baud rate to %lu", configuration.baudRate);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (!configuration.initFunction(configuration.uartPort)) {
|
||||
TT_LOG_E(TAG, "Failed to init");
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Reference: https://gpsd.gitlab.io/gpsd/NMEA.html
|
||||
while (!isThreadInterrupted()) {
|
||||
if (uart::readUntil(configuration.uartPort, (uint8_t*)buffer, GPS_UART_BUFFER_SIZE, '\n', 100 / portTICK_PERIOD_MS) > 0) {
|
||||
TT_LOG_D(TAG, "RX: %s", buffer);
|
||||
switch (minmea_sentence_id((char*)buffer, false)) {
|
||||
case MINMEA_SENTENCE_RMC:
|
||||
minmea_sentence_rmc frame;
|
||||
if (minmea_parse_rmc(&frame, (char*)buffer)) {
|
||||
for (auto& subscription : locationSubscriptions) {
|
||||
(*subscription.onData)(getId(), frame);
|
||||
}
|
||||
TT_LOG_D(TAG, "RX RMC %f lat, %f lon, %f m/s", minmea_tocoord(&frame.latitude), minmea_tocoord(&frame.longitude), minmea_tofloat(&frame.speed));
|
||||
} else {
|
||||
TT_LOG_W(TAG, "RX RMC parse error: %s", buffer);
|
||||
}
|
||||
break;
|
||||
case MINMEA_SENTENCE_GGA:
|
||||
minmea_sentence_gga gga_frame;
|
||||
if (minmea_parse_gga(&gga_frame, (char*)buffer)) {
|
||||
TT_LOG_D(TAG, "RX GGA %f lat, %f lon", minmea_tocoord(&gga_frame.latitude), minmea_tocoord(&gga_frame.longitude));
|
||||
} else {
|
||||
TT_LOG_W(TAG, "RX GGA parse error: %s", buffer);
|
||||
}
|
||||
break;
|
||||
case MINMEA_SENTENCE_GSV:
|
||||
minmea_sentence_gsv gsv_frame;
|
||||
if (minmea_parse_gsv(&gsv_frame, (char*)buffer)) {
|
||||
for (auto& sat : gsv_frame.sats) {
|
||||
if (sat.nr != 0 && sat.elevation != 0 && sat.snr != 0) {
|
||||
for (auto& subscription : satelliteSubscriptions) {
|
||||
(*subscription.onData)(getId(), sat);
|
||||
}
|
||||
}
|
||||
TT_LOG_D(TAG, "Satellite: id %d, elevation %d, azimuth %d, snr %d", sat.nr, sat.elevation, sat.azimuth, sat.snr);
|
||||
}
|
||||
TT_LOG_D(TAG, "RX GGA %f lat, %f lon", minmea_tocoord(&gga_frame.latitude), minmea_tocoord(&gga_frame.longitude));
|
||||
} else {
|
||||
TT_LOG_W(TAG, "RX GGA parse error: %s", buffer);
|
||||
}
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool GpsDevice::start() {
|
||||
auto scoped_lockable = mutex.scoped();
|
||||
scoped_lockable->lock(portMAX_DELAY);
|
||||
|
||||
if (thread != nullptr && thread->getState() != Thread::State::Stopped) {
|
||||
TT_LOG_W(TAG, "Already started");
|
||||
return true;
|
||||
}
|
||||
|
||||
if (uart::isStarted(configuration.uartPort)) {
|
||||
TT_LOG_E(TAG, "UART %d already in use", configuration.uartPort);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!uart::start(configuration.uartPort)) {
|
||||
TT_LOG_E(TAG, "UART %d failed to start", configuration.uartPort);
|
||||
return false;
|
||||
}
|
||||
|
||||
threadInterrupted = false;
|
||||
|
||||
thread = std::make_unique<Thread>(
|
||||
"gps",
|
||||
4096,
|
||||
threadMainStatic,
|
||||
this
|
||||
);
|
||||
thread->setPriority(tt::Thread::Priority::High);
|
||||
thread->start();
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsDevice::stop() {
|
||||
auto scoped_lockable = mutex.scoped();
|
||||
scoped_lockable->lock();
|
||||
|
||||
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
|
||||
scoped_lockable->unlock();
|
||||
// Wait for thread to finish
|
||||
old_thread->join();
|
||||
// Re-lock to continue logic below
|
||||
scoped_lockable->lock();
|
||||
}
|
||||
}
|
||||
|
||||
if (uart::isStarted(configuration.uartPort)) {
|
||||
if (!uart::stop(configuration.uartPort)) {
|
||||
TT_LOG_E(TAG, "UART %d failed to stop", configuration.uartPort);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsDevice::isStarted() const {
|
||||
auto scoped_lockable = mutex.scoped();
|
||||
scoped_lockable->lock(portMAX_DELAY);
|
||||
return thread != nullptr && thread->getState() != Thread::State::Stopped;
|
||||
}
|
||||
|
||||
bool GpsDevice::isThreadInterrupted() const {
|
||||
auto scoped_lockable = mutex.scoped();
|
||||
scoped_lockable->lock(portMAX_DELAY);
|
||||
return threadInterrupted;
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,214 @@
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/hal/uart/Uart.h>
|
||||
#include <Tactility/kernel/Kernel.h>
|
||||
|
||||
#define TAG "gps"
|
||||
#define GPS_UART_BUFFER_SIZE 256
|
||||
|
||||
using namespace tt;
|
||||
using namespace tt::hal;
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
static int ackGps(uart_port_t port, uint8_t* buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedId) {
|
||||
uint16_t ubxFrameCounter = 0;
|
||||
uint32_t startTime = kernel::getTicks();
|
||||
uint16_t needRead;
|
||||
|
||||
while (kernel::getTicks() - startTime < 800) {
|
||||
while (uart::available(port)) {
|
||||
uint8_t c;
|
||||
uart::readByte(port, &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:
|
||||
needRead |= (c << 8);
|
||||
ubxFrameCounter++;
|
||||
break;
|
||||
case 6:
|
||||
if (needRead >= size) {
|
||||
ubxFrameCounter = 0;
|
||||
break;
|
||||
}
|
||||
if (uart::read(port, buffer, needRead) != needRead) {
|
||||
ubxFrameCounter = 0;
|
||||
} else {
|
||||
return needRead;
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool configureGps(uart_port_t port, uint8_t* buffer, size_t bufferSize) {
|
||||
// According to the reference implementation, L76K GPS uses 9600 baudrate, but the default in the developer device was 38400
|
||||
// https://github.com/Xinyuan-LilyGO/T-Deck/blob/master/examples/GPSShield/GPSShield.ino
|
||||
bool result = false;
|
||||
uint32_t startTimeout;
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
if (!uart::writeString(port, "$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n")) {
|
||||
TT_LOG_E(TAG, "Failed to write init string");
|
||||
}
|
||||
|
||||
kernel::delayMillis(5U);
|
||||
// Get version information
|
||||
startTimeout = kernel::getMillis() + 1000;
|
||||
|
||||
TT_LOG_I(TAG, "Manual flushing of input");
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_rom_printf("Waiting...");
|
||||
#endif
|
||||
while (uart::available(port)) {
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_rom_printf(".");
|
||||
#endif
|
||||
uart::readUntil(port, buffer, bufferSize, '\n');
|
||||
if (kernel::getMillis() > startTimeout) {
|
||||
TT_LOG_E(TAG, "NMEA timeout");
|
||||
return false;
|
||||
}
|
||||
};
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_rom_printf("\n");
|
||||
#endif
|
||||
uart::flush(port);
|
||||
kernel::delayMillis(200);
|
||||
|
||||
if (!uart::writeString(port, "$PCAS06,0*1B\r\n")) {
|
||||
TT_LOG_E(TAG, "Failed to write PCAS06");
|
||||
}
|
||||
|
||||
startTimeout = kernel::getMillis() + 500;
|
||||
while (!uart::available(port)) {
|
||||
if (kernel::getMillis() > startTimeout) {
|
||||
TT_LOG_E(TAG, "L76K timeout");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
auto ver = uart::readStringUntil(port, '\n');
|
||||
if (ver.starts_with("$GPTXT,01,01,02")) {
|
||||
TT_LOG_I(TAG, "L76K GNSS init success");
|
||||
result = true;
|
||||
break;
|
||||
}
|
||||
kernel::delayMillis(500);
|
||||
}
|
||||
// Initialize the L76K Chip, use GPS + GLONASS
|
||||
uart::writeString(port, "$PCAS04,5*1C\r\n");
|
||||
kernel::delayMillis(250);
|
||||
uart::writeString(port, "$PCAS03,1,1,1,1,1,1,1,1,1,1,,,0,0*26\r\n");
|
||||
kernel::delayMillis(250);
|
||||
|
||||
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
|
||||
uart::writeString(port, "$PCAS11,3*1E\r\n");
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static bool recoverGps(uart_port_t port, uint8_t* buffer, size_t bufferSize) {
|
||||
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};
|
||||
|
||||
if (!uart::write(port, cfg_clear1, sizeof(cfg_clear1), 10)) {
|
||||
return false;
|
||||
TT_LOG_E(TAG, "Failed to send ack 1");
|
||||
}
|
||||
|
||||
if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) {
|
||||
TT_LOG_I(TAG, "Ack 1");
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Ack 1 failed");
|
||||
}
|
||||
|
||||
if (!uart::write(port, cfg_clear2, sizeof(cfg_clear2))) {
|
||||
return false;
|
||||
TT_LOG_E(TAG, "Failed to send ack 2");
|
||||
}
|
||||
|
||||
if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) {
|
||||
TT_LOG_I(TAG, "Ack 2");
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Ack 2 failed");
|
||||
}
|
||||
|
||||
if (!uart::write(port, cfg_clear3, sizeof(cfg_clear3))) {
|
||||
TT_LOG_E(TAG, "Failed to send ack 3");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) {
|
||||
TT_LOG_I(TAG, "Ack 3");
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Ack 3 failed");
|
||||
}
|
||||
|
||||
// UBX-CFG-RATE, Size 8, 'Navigation/measurement rate settings'
|
||||
uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30};
|
||||
uart::write(port, cfg_rate, sizeof(cfg_rate));
|
||||
if (ackGps(port, buffer, bufferSize, 0x06, 0x08)) {
|
||||
TT_LOG_I(TAG, "Ack completed");
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool initGpsDefault(uart_port_t port) {
|
||||
uint8_t buffer[GPS_UART_BUFFER_SIZE];
|
||||
if (!configureGps(port, buffer, GPS_UART_BUFFER_SIZE)) {
|
||||
if (!recoverGps(port, buffer, GPS_UART_BUFFER_SIZE)) {
|
||||
uint32_t initial_baud_rate = std::max(uart::getBaudRate(port), (uint32_t)9600U);
|
||||
uart::setBaudRate(port, 9600U);
|
||||
if (!recoverGps(port, buffer, GPS_UART_BUFFER_SIZE)) {
|
||||
TT_LOG_E(TAG, "Recovery repeatedly failed");
|
||||
return false;
|
||||
} else {
|
||||
TT_LOG_I(TAG, "Recovery 2 complete");
|
||||
}
|
||||
uart::setBaudRate(port, initial_baud_rate);
|
||||
} else {
|
||||
TT_LOG_I(TAG, "Recovery 1 complete");
|
||||
}
|
||||
}
|
||||
|
||||
TT_LOG_I(TAG, "Init complete");
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace tt::hal::gps
|
||||
@@ -0,0 +1,15 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/service/Service.h"
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
bool init(const std::vector<GpsDevice::Configuration>& configurations) {
|
||||
for (auto& configuration : configurations) {
|
||||
auto device = std::make_shared<GpsDevice>(configuration);
|
||||
hal::registerDevice(std::move(device));
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // 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 lockable = mutex.scoped();
|
||||
lockable->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 lockable = mutex.scoped();
|
||||
lockable->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 lockable = mutex.scoped();
|
||||
lockable->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 lockable = mutex.scoped();
|
||||
lockable->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 lockable = mutex.scoped();
|
||||
lockable->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
|
||||
Reference in New Issue
Block a user