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
@@ -16,6 +16,7 @@ namespace tt {
|
||||
|
||||
#define TAG "tactility"
|
||||
|
||||
namespace service::gps { extern const ServiceManifest manifest; }
|
||||
namespace service::wifi { extern const ServiceManifest manifest; }
|
||||
namespace service::sdcard { extern const ServiceManifest manifest; }
|
||||
|
||||
@@ -25,6 +26,7 @@ static const hal::Configuration* hardwareConfig = nullptr;
|
||||
|
||||
static void registerAndStartSystemServices() {
|
||||
TT_LOG_I(TAG, "Registering and starting system services");
|
||||
addService(service::gps::manifest);
|
||||
addService(service::sdcard::manifest);
|
||||
addService(service::wifi::manifest);
|
||||
}
|
||||
|
||||
@@ -1,5 +1,6 @@
|
||||
#include "Tactility/hal/Configuration.h"
|
||||
#include "Tactility/hal/Device.h"
|
||||
#include "Tactility/hal/gps/GpsInit.h"
|
||||
#include "Tactility/hal/i2c/I2c.h"
|
||||
#include "Tactility/hal/power/PowerDevice.h"
|
||||
#include "Tactility/hal/spi/Spi.h"
|
||||
@@ -28,6 +29,8 @@ void init(const Configuration& configuration) {
|
||||
tt_check(uart::init(configuration.uart), "UART init failed");
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitUartEnd);
|
||||
|
||||
tt_check(gps::init(configuration.gps), "GPS init failed");
|
||||
|
||||
if (configuration.initBoot != nullptr) {
|
||||
TT_LOG_I(TAG, "Init power");
|
||||
tt_check(configuration.initBoot(), "Init power failed");
|
||||
|
||||
@@ -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
|
||||
@@ -4,6 +4,8 @@
|
||||
#include <Tactility/LogMessages.h>
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
#include <sstream>
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
#include <esp_check.h>
|
||||
#endif
|
||||
@@ -35,16 +37,16 @@ static const char* initModeToString(InitMode mode) {
|
||||
}
|
||||
|
||||
static void printInfo(const Data& data) {
|
||||
TT_LOG_V(TAG, "UART info for port %d", data.configuration.port);
|
||||
TT_LOG_V(TAG, " isStarted: %d", data.isStarted);
|
||||
TT_LOG_V(TAG, " isConfigured: %d", data.isConfigured);
|
||||
TT_LOG_V(TAG, " initMode: %s", initModeToString(data.configuration.initMode));
|
||||
TT_LOG_V(TAG, " canReinit: %d", data.configuration.canReinit);
|
||||
TT_LOG_V(TAG, " hasMutableConfiguration: %d", data.configuration.hasMutableConfiguration);
|
||||
TT_LOG_V(TAG, " RX pin: %d", data.configuration.rxPin);
|
||||
TT_LOG_V(TAG, " TX pin: %d", data.configuration.txPin);
|
||||
TT_LOG_V(TAG, " RTS pin: %d", data.configuration.rtsPin);
|
||||
TT_LOG_V(TAG, " CTS pin: %d", data.configuration.ctsPin);
|
||||
TT_LOG_D(TAG, "UART info for port %d", data.configuration.port);
|
||||
TT_LOG_D(TAG, " isStarted: %d", data.isStarted);
|
||||
TT_LOG_D(TAG, " isConfigured: %d", data.isConfigured);
|
||||
TT_LOG_D(TAG, " initMode: %s", initModeToString(data.configuration.initMode));
|
||||
TT_LOG_D(TAG, " canReinit: %d", data.configuration.canReinit);
|
||||
TT_LOG_D(TAG, " hasMutableConfiguration: %d", data.configuration.hasMutableConfiguration);
|
||||
TT_LOG_D(TAG, " RX pin: %d", data.configuration.rxPin);
|
||||
TT_LOG_D(TAG, " TX pin: %d", data.configuration.txPin);
|
||||
TT_LOG_D(TAG, " RTS pin: %d", data.configuration.rtsPin);
|
||||
TT_LOG_D(TAG, " CTS pin: %d", data.configuration.ctsPin);
|
||||
}
|
||||
|
||||
bool init(const std::vector<uart::Configuration>& configurations) {
|
||||
@@ -118,6 +120,7 @@ static bool startLocked(uart_port_t port) {
|
||||
#else
|
||||
intr_alloc_flags = 0;
|
||||
#endif
|
||||
|
||||
esp_err_t result = uart_param_config(config.port, &config.config);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Failed to configure: %s", port, esp_err_to_name(result));
|
||||
@@ -174,13 +177,11 @@ static bool stopLocked(uart_port_t port) {
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Failed to delete driver: %s", port, esp_err_to_name(result));
|
||||
return false;
|
||||
} else {
|
||||
data.isStarted = false;
|
||||
}
|
||||
#else
|
||||
data.isStarted = true;
|
||||
#endif // ESP_PLATFORM
|
||||
|
||||
data.isStarted = false;
|
||||
|
||||
TT_LOG_I(TAG, "(%d) Stopped", port);
|
||||
return true;
|
||||
}
|
||||
@@ -231,6 +232,10 @@ size_t read(uart_port_t port, uint8_t* buffer, size_t bufferSize, TickType_t tim
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool readByte(uart_port_t port, uint8_t* output, TickType_t timeout) {
|
||||
return read(port, output, 1, timeout) == 1;
|
||||
}
|
||||
|
||||
size_t write(uart_port_t port, const uint8_t* buffer, size_t bufferSize, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
if (lock(port, timeout)) {
|
||||
@@ -244,4 +249,114 @@ size_t write(uart_port_t port, const uint8_t* buffer, size_t bufferSize, TickTyp
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool writeString(uart_port_t port, const char* buffer, TickType_t timeout) {
|
||||
while (*buffer != 0) {
|
||||
if (write(port, (const uint8_t*)buffer, 1, timeout)) {
|
||||
buffer++;
|
||||
} else {
|
||||
TT_LOG_E(TAG, "Failed to write - breaking off");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t available(uart_port_t port, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
size_t size = 0;
|
||||
if (lock(port, timeout)) {
|
||||
uart_get_buffered_data_len(port, &size);
|
||||
unlock(port);
|
||||
return size;
|
||||
} else {
|
||||
TT_LOG_E(TAG, LOG_MESSAGE_MUTEX_LOCK_FAILED_FMT, "write()");
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
return 0;
|
||||
}
|
||||
|
||||
void flush(uart_port_t port, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
size_t size = 0;
|
||||
if (lock(port, timeout)) {
|
||||
uart_flush(port);
|
||||
unlock(port);
|
||||
} else {
|
||||
TT_LOG_E(TAG, LOG_MESSAGE_MUTEX_LOCK_FAILED_FMT, "write()");
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
void flushInput(uart_port_t port, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
size_t size = 0;
|
||||
if (lock(port, timeout)) {
|
||||
uart_flush_input(port);
|
||||
unlock(port);
|
||||
} else {
|
||||
TT_LOG_E(TAG, LOG_MESSAGE_MUTEX_LOCK_FAILED_FMT, "write()");
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
uint32_t getBaudRate(uart_port_t port) {
|
||||
#ifdef ESP_PLATFORM
|
||||
uint32_t baud_rate = 0;
|
||||
uart_get_baudrate(port, &baud_rate);
|
||||
return baud_rate;
|
||||
#else
|
||||
return 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
bool setBaudRate(uart_port_t port, uint32_t baudRate, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
if (lock(port, timeout)) {
|
||||
uart_set_baudrate(port, baudRate);
|
||||
unlock(port);
|
||||
} else {
|
||||
TT_LOG_E(TAG, LOG_MESSAGE_MUTEX_LOCK_FAILED_FMT, "write()");
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
return true;
|
||||
}
|
||||
|
||||
bool readUntil(uart_port_t port, uint8_t* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout) {
|
||||
bool success = false;
|
||||
size_t index = 0;
|
||||
size_t index_limit = bufferSize - 1;
|
||||
while (readByte(port, buffer, timeout) && index < index_limit) {
|
||||
if (*buffer == untilByte) {
|
||||
success = true;
|
||||
// We have the extra space because index < index_limit
|
||||
if (buffer++) {
|
||||
*buffer = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
buffer++;
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
std::string readStringUntil(uart_port_t port, char untilChar, TickType_t timeout) {
|
||||
std::stringstream output;
|
||||
char buffer;
|
||||
bool success = false;
|
||||
while (readByte(port, (uint8_t*)&buffer, timeout)) {
|
||||
if (buffer == untilChar) {
|
||||
success = true;
|
||||
break;
|
||||
}
|
||||
output << buffer;
|
||||
}
|
||||
|
||||
if (success) {
|
||||
return output.str();
|
||||
} else {
|
||||
return {};
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace tt::hal::uart
|
||||
|
||||
@@ -0,0 +1,38 @@
|
||||
#include "Tactility/service/ServiceManifest.h"
|
||||
#include "Tactility/service/ServiceRegistry.h"
|
||||
#include "Tactility/service/gps/GpsService.h"
|
||||
|
||||
using tt::hal::gps::GpsDevice;
|
||||
|
||||
namespace tt::service::gps {
|
||||
|
||||
extern ServiceManifest manifest;
|
||||
|
||||
static std::shared_ptr<GpsService> findGpsService() {
|
||||
auto service = findServiceById(manifest.id);
|
||||
assert(service != nullptr);
|
||||
return std::static_pointer_cast<GpsService>(service);
|
||||
}
|
||||
|
||||
|
||||
void addGpsDevice(const std::shared_ptr<GpsDevice>& device) {
|
||||
return findGpsService()->addGpsDevice(device);
|
||||
}
|
||||
|
||||
void removeGpsDevice(const std::shared_ptr<GpsDevice>& device) {
|
||||
return findGpsService()->removeGpsDevice(device);
|
||||
}
|
||||
|
||||
bool startReceiving() {
|
||||
return findGpsService()->startReceiving();
|
||||
}
|
||||
|
||||
void stopReceiving() {
|
||||
findGpsService()->stopReceiving();
|
||||
}
|
||||
|
||||
bool isReceiving() {
|
||||
return findGpsService()->isReceiving();
|
||||
}
|
||||
|
||||
} // namespace tt::service::gps
|
||||
@@ -0,0 +1,163 @@
|
||||
#include "Tactility/service/gps/GpsService.h"
|
||||
#include "Tactility/service/ServiceManifest.h"
|
||||
|
||||
#define TAG "gps_service"
|
||||
|
||||
using tt::hal::gps::GpsDevice;
|
||||
|
||||
namespace tt::service::gps {
|
||||
|
||||
GpsService::GpsDeviceRecord* _Nullable GpsService::findGpsRecord(const std::shared_ptr<GpsDevice>& device) {
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
auto result = std::views::filter(deviceRecords, [&device](auto& record){
|
||||
return record.device.get() == device.get();
|
||||
});
|
||||
if (!result.empty()) {
|
||||
return &result.front();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void GpsService::addGpsDevice(const std::shared_ptr<GpsDevice>& device) {
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
GpsDeviceRecord record = { .device = device };
|
||||
|
||||
if (isReceiving()) {
|
||||
startGpsDevice(record);
|
||||
}
|
||||
|
||||
deviceRecords.push_back(record);
|
||||
}
|
||||
|
||||
void GpsService::removeGpsDevice(const std::shared_ptr<GpsDevice>& device) {
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
GpsDeviceRecord* record = findGpsRecord(device);
|
||||
|
||||
if (isReceiving()) {
|
||||
stopGpsDevice(*record);
|
||||
}
|
||||
|
||||
std::erase_if(deviceRecords, [&device](auto& reference){
|
||||
return reference.device.get() == device.get();
|
||||
});
|
||||
}
|
||||
|
||||
void GpsService::onStart(tt::service::ServiceContext &serviceContext) {
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
deviceRecords.clear();
|
||||
|
||||
auto devices = hal::findDevices<GpsDevice>(hal::Device::Type::Gps);
|
||||
for (auto& device : devices) {
|
||||
TT_LOG_I(TAG, "[device %lu] added", device->getId());
|
||||
addGpsDevice(device);
|
||||
}
|
||||
}
|
||||
|
||||
void GpsService::onStop(tt::service::ServiceContext &serviceContext) {
|
||||
if (isReceiving()) {
|
||||
stopReceiving();
|
||||
}
|
||||
}
|
||||
|
||||
bool GpsService::startGpsDevice(GpsDeviceRecord& record) {
|
||||
TT_LOG_I(TAG, "[device %lu] starting", record.device->getId());
|
||||
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
auto device = record.device;
|
||||
|
||||
if (!device->start()) {
|
||||
TT_LOG_E(TAG, "[device %lu] starting failed", record.device->getId());
|
||||
return false;
|
||||
}
|
||||
|
||||
record.satelliteSubscriptionId = device->subscribeSatellites([this](hal::Device::Id deviceId, auto& record) {
|
||||
onSatelliteInfo(deviceId, record);
|
||||
});
|
||||
|
||||
record.locationSubscriptionId = device->subscribeLocations([this](hal::Device::Id deviceId, auto& record) {
|
||||
onRmcSentence(deviceId, record);
|
||||
});
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsService::stopGpsDevice(GpsDeviceRecord& record) {
|
||||
TT_LOG_I(TAG, "[device %lu] stopping", record.device->getId());
|
||||
|
||||
auto device = record.device;
|
||||
|
||||
if (!device->stop()) {
|
||||
TT_LOG_E(TAG, "[device %lu] stopping failed", record.device->getId());
|
||||
return false;
|
||||
}
|
||||
|
||||
device->unsubscribeSatellites(record.satelliteSubscriptionId);
|
||||
device->unsubscribeLocations(record.locationSubscriptionId);
|
||||
|
||||
record.satelliteSubscriptionId = -1;
|
||||
record.locationSubscriptionId = -1;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool GpsService::startReceiving() {
|
||||
TT_LOG_I(TAG, "Start receiving");
|
||||
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
bool started_one_or_more = false;
|
||||
|
||||
for (auto& record : deviceRecords) {
|
||||
started_one_or_more |= startGpsDevice(record);
|
||||
}
|
||||
|
||||
receiving = started_one_or_more;
|
||||
|
||||
return receiving;
|
||||
}
|
||||
|
||||
void GpsService::stopReceiving() {
|
||||
TT_LOG_I(TAG, "Stop receiving");
|
||||
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
|
||||
for (auto& record : deviceRecords) {
|
||||
stopGpsDevice(record);
|
||||
}
|
||||
|
||||
receiving = false;
|
||||
}
|
||||
|
||||
bool GpsService::isReceiving() {
|
||||
auto lockable = mutex.scoped();
|
||||
lockable->lock();
|
||||
return receiving;
|
||||
}
|
||||
|
||||
void GpsService::onSatelliteInfo(hal::Device::Id deviceId, const minmea_sat_info& info) {
|
||||
TT_LOG_I(TAG, "[device %lu] satellite %d", deviceId, info.nr);
|
||||
}
|
||||
|
||||
void GpsService::onRmcSentence(hal::Device::Id deviceId, const minmea_sentence_rmc& rmc) {
|
||||
TT_LOG_I(TAG, "[device %lu] coordinates %f %f", deviceId, minmea_tofloat(&rmc.longitude), minmea_tofloat(&rmc.latitude));
|
||||
}
|
||||
|
||||
extern const ServiceManifest manifest = {
|
||||
.id = "Gps",
|
||||
.createService = create<GpsService>
|
||||
};
|
||||
|
||||
} // tt::hal::gps
|
||||
Reference in New Issue
Block a user