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,95 @@
|
||||
#include "Tactility/hal/Device.h"
|
||||
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
namespace tt::hal {
|
||||
|
||||
std::vector<std::shared_ptr<Device>> devices;
|
||||
Mutex mutex = Mutex(Mutex::Type::Recursive);
|
||||
static Device::Id nextId = 0;
|
||||
|
||||
#define TAG "devices"
|
||||
|
||||
Device::Device() : id(nextId++) {}
|
||||
|
||||
template <std::ranges::range RangeType>
|
||||
auto toVector(RangeType&& range) {
|
||||
auto view = range | std::views::common;
|
||||
return std::vector(view.begin(), view.end());
|
||||
}
|
||||
|
||||
void registerDevice(const std::shared_ptr<Device>& device) {
|
||||
auto scoped_mutex = mutex.asScopedLock();
|
||||
scoped_mutex.lock();
|
||||
|
||||
if (findDevice(device->getId()) == nullptr) {
|
||||
devices.push_back(device);
|
||||
TT_LOG_I(TAG, "Registered %s with id %lu", device->getName().c_str(), device->getId());
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Device %s with id %lu was already registered", device->getName().c_str(), device->getId());
|
||||
}
|
||||
}
|
||||
|
||||
void deregisterDevice(const std::shared_ptr<Device>& device) {
|
||||
auto scoped_mutex = mutex.asScopedLock();
|
||||
scoped_mutex.lock();
|
||||
|
||||
auto id_to_remove = device->getId();
|
||||
auto remove_iterator = std::remove_if(devices.begin(), devices.end(), [id_to_remove](const auto& device) {
|
||||
return device->getId() == id_to_remove;
|
||||
});
|
||||
if (remove_iterator != devices.end()) {
|
||||
TT_LOG_I(TAG, "Deregistering %s with id %lu", device->getName().c_str(), device->getId());
|
||||
devices.erase(remove_iterator);
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Deregistering %s with id %lu failed: not found", device->getName().c_str(), device->getId());
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::shared_ptr<Device>> findDevices(const std::function<bool(const std::shared_ptr<Device>&)>& filterFunction) {
|
||||
auto scoped_mutex = mutex.asScopedLock();
|
||||
scoped_mutex.lock();
|
||||
|
||||
auto devices_view = devices | std::views::filter([&filterFunction](auto& device) {
|
||||
return filterFunction(device);
|
||||
});
|
||||
return toVector(devices_view);
|
||||
}
|
||||
|
||||
std::shared_ptr<Device> _Nullable findDevice(const std::function<bool(const std::shared_ptr<Device>&)>& filterFunction) {
|
||||
auto scoped_mutex = mutex.asScopedLock();
|
||||
scoped_mutex.lock();
|
||||
|
||||
auto result_set = devices | std::views::filter([&filterFunction](auto& device) {
|
||||
return filterFunction(device);
|
||||
});
|
||||
if (!result_set.empty()) {
|
||||
return result_set.front();
|
||||
} else {
|
||||
return nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<Device> _Nullable findDevice(std::string name) {
|
||||
return findDevice([&name](auto& device){
|
||||
return device->getName() == name;
|
||||
});
|
||||
}
|
||||
|
||||
std::shared_ptr<Device> _Nullable findDevice(Device::Id id) {
|
||||
return findDevice([id](auto& device){
|
||||
return device->getId() == id;
|
||||
});
|
||||
}
|
||||
|
||||
std::vector<std::shared_ptr<Device>> findDevices(Device::Type type) {
|
||||
return findDevices([type](auto& device) {
|
||||
return device->getType() == type;
|
||||
});
|
||||
}
|
||||
|
||||
std::vector<std::shared_ptr<Device>> getDevices() {
|
||||
return devices;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,53 @@
|
||||
#include "Tactility/hal/Configuration.h"
|
||||
#include "Tactility/hal/Device.h"
|
||||
#include "Tactility/hal/gps/GpsInit.h"
|
||||
#include "Tactility/hal/i2c/I2cInit.h"
|
||||
#include "Tactility/hal/power/PowerDevice.h"
|
||||
#include "Tactility/hal/spi/SpiInit.h"
|
||||
#include "Tactility/hal/uart/UartInit.h"
|
||||
|
||||
#include <Tactility/kernel/SystemEvents.h>
|
||||
|
||||
#define TAG "hal"
|
||||
|
||||
#define TT_SDCARD_MOUNT_POINT "/sdcard"
|
||||
|
||||
namespace tt::hal {
|
||||
|
||||
void init(const Configuration& configuration) {
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitHalBegin);
|
||||
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitI2cBegin);
|
||||
tt_check(i2c::init(configuration.i2c), "I2C init failed");
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitI2cEnd);
|
||||
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitSpiBegin);
|
||||
tt_check(spi::init(configuration.spi), "SPI init failed");
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitSpiEnd);
|
||||
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitUartBegin);
|
||||
tt_check(uart::init(configuration.uart), "UART init failed");
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitUartEnd);
|
||||
|
||||
if (configuration.initBoot != nullptr) {
|
||||
TT_LOG_I(TAG, "Init power");
|
||||
tt_check(configuration.initBoot(), "Init power failed");
|
||||
}
|
||||
|
||||
if (configuration.sdcard != nullptr) {
|
||||
TT_LOG_I(TAG, "Mounting sdcard");
|
||||
if (!configuration.sdcard->mount(TT_SDCARD_MOUNT_POINT)) {
|
||||
TT_LOG_W(TAG, "SD card mount failed (init can continue)");
|
||||
}
|
||||
hal::registerDevice(configuration.sdcard);
|
||||
}
|
||||
|
||||
if (configuration.power != nullptr) {
|
||||
std::shared_ptr<tt::hal::power::PowerDevice> power = configuration.power();
|
||||
hal::registerDevice(power);
|
||||
}
|
||||
|
||||
kernel::systemEventPublish(kernel::SystemEvent::BootInitHalEnd);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -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
|
||||
@@ -0,0 +1,287 @@
|
||||
#include "Tactility/hal/i2c/I2c.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
#include <esp_check.h>
|
||||
#endif // ESP_PLATFORM
|
||||
|
||||
#define TAG "i2c"
|
||||
|
||||
namespace tt::hal::i2c {
|
||||
|
||||
struct Data {
|
||||
Mutex mutex;
|
||||
bool isConfigured = false;
|
||||
bool isStarted = false;
|
||||
Configuration configuration;
|
||||
};
|
||||
|
||||
static const uint8_t ACK_CHECK_EN = 1;
|
||||
static Data dataArray[I2C_NUM_MAX];
|
||||
|
||||
bool init(const std::vector<i2c::Configuration>& configurations) {
|
||||
TT_LOG_I(TAG, "Init");
|
||||
for (const auto& configuration: configurations) {
|
||||
#ifdef ESP_PLATFORM
|
||||
if (configuration.config.mode != I2C_MODE_MASTER) {
|
||||
TT_LOG_E(TAG, "Currently only master mode is supported");
|
||||
return false;
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
Data& data = dataArray[configuration.port];
|
||||
data.configuration = configuration;
|
||||
data.isConfigured = true;
|
||||
}
|
||||
|
||||
for (const auto& config: configurations) {
|
||||
if (config.initMode == InitMode::ByTactility) {
|
||||
if (!start(config.port)) {
|
||||
return false;
|
||||
}
|
||||
} else if (config.initMode == InitMode::ByExternal) {
|
||||
dataArray[config.port].isStarted = true;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool configure(i2c_port_t port, const i2c_config_t& configuration) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[port];
|
||||
if (data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Cannot reconfigure while interface is started", port);
|
||||
return false;
|
||||
} else if (!data.configuration.isMutable) {
|
||||
TT_LOG_E(TAG, "(%d) Mutation not allowed because configuration is immutable", port);
|
||||
return false;
|
||||
} else {
|
||||
data.configuration.config = configuration;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool start(i2c_port_t port) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[port];
|
||||
Configuration& config = data.configuration;
|
||||
|
||||
if (data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Already started", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!data.isConfigured) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Not configured", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_err_t result = i2c_param_config(port, &config.config);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Failed to configure: %s", port, esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
|
||||
result = i2c_driver_install(port, config.config.mode, 0, 0, 0);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Failed to install driver: %s", port, esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
|
||||
data.isStarted = true;
|
||||
|
||||
TT_LOG_I(TAG, "(%d) Started", port);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stop(i2c_port_t port) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[port];
|
||||
Configuration& config = data.configuration;
|
||||
|
||||
if (!config.isMutable) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Not allowed for immutable configuration", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Not started", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_err_t result = i2c_driver_delete(port);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Failed to delete driver: %s", port, esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
#endif // ESP_PLATFORM
|
||||
|
||||
data.isStarted = false;
|
||||
|
||||
TT_LOG_I(TAG, "(%d) Stopped", port);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isStarted(i2c_port_t port) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
lock.lock();
|
||||
return dataArray[port].isStarted;
|
||||
}
|
||||
|
||||
bool masterRead(i2c_port_t port, uint8_t address, uint8_t* data, size_t dataSize, TickType_t timeout) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
auto result = i2c_master_read_from_device(port, address, data, dataSize, timeout);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return result == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterReadRegister(i2c_port_t port, uint8_t address, uint8_t reg, uint8_t* data, size_t dataSize, TickType_t timeout) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||||
// Set address pointer
|
||||
i2c_master_start(cmd);
|
||||
i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
|
||||
i2c_master_write(cmd, ®, 1, ACK_CHECK_EN);
|
||||
// Read length of response from current pointer
|
||||
i2c_master_start(cmd);
|
||||
i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_READ, ACK_CHECK_EN);
|
||||
if (dataSize > 1) {
|
||||
i2c_master_read(cmd, data, dataSize - 1, I2C_MASTER_ACK);
|
||||
}
|
||||
i2c_master_read_byte(cmd, data + dataSize - 1, I2C_MASTER_NACK);
|
||||
i2c_master_stop(cmd);
|
||||
// TODO: We're passing an inaccurate timeout value as we already lost time with locking
|
||||
esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
|
||||
i2c_cmd_link_delete(cmd);
|
||||
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
|
||||
return result == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterWrite(i2c_port_t port, uint8_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
auto result = i2c_master_write_to_device(port, address, data, dataSize, timeout);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return result == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterWriteRegister(i2c_port_t port, uint8_t address, uint8_t reg, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
|
||||
tt_check(reg != 0);
|
||||
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
i2c_cmd_handle_t cmd = i2c_cmd_link_create();
|
||||
i2c_master_start(cmd);
|
||||
i2c_master_write_byte(cmd, (address << 1) | I2C_MASTER_WRITE, ACK_CHECK_EN);
|
||||
i2c_master_write_byte(cmd, reg, ACK_CHECK_EN);
|
||||
i2c_master_write(cmd, (uint8_t*) data, dataSize, ACK_CHECK_EN);
|
||||
i2c_master_stop(cmd);
|
||||
// TODO: We're passing an inaccurate timeout value as we already lost time with locking
|
||||
esp_err_t result = i2c_master_cmd_begin(port, cmd, timeout);
|
||||
i2c_cmd_link_delete(cmd);
|
||||
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return result == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterWriteRegisterArray(i2c_port_t port, uint8_t address, const uint8_t* data, uint16_t dataSize, TickType_t timeout) {
|
||||
#ifdef ESP_PLATFORM
|
||||
assert(dataSize % 2 == 0);
|
||||
bool result = true;
|
||||
for (int i = 0; i < dataSize; i += 2) {
|
||||
// TODO: We're passing an inaccurate timeout value as we already lost time with locking and previous writes in this loop
|
||||
if (!masterWriteRegister(port, address, data[i], &data[i + 1], 1, timeout)) {
|
||||
result = false;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterWriteRead(i2c_port_t port, uint8_t address, const uint8_t* writeData, size_t writeDataSize, uint8_t* readData, size_t readDataSize, TickType_t timeout) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
esp_err_t result = i2c_master_write_read_device(port, address, writeData, writeDataSize, readData, readDataSize, timeout);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return result == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
bool masterHasDeviceAtAddress(i2c_port_t port, uint8_t address, TickType_t timeout) {
|
||||
auto lock = getLock(port).asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
TT_LOG_E(TAG, "(%d) Mutex timeout", port);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
uint8_t message[2] = { 0, 0 };
|
||||
// TODO: We're passing an inaccurate timeout value as we already lost time with locking
|
||||
return i2c_master_write_to_device(port, address, message, 2, timeout) == ESP_OK;
|
||||
#else
|
||||
return false;
|
||||
#endif // ESP_PLATFORM
|
||||
}
|
||||
|
||||
Lock& getLock(i2c_port_t port) {
|
||||
return dataArray[port].mutex;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,65 @@
|
||||
#include "Tactility/hal/i2c/I2cDevice.h"
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
namespace tt::hal::i2c {
|
||||
|
||||
bool I2cDevice::readRegister12(uint8_t reg, float& out) const {
|
||||
std::uint8_t data[2] = {0};
|
||||
if (tt::hal::i2c::masterReadRegister(port, address, reg, data, 2, DEFAULT_TIMEOUT)) {
|
||||
out = (data[0] & 0x0F) << 8 | data[1];
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool I2cDevice::readRegister14(uint8_t reg, float& out) const {
|
||||
std::uint8_t data[2] = {0};
|
||||
if (tt::hal::i2c::masterReadRegister(port, address, reg, data, 2, DEFAULT_TIMEOUT)) {
|
||||
out = (data[0] & 0x3F) << 8 | data[1];
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool I2cDevice::readRegister16(uint8_t reg, uint16_t& out) const {
|
||||
std::uint8_t data[2] = {0};
|
||||
if (tt::hal::i2c::masterReadRegister(port, address, reg, data, 2, DEFAULT_TIMEOUT)) {
|
||||
out = data[0] << 8 | data[1];
|
||||
return true;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool I2cDevice::readRegister8(uint8_t reg, uint8_t& result) const {
|
||||
return tt::hal::i2c::masterWriteRead(port, address, ®, 1, &result, 1, DEFAULT_TIMEOUT);
|
||||
}
|
||||
|
||||
bool I2cDevice::writeRegister8(uint8_t reg, uint8_t value) const {
|
||||
return tt::hal::i2c::masterWriteRegister(port, address, reg, &value, 1, DEFAULT_TIMEOUT);
|
||||
}
|
||||
|
||||
bool I2cDevice::bitOn(uint8_t reg, uint8_t bitmask) const {
|
||||
uint8_t state;
|
||||
if (readRegister8(reg, state)) {
|
||||
state |= bitmask;
|
||||
return writeRegister8(reg, state);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool I2cDevice::bitOff(uint8_t reg, uint8_t bitmask) const {
|
||||
uint8_t state;
|
||||
if (readRegister8(reg, state)) {
|
||||
state &= ~bitmask;
|
||||
return writeRegister8(reg, state);
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,17 @@
|
||||
#include "Tactility/hal/Device.h"
|
||||
#include "Tactility/hal/sdcard/SdCardDevice.h"
|
||||
|
||||
namespace tt::hal::sdcard {
|
||||
|
||||
std::shared_ptr<SdCardDevice> _Nullable find(const std::string& path) {
|
||||
auto sdcards = findDevices<SdCardDevice>(Device::Type::SdCard);
|
||||
for (auto& sdcard : sdcards) {
|
||||
if (sdcard->isMounted() && path.starts_with(sdcard->getMountPath())) {
|
||||
return sdcard;
|
||||
}
|
||||
}
|
||||
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/sdcard/SpiSdCardDevice.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
|
||||
#include <driver/gpio.h>
|
||||
#include <esp_vfs_fat.h>
|
||||
#include <sdmmc_cmd.h>
|
||||
|
||||
#define TAG "spi_sdcard"
|
||||
|
||||
namespace tt::hal::sdcard {
|
||||
|
||||
/**
|
||||
* Before we can initialize the sdcard's SPI communications, we have to set all
|
||||
* other SPI pins on the board high.
|
||||
* See https://github.com/espressif/esp-idf/issues/1597
|
||||
* See https://github.com/Xinyuan-LilyGO/T-Deck/blob/master/examples/UnitTest/UnitTest.ino
|
||||
* @return success result
|
||||
*/
|
||||
bool SpiSdCardDevice::applyGpioWorkAround() {
|
||||
TT_LOG_D(TAG, "init");
|
||||
|
||||
uint64_t pin_bit_mask = BIT64(config->spiPinCs);
|
||||
for (auto const& pin: config->csPinWorkAround) {
|
||||
pin_bit_mask |= BIT64(pin);
|
||||
}
|
||||
|
||||
gpio_config_t sd_gpio_config = {
|
||||
.pin_bit_mask = pin_bit_mask,
|
||||
.mode = GPIO_MODE_OUTPUT,
|
||||
.pull_up_en = GPIO_PULLUP_DISABLE,
|
||||
.pull_down_en = GPIO_PULLDOWN_DISABLE,
|
||||
.intr_type = GPIO_INTR_DISABLE,
|
||||
};
|
||||
|
||||
if (gpio_config(&sd_gpio_config) != ESP_OK) {
|
||||
TT_LOG_E(TAG, "GPIO init failed");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (auto const& pin: config->csPinWorkAround) {
|
||||
if (gpio_set_level(pin, 1) != ESP_OK) {
|
||||
TT_LOG_E(TAG, "Failed to set board CS pin high");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SpiSdCardDevice::mountInternal(const std::string& newMountPath) {
|
||||
TT_LOG_I(TAG, "Mounting %s", newMountPath.c_str());
|
||||
|
||||
esp_vfs_fat_sdmmc_mount_config_t mount_config = {
|
||||
.format_if_mount_failed = config->formatOnMountFailed,
|
||||
.max_files = config->maxOpenFiles,
|
||||
.allocation_unit_size = config->allocUnitSize,
|
||||
.disk_status_check_enable = config->statusCheckEnabled,
|
||||
.use_one_fat = false
|
||||
};
|
||||
|
||||
// Init without card detect (CD) and write protect (WD)
|
||||
sdspi_device_config_t slot_config = SDSPI_DEVICE_CONFIG_DEFAULT();
|
||||
slot_config.host_id = config->spiHost;
|
||||
slot_config.gpio_cs = config->spiPinCs;
|
||||
slot_config.gpio_cd = config->spiPinCd;
|
||||
slot_config.gpio_wp = config->spiPinWp;
|
||||
slot_config.gpio_int = config->spiPinInt;
|
||||
|
||||
sdmmc_host_t host = SDSPI_HOST_DEFAULT();
|
||||
// The following value is from T-Deck repo's UnitTest.ino project:
|
||||
// https://github.com/Xinyuan-LilyGO/T-Deck/blob/master/examples/UnitTest/UnitTest.ino
|
||||
// Observation: Using this automatically sets the bus to 20MHz
|
||||
host.max_freq_khz = config->spiFrequencyKhz;
|
||||
host.slot = config->spiHost;
|
||||
|
||||
esp_err_t result = esp_vfs_fat_sdspi_mount(newMountPath.c_str(), &host, &slot_config, &mount_config, &card);
|
||||
|
||||
if (result != ESP_OK) {
|
||||
if (result == ESP_FAIL) {
|
||||
TT_LOG_E(TAG, "Mounting failed. Ensure the card is formatted with FAT.");
|
||||
} else {
|
||||
TT_LOG_E(TAG, "Mounting failed (%s)", esp_err_to_name(result));
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
mountPath = newMountPath;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool SpiSdCardDevice::mount(const std::string& newMountPath) {
|
||||
if (!applyGpioWorkAround()) {
|
||||
TT_LOG_E(TAG, "Failed to set SPI CS pins high. This is a pre-requisite for mounting.");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (mountInternal(newMountPath)) {
|
||||
sdmmc_card_print_info(stdout, card);
|
||||
return true;
|
||||
} else {
|
||||
TT_LOG_E(TAG, "Mount failed for %s", newMountPath.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool SpiSdCardDevice::unmount() {
|
||||
if (card == nullptr) {
|
||||
TT_LOG_E(TAG, "Can't unmount: not mounted");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (esp_vfs_fat_sdcard_unmount(mountPath.c_str(), card) == ESP_OK) {
|
||||
mountPath = "";
|
||||
card = nullptr;
|
||||
return true;
|
||||
} else {
|
||||
TT_LOG_E(TAG, "Unmount failed for %s", mountPath.c_str());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: Refactor to "bool getStatus(Status* status)" method so that it can fail when the lvgl lock fails
|
||||
SdCardDevice::State SpiSdCardDevice::getState() const {
|
||||
if (card == nullptr) {
|
||||
return State::Unmounted;
|
||||
}
|
||||
|
||||
/**
|
||||
* The SD card and the screen are on the same SPI bus.
|
||||
* Writing and reading to the bus from 2 devices at the same time causes crashes.
|
||||
* This work-around ensures that this check is only happening when LVGL isn't rendering.
|
||||
*/
|
||||
auto lock = getLock().asScopedLock();
|
||||
bool locked = lock.lock(50); // TODO: Refactor to a more reliable locking mechanism
|
||||
if (!locked) {
|
||||
TT_LOG_E(TAG, LOG_MESSAGE_MUTEX_LOCK_FAILED_FMT, "LVGL");
|
||||
return State::Unknown;
|
||||
}
|
||||
|
||||
bool result = sdmmc_get_status(card) == ESP_OK;
|
||||
|
||||
if (result) {
|
||||
return State::Mounted;
|
||||
} else {
|
||||
return State::Error;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,146 @@
|
||||
#include "Tactility/hal/spi/Spi.h"
|
||||
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
#define TAG "spi"
|
||||
|
||||
namespace tt::hal::spi {
|
||||
|
||||
struct Data {
|
||||
std::shared_ptr<Lock> lock;
|
||||
bool isConfigured = false;
|
||||
bool isStarted = false;
|
||||
Configuration configuration;
|
||||
};
|
||||
|
||||
static Data dataArray[SPI_HOST_MAX];
|
||||
|
||||
bool init(const std::vector<spi::Configuration>& configurations) {
|
||||
TT_LOG_I(TAG, "Init");
|
||||
for (const auto& configuration: configurations) {
|
||||
Data& data = dataArray[configuration.device];
|
||||
data.configuration = configuration;
|
||||
data.isConfigured = true;
|
||||
if (configuration.lock != nullptr) {
|
||||
data.lock = configuration.lock;
|
||||
} else {
|
||||
data.lock = std::make_shared<Mutex>();
|
||||
}
|
||||
}
|
||||
|
||||
for (const auto& config: configurations) {
|
||||
if (config.initMode == InitMode::ByTactility) {
|
||||
if (!start(config.device)) {
|
||||
return false;
|
||||
}
|
||||
} else if (config.initMode == InitMode::ByExternal) {
|
||||
dataArray[config.device].isStarted = true;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool configure(spi_host_device_t device, const spi_bus_config_t& configuration) {
|
||||
auto lock = getLock(device)->asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[device];
|
||||
if (data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Cannot reconfigure while interface is started", device);
|
||||
return false;
|
||||
} else if (!data.configuration.isMutable) {
|
||||
TT_LOG_E(TAG, "(%d) Mutation not allowed by original configuration", device);
|
||||
return false;
|
||||
} else {
|
||||
data.configuration.config = configuration;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
bool start(spi_host_device_t device) {
|
||||
auto lock = getLock(device)->asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[device];
|
||||
|
||||
if (data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Already started", device);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!data.isConfigured) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Not configured", device);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
Configuration& config = data.configuration;
|
||||
auto result = spi_bus_initialize(device, &data.configuration.config, data.configuration.dma);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Starting: Failed to initialize: %s", device, esp_err_to_name(result));
|
||||
return false;
|
||||
} else {
|
||||
data.isStarted = true;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
data.isStarted = true;
|
||||
|
||||
#endif
|
||||
|
||||
TT_LOG_I(TAG, "(%d) Started", device);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool stop(spi_host_device_t device) {
|
||||
auto lock = getLock(device)->asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
Data& data = dataArray[device];
|
||||
Configuration& config = data.configuration;
|
||||
|
||||
if (!config.isMutable) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Not allowed, immutable", device);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!data.isStarted) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Not started", device);
|
||||
return false;
|
||||
}
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
auto result = spi_bus_free(device);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "(%d) Stopping: Failed to free device: %s", device, esp_err_to_name(result));
|
||||
return false;
|
||||
} else {
|
||||
data.isStarted = false;
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
data.isStarted = false;
|
||||
|
||||
#endif
|
||||
|
||||
TT_LOG_I(TAG, "(%d) Stopped", device);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isStarted(spi_host_device_t device) {
|
||||
auto lock = getLock(device)->asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
return dataArray[device].isStarted;
|
||||
}
|
||||
|
||||
std::shared_ptr<Lock> getLock(spi_host_device_t device) {
|
||||
return dataArray[device].lock;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,174 @@
|
||||
#include "Tactility/hal/uart/Uart.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
#include <ranges>
|
||||
#include <cstring>
|
||||
|
||||
#ifdef ESP_PLATFORM
|
||||
#include "Tactility/TactilityHeadless.h"
|
||||
#include "Tactility/hal/uart/UartEsp.h"
|
||||
#include <esp_check.h>
|
||||
#else
|
||||
#include "Tactility/hal/uart/UartPosix.h"
|
||||
#include <dirent.h>
|
||||
#endif
|
||||
|
||||
#define TAG "uart"
|
||||
|
||||
namespace tt::hal::uart {
|
||||
|
||||
constexpr uint32_t uartIdNotInUse = 0;
|
||||
|
||||
struct UartEntry {
|
||||
uint32_t usageId = uartIdNotInUse;
|
||||
Configuration configuration;
|
||||
};
|
||||
|
||||
static std::vector<UartEntry> uartEntries = {};
|
||||
static uint32_t lastUartId = uartIdNotInUse;
|
||||
|
||||
bool init(const std::vector<uart::Configuration>& configurations) {
|
||||
TT_LOG_I(TAG, "Init");
|
||||
for (const auto& configuration: configurations) {
|
||||
uartEntries.push_back({
|
||||
.usageId = uartIdNotInUse,
|
||||
.configuration = configuration
|
||||
});
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Uart::writeString(const char* buffer, TickType_t timeout) {
|
||||
auto size = strlen(buffer);
|
||||
writeBytes((std::byte*)buffer, size, timeout);
|
||||
return true;
|
||||
}
|
||||
|
||||
size_t Uart::readUntil(std::byte* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout, bool addNullTerminator) {
|
||||
TickType_t start_time = kernel::getTicks();
|
||||
auto* buffer_write_ptr = reinterpret_cast<uint8_t*>(buffer);
|
||||
uint8_t* buffer_limit = buffer_write_ptr + bufferSize - 1; // Keep 1 extra char as mull terminator
|
||||
TickType_t timeout_left = timeout;
|
||||
while (readByte(reinterpret_cast<std::byte*>(buffer_write_ptr), timeout_left) && buffer_write_ptr < buffer_limit) {
|
||||
#ifdef DEBUG_READ_UNTIL
|
||||
// If first successful read and we're not receiving an empty response
|
||||
if (buffer_write_ptr == buffer && *buffer_write_ptr != 0x00U && *buffer_write_ptr != untilByte) {
|
||||
printf(">>");
|
||||
}
|
||||
#endif
|
||||
|
||||
if (*buffer_write_ptr == untilByte) {
|
||||
// TODO: Fix when untilByte is null terminator char already
|
||||
if (addNullTerminator) {
|
||||
buffer_write_ptr++;
|
||||
*buffer_write_ptr = 0x00U;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
#ifdef DEBUG_READ_UNTIL
|
||||
printf("%c", *buffer_write_ptr);
|
||||
#endif
|
||||
|
||||
buffer_write_ptr++;
|
||||
|
||||
TickType_t now = kernel::getTicks();
|
||||
if (now > (start_time + timeout)) {
|
||||
#ifdef DEBUG_READ_UNTIL
|
||||
TT_LOG_W(TAG, "readUntil() timeout");
|
||||
#endif
|
||||
break;
|
||||
} else {
|
||||
timeout_left = timeout - (now - start_time);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef DEBUG_READ_UNTIL
|
||||
// If we read data and it's not an empty response
|
||||
if (buffer_write_ptr != buffer && *buffer != 0x00U && *buffer != untilByte) {
|
||||
printf("\n");
|
||||
}
|
||||
#endif
|
||||
|
||||
if (addNullTerminator && (buffer_write_ptr > reinterpret_cast<uint8_t*>(buffer))) {
|
||||
return reinterpret_cast<size_t>(buffer_write_ptr) - reinterpret_cast<size_t>(buffer) - 1UL;
|
||||
} else {
|
||||
return reinterpret_cast<size_t>(buffer_write_ptr) - reinterpret_cast<size_t>(buffer);
|
||||
}
|
||||
}
|
||||
|
||||
std::unique_ptr<Uart> open(std::string name) {
|
||||
TT_LOG_I(TAG, "Open %s", name.c_str());
|
||||
|
||||
auto result = std::views::filter(uartEntries, [&name](auto& entry) {
|
||||
return entry.configuration.name == name;
|
||||
});
|
||||
|
||||
if (result.empty()) {
|
||||
TT_LOG_E(TAG, "UART not found: %s", name.c_str());
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
auto& entry = *result.begin();
|
||||
if (entry.usageId != uartIdNotInUse) {
|
||||
TT_LOG_E(TAG, "UART in use: %s", name.c_str());
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
auto uart = create(entry.configuration);
|
||||
assert(uart != nullptr);
|
||||
entry.usageId = uart->getId();
|
||||
TT_LOG_I(TAG, "Opened %lu", entry.usageId);
|
||||
return uart;
|
||||
}
|
||||
|
||||
void close(uint32_t uartId) {
|
||||
TT_LOG_I(TAG, "Close %lu", uartId);
|
||||
auto result = std::views::filter(uartEntries, [&uartId](auto& entry) {
|
||||
return entry.usageId == uartId;
|
||||
});
|
||||
|
||||
if (!result.empty()) {
|
||||
auto& entry = *result.begin();
|
||||
entry.usageId = uartIdNotInUse;
|
||||
} else {
|
||||
TT_LOG_W(TAG, "Auto-closing UART, but can't find it");
|
||||
}
|
||||
}
|
||||
|
||||
std::vector<std::string> getNames() {
|
||||
std::vector<std::string> names;
|
||||
#ifdef ESP_PLATFORM
|
||||
for (auto& config : getConfiguration()->uart) {
|
||||
names.push_back(config.name);
|
||||
}
|
||||
#else
|
||||
DIR* dir = opendir("/dev");
|
||||
if (dir == nullptr) {
|
||||
TT_LOG_E(TAG, "Failed to read /dev");
|
||||
return names;
|
||||
}
|
||||
struct dirent* current_entry;
|
||||
while ((current_entry = readdir(dir)) != nullptr) {
|
||||
auto name = std::string(current_entry->d_name);
|
||||
if (name.starts_with("tty")) {
|
||||
auto path = std::string("/dev/") + name;
|
||||
names.push_back(path);
|
||||
}
|
||||
}
|
||||
|
||||
closedir(dir);
|
||||
#endif
|
||||
return names;
|
||||
}
|
||||
|
||||
Uart::Uart() : id(++lastUartId) {}
|
||||
|
||||
Uart::~Uart() {
|
||||
close(getId());
|
||||
}
|
||||
|
||||
} // namespace tt::hal::uart
|
||||
@@ -0,0 +1,156 @@
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/uart/UartEsp.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/Mutex.h>
|
||||
|
||||
#include <sstream>
|
||||
#include <esp_check.h>
|
||||
|
||||
#define TAG "uart"
|
||||
|
||||
namespace tt::hal::uart {
|
||||
|
||||
bool UartEsp::start() {
|
||||
TT_LOG_I(TAG, "[%s] Starting", configuration.name.c_str());
|
||||
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (started) {
|
||||
TT_LOG_E(TAG, "[%s] Starting: Already started", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
int intr_alloc_flags;
|
||||
#if CONFIG_UART_ISR_IN_IRAM
|
||||
intr_alloc_flags = ESP_INTR_FLAG_IRAM;
|
||||
#else
|
||||
intr_alloc_flags = 0;
|
||||
#endif
|
||||
|
||||
esp_err_t result = uart_param_config(configuration.port, &configuration.config);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "[%s] Starting: Failed to configure: %s", configuration.name.c_str(), esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
|
||||
if (uart_is_driver_installed(configuration.port)) {
|
||||
TT_LOG_W(TAG, "[%s] Driver was still installed. You probably forgot to stop, or another system uses/used the driver.", configuration.name.c_str());
|
||||
uart_driver_delete(configuration.port);
|
||||
}
|
||||
|
||||
result = uart_set_pin(configuration.port, configuration.txPin, configuration.rxPin, configuration.rtsPin, configuration.ctsPin);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "[%s] Starting: Failed set pins: %s", configuration.name.c_str(), esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
|
||||
result = uart_driver_install(configuration.port, (int)configuration.rxBufferSize, (int)configuration.txBufferSize, 0, nullptr, intr_alloc_flags);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "[%s] Starting: Failed to install driver: %s", configuration.name.c_str(), esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
|
||||
started = true;
|
||||
|
||||
TT_LOG_I(TAG, "[%s] Started", configuration.name.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UartEsp::stop() {
|
||||
TT_LOG_I(TAG, "[%s] Stopping", configuration.name.c_str());
|
||||
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (!started) {
|
||||
TT_LOG_E(TAG, "[%s] Stopping: Not started", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
esp_err_t result = uart_driver_delete(configuration.port);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "[%s] Stopping: Failed to delete driver: %s", configuration.name.c_str(), esp_err_to_name(result));
|
||||
return false;
|
||||
}
|
||||
|
||||
started = false;
|
||||
|
||||
TT_LOG_I(TAG, "[%s] Stopped", configuration.name.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UartEsp::isStarted() const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
return started;
|
||||
}
|
||||
|
||||
size_t UartEsp::readBytes(std::byte* buffer, size_t bufferSize, TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto start_time = kernel::getTicks();
|
||||
auto lock_time = kernel::getTicks() - start_time;
|
||||
auto remaining_timeout = std::max(timeout - lock_time, 0UL);
|
||||
auto result = uart_read_bytes(configuration.port, buffer, bufferSize, remaining_timeout);
|
||||
return result;
|
||||
}
|
||||
|
||||
bool UartEsp::readByte(std::byte* output, TickType_t timeout) {
|
||||
return readBytes(output, 1, timeout) == 1;
|
||||
}
|
||||
|
||||
size_t UartEsp::writeBytes(const std::byte* buffer, size_t bufferSize, TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return uart_write_bytes(configuration.port, buffer, bufferSize);
|
||||
}
|
||||
|
||||
size_t UartEsp::available(TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
size_t size = 0;
|
||||
uart_get_buffered_data_len(configuration.port, &size);
|
||||
return size;
|
||||
}
|
||||
|
||||
void UartEsp::flushInput() {
|
||||
uart_flush_input(configuration.port);
|
||||
}
|
||||
|
||||
uint32_t UartEsp::getBaudRate() {
|
||||
uint32_t baud_rate = 0;
|
||||
auto result = uart_get_baudrate(configuration.port, &baud_rate);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return baud_rate;
|
||||
}
|
||||
|
||||
bool UartEsp::setBaudRate(uint32_t baudRate, TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
auto result = uart_set_baudrate(configuration.port, baudRate);
|
||||
ESP_ERROR_CHECK_WITHOUT_ABORT(result);
|
||||
return result == ESP_OK;
|
||||
}
|
||||
|
||||
std::unique_ptr<Uart> create(const Configuration& configuration) {
|
||||
return std::make_unique<UartEsp>(configuration);
|
||||
}
|
||||
|
||||
} // namespace tt::hal::uart
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,191 @@
|
||||
#ifndef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/uart/UartPosix.h"
|
||||
#include "Tactility/hal/uart/Uart.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <sstream>
|
||||
#include <sys/ioctl.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#define TAG "uart"
|
||||
|
||||
namespace tt::hal::uart {
|
||||
|
||||
bool UartPosix::start() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (device != nullptr) {
|
||||
TT_LOG_E(TAG, "[%s] Starting: Already started", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
auto file = fopen(configuration.name.c_str(), "w");
|
||||
if (file == nullptr) {
|
||||
TT_LOG_E(TAG, "[%s] Open device failed", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
auto new_device = std::unique_ptr<FILE, AutoCloseFileDeleter>(file);
|
||||
|
||||
struct termios tty;
|
||||
if (tcgetattr(fileno(file), &tty) < 0) {
|
||||
printf("[%s] tcgetattr failed: %s\n", configuration.name.c_str(), strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
if (cfsetospeed(&tty, (speed_t)configuration.baudRate) == -1) {
|
||||
TT_LOG_E(TAG, "[%s] Setting output speed failed", configuration.name.c_str());
|
||||
}
|
||||
|
||||
if (cfsetispeed(&tty, (speed_t)configuration.baudRate) == -1) {
|
||||
TT_LOG_E(TAG, "[%s] Setting input speed failed", configuration.name.c_str());
|
||||
}
|
||||
|
||||
tty.c_cflag |= (CLOCAL | CREAD); /* ignore modem controls */
|
||||
tty.c_cflag &= ~CSIZE;
|
||||
tty.c_cflag |= CS8; /* 8-bit characters */
|
||||
tty.c_cflag &= ~PARENB; /* no parity bit */
|
||||
tty.c_cflag &= ~CSTOPB; /* only need 1 stop bit */
|
||||
tty.c_cflag &= ~CRTSCTS; /* no hardware flowcontrol */
|
||||
|
||||
tty.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR | ICRNL | IXON);
|
||||
tty.c_lflag &= ~(ECHO | ECHONL | ICANON | ISIG | IEXTEN);
|
||||
tty.c_oflag &= ~OPOST;
|
||||
|
||||
/* fetch bytes as they become available */
|
||||
tty.c_cc[VMIN] = 1;
|
||||
tty.c_cc[VTIME] = 1;
|
||||
|
||||
if (tcsetattr(fileno(file), TCSANOW, &tty) != 0) {
|
||||
printf("[%s] tcsetattr failed: %s\n", configuration.name.c_str(), strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
device = std::move(new_device);
|
||||
|
||||
TT_LOG_I(TAG, "[%s] Started", configuration.name.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UartPosix::stop() {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
|
||||
if (device == nullptr) {
|
||||
TT_LOG_E(TAG, "[%s] Stopping: Not started", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
device = nullptr;
|
||||
|
||||
TT_LOG_I(TAG, "[%s] Stopped", configuration.name.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UartPosix::isStarted() const {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
return device != nullptr;
|
||||
}
|
||||
|
||||
size_t UartPosix::readBytes(std::byte* buffer, size_t bufferSize, TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
if (awaitAvailable(timeout)) {
|
||||
return read(fileno(device.get()), buffer, bufferSize);
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
bool UartPosix::readByte(std::byte* output, TickType_t timeout) {
|
||||
if (awaitAvailable(timeout)) {
|
||||
return read(fileno(device.get()), output, 1) == 1;
|
||||
} else {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
size_t UartPosix::writeBytes(const std::byte* buffer, size_t bufferSize, TickType_t timeout) {
|
||||
if (!mutex.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return write(fileno(device.get()), buffer, bufferSize);
|
||||
}
|
||||
|
||||
size_t UartPosix::available(TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t bytes_available = 0;
|
||||
ioctl(fileno(device.get()), FIONREAD, bytes_available);
|
||||
return bytes_available;
|
||||
}
|
||||
|
||||
void UartPosix::flushInput() {
|
||||
// TODO
|
||||
}
|
||||
|
||||
uint32_t UartPosix::getBaudRate() {
|
||||
struct termios tty;
|
||||
if (tcgetattr(fileno(device.get()), &tty) < 0) {
|
||||
printf("[%s] tcgetattr failed: %s\n", configuration.name.c_str(), strerror(errno));
|
||||
return false;
|
||||
} else {
|
||||
return (uint32_t)cfgetispeed(&tty);
|
||||
}
|
||||
}
|
||||
|
||||
bool UartPosix::setBaudRate(uint32_t baudRate, TickType_t timeout) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
if (!lock.lock(timeout)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
struct termios tty;
|
||||
if (tcgetattr(fileno(device.get()), &tty) < 0) {
|
||||
printf("[%s] tcgetattr failed: %s\n", configuration.name.c_str(), strerror(errno));
|
||||
return false;
|
||||
}
|
||||
|
||||
if (cfsetospeed(&tty, (speed_t)configuration.baudRate) == -1) {
|
||||
TT_LOG_E(TAG, "[%s] Failed to set output speed", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
if (cfsetispeed(&tty, (speed_t)configuration.baudRate) == -1) {
|
||||
TT_LOG_E(TAG, "[%s] Failed to set input speed", configuration.name.c_str());
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UartPosix::awaitAvailable(TickType_t timeout) {
|
||||
auto start_time = kernel::getTicks();
|
||||
do {
|
||||
if (available(timeout) > 0) {
|
||||
return true;
|
||||
}
|
||||
kernel::delayTicks(timeout / 10);
|
||||
} while ((kernel::getTicks() - start()) < timeout);
|
||||
return false;
|
||||
}
|
||||
|
||||
std::unique_ptr<Uart> create(const Configuration& configuration) {
|
||||
return std::make_unique<UartPosix>(configuration);
|
||||
}
|
||||
|
||||
} // namespace tt::hal::uart
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,108 @@
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/usb/Usb.h"
|
||||
#include "Tactility/TactilityHeadless.h"
|
||||
#include "Tactility/hal/sdcard/SpiSdCardDevice.h"
|
||||
#include "Tactility/hal/usb/UsbTusb.h"
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
|
||||
namespace tt::hal::usb {
|
||||
|
||||
#define TAG "usb"
|
||||
|
||||
#define BOOT_FLAG 42
|
||||
|
||||
struct BootMode {
|
||||
uint32_t flag = 0;
|
||||
};
|
||||
|
||||
static Mode currentMode = Mode::Default;
|
||||
static RTC_NOINIT_ATTR BootMode bootMode;
|
||||
|
||||
sdmmc_card_t* _Nullable getCard() {
|
||||
auto sdcard = getConfiguration()->sdcard;
|
||||
if (sdcard == nullptr) {
|
||||
TT_LOG_W(TAG, "No SD card configuration found");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (!sdcard->isMounted()) {
|
||||
TT_LOG_W(TAG, "SD card not mounted");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
auto spi_sdcard = std::static_pointer_cast<sdcard::SpiSdCardDevice>(sdcard);
|
||||
if (spi_sdcard == nullptr) {
|
||||
TT_LOG_W(TAG, "SD card interface is not supported (must be SpiSdCard)");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
auto* card = spi_sdcard->getCard();
|
||||
if (card == nullptr) {
|
||||
TT_LOG_W(TAG, "SD card has no card object available");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return card;
|
||||
}
|
||||
|
||||
static bool canStartNewMode() {
|
||||
return isSupported() && (currentMode == Mode::Default || currentMode == Mode::None);
|
||||
}
|
||||
|
||||
bool isSupported() {
|
||||
return tusbIsSupported();
|
||||
}
|
||||
|
||||
bool startMassStorageWithSdmmc() {
|
||||
if (!canStartNewMode()) {
|
||||
TT_LOG_E(TAG, "Can't start");
|
||||
return false;
|
||||
}
|
||||
|
||||
if (tusbStartMassStorageWithSdmmc()) {
|
||||
currentMode = Mode::MassStorageSdmmc;
|
||||
return true;
|
||||
} else {
|
||||
TT_LOG_E(TAG, "Failed to init mass storage");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void stop() {
|
||||
if (canStartNewMode()) {
|
||||
return;
|
||||
}
|
||||
|
||||
tusbStop();
|
||||
|
||||
currentMode = Mode::None;
|
||||
}
|
||||
|
||||
Mode getMode() {
|
||||
return currentMode;
|
||||
}
|
||||
|
||||
bool canRebootIntoMassStorageSdmmc() {
|
||||
return tusbIsSupported() && getCard() != nullptr;
|
||||
}
|
||||
|
||||
void rebootIntoMassStorageSdmmc() {
|
||||
if (tusbIsSupported()) {
|
||||
bootMode.flag = BOOT_FLAG;
|
||||
esp_restart();
|
||||
}
|
||||
}
|
||||
|
||||
bool isUsbBootMode() {
|
||||
return bootMode.flag == BOOT_FLAG;
|
||||
}
|
||||
|
||||
void resetUsbBootMode() {
|
||||
bootMode.flag = 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,21 @@
|
||||
#ifndef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/usb/Usb.h"
|
||||
|
||||
#define TAG "usb"
|
||||
|
||||
namespace tt::hal::usb {
|
||||
|
||||
bool startMassStorageWithSdmmc() { return false; }
|
||||
void stop() {}
|
||||
Mode getMode() { return Mode::Default; }
|
||||
bool isSupported() { return false; }
|
||||
|
||||
bool canRebootIntoMassStorageSdmmc() { return false; }
|
||||
void rebootIntoMassStorageSdmmc() {}
|
||||
bool isUsbBootMode() { return false; }
|
||||
void resetUsbBootMode() {}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,173 @@
|
||||
#ifdef ESP_PLATFORM
|
||||
|
||||
#include "Tactility/hal/usb/UsbTusb.h"
|
||||
|
||||
#include <sdkconfig.h>
|
||||
|
||||
#if CONFIG_TINYUSB_MSC_ENABLED == 1
|
||||
|
||||
#include <Tactility/Log.h>
|
||||
#include <tinyusb.h>
|
||||
#include <tusb_msc_storage.h>
|
||||
|
||||
#define TAG "usb"
|
||||
#define EPNUM_MSC 1
|
||||
#define TUSB_DESC_TOTAL_LEN (TUD_CONFIG_DESC_LEN + TUD_MSC_DESC_LEN)
|
||||
|
||||
namespace tt::hal::usb {
|
||||
extern sdmmc_card_t* _Nullable getCard();
|
||||
}
|
||||
|
||||
enum {
|
||||
ITF_NUM_MSC = 0,
|
||||
ITF_NUM_TOTAL
|
||||
};
|
||||
|
||||
enum {
|
||||
EDPT_CTRL_OUT = 0x00,
|
||||
EDPT_CTRL_IN = 0x80,
|
||||
|
||||
EDPT_MSC_OUT = 0x01,
|
||||
EDPT_MSC_IN = 0x81,
|
||||
};
|
||||
|
||||
static bool driverInstalled = false;
|
||||
|
||||
static tusb_desc_device_t descriptor_config = {
|
||||
.bLength = sizeof(descriptor_config),
|
||||
.bDescriptorType = TUSB_DESC_DEVICE,
|
||||
.bcdUSB = 0x0200,
|
||||
.bDeviceClass = TUSB_CLASS_MISC,
|
||||
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
|
||||
.bDeviceProtocol = MISC_PROTOCOL_IAD,
|
||||
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
|
||||
.idVendor = 0x303A, // TODO: Espressif VID. Do we need to change this?
|
||||
.idProduct = 0x4002,
|
||||
.bcdDevice = 0x100,
|
||||
.iManufacturer = 0x01,
|
||||
.iProduct = 0x02,
|
||||
.iSerialNumber = 0x03,
|
||||
.bNumConfigurations = 0x01
|
||||
};
|
||||
|
||||
static char const* string_desc_arr[] = {
|
||||
(const char[]) { 0x09, 0x04 }, // 0: is supported language is English (0x0409)
|
||||
"Espressif", // 1: Manufacturer
|
||||
"Tactility Device", // 2: Product
|
||||
"42", // 3: Serials
|
||||
"Tactility Mass Storage", // 4. MSC
|
||||
};
|
||||
|
||||
static uint8_t const msc_fs_configuration_desc[] = {
|
||||
// Config number, interface count, string index, total length, attribute, power in mA
|
||||
TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, TUSB_DESC_TOTAL_LEN, TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
|
||||
|
||||
// Interface number, string index, EP Out & EP In address, EP size
|
||||
TUD_MSC_DESCRIPTOR(ITF_NUM_MSC, 0, EDPT_MSC_OUT, EDPT_MSC_IN, 64),
|
||||
};
|
||||
|
||||
#if (TUD_OPT_HIGH_SPEED)
|
||||
static const tusb_desc_device_qualifier_t device_qualifier = {
|
||||
.bLength = sizeof(tusb_desc_device_qualifier_t),
|
||||
.bDescriptorType = TUSB_DESC_DEVICE_QUALIFIER,
|
||||
.bcdUSB = 0x0200,
|
||||
.bDeviceClass = TUSB_CLASS_MISC,
|
||||
.bDeviceSubClass = MISC_SUBCLASS_COMMON,
|
||||
.bDeviceProtocol = MISC_PROTOCOL_IAD,
|
||||
.bMaxPacketSize0 = CFG_TUD_ENDPOINT0_SIZE,
|
||||
.bNumConfigurations = 0x01,
|
||||
.bReserved = 0
|
||||
};
|
||||
|
||||
static uint8_t const msc_hs_configuration_desc[] = {
|
||||
// Config number, interface count, string index, total length, attribute, power in mA
|
||||
TUD_CONFIG_DESCRIPTOR(1, ITF_NUM_TOTAL, 0, TUSB_DESC_TOTAL_LEN, TUSB_DESC_CONFIG_ATT_REMOTE_WAKEUP, 100),
|
||||
|
||||
// Interface number, string index, EP Out & EP In address, EP size
|
||||
TUD_MSC_DESCRIPTOR(ITF_NUM_MSC, 0, EDPT_MSC_OUT, EDPT_MSC_IN, 512),
|
||||
};
|
||||
#endif // TUD_OPT_HIGH_SPEED
|
||||
|
||||
static void storage_mount_changed_cb(tinyusb_msc_event_t* event) {
|
||||
if (event->mount_changed_data.is_mounted) {
|
||||
TT_LOG_I(TAG, "Mounted");
|
||||
} else {
|
||||
TT_LOG_I(TAG, "Unmounted");
|
||||
}
|
||||
}
|
||||
|
||||
static bool ensureDriverInstalled() {
|
||||
if (driverInstalled) {
|
||||
return true;
|
||||
}
|
||||
|
||||
const tinyusb_config_t tusb_cfg = {
|
||||
.device_descriptor = &descriptor_config,
|
||||
.string_descriptor = string_desc_arr,
|
||||
.string_descriptor_count = sizeof(string_desc_arr) / sizeof(string_desc_arr[0]),
|
||||
.external_phy = false,
|
||||
#if (TUD_OPT_HIGH_SPEED)
|
||||
.fs_configuration_descriptor = msc_fs_configuration_desc,
|
||||
.hs_configuration_descriptor = msc_hs_configuration_desc,
|
||||
.qualifier_descriptor = &device_qualifier,
|
||||
#else
|
||||
.configuration_descriptor = msc_fs_configuration_desc,
|
||||
#endif // TUD_OPT_HIGH_SPEED
|
||||
.self_powered = false,
|
||||
.vbus_monitor_io = 0
|
||||
};
|
||||
|
||||
if (tinyusb_driver_install(&tusb_cfg) != ESP_OK) {
|
||||
TT_LOG_E(TAG, "Failed to install TinyUSB driver");
|
||||
return false;
|
||||
}
|
||||
|
||||
driverInstalled = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool tusbIsSupported() { return true; }
|
||||
|
||||
bool tusbStartMassStorageWithSdmmc() {
|
||||
ensureDriverInstalled();
|
||||
|
||||
auto* card = tt::hal::usb::getCard();
|
||||
if (card == nullptr) {
|
||||
TT_LOG_E(TAG, "SD card not mounted");
|
||||
return false;
|
||||
}
|
||||
|
||||
const tinyusb_msc_sdmmc_config_t config_sdmmc = {
|
||||
.card = card,
|
||||
.callback_mount_changed = storage_mount_changed_cb,
|
||||
.callback_premount_changed = nullptr,
|
||||
.mount_config = {
|
||||
.format_if_mount_failed = false,
|
||||
.max_files = 5,
|
||||
.allocation_unit_size = 0,
|
||||
.disk_status_check_enable = false,
|
||||
.use_one_fat = false
|
||||
}
|
||||
};
|
||||
|
||||
auto result = tinyusb_msc_storage_init_sdmmc(&config_sdmmc);
|
||||
if (result != ESP_OK) {
|
||||
TT_LOG_E(TAG, "TinyUSB init failed: %s", esp_err_to_name(result));
|
||||
}
|
||||
|
||||
return result == ESP_OK;
|
||||
}
|
||||
|
||||
void tusbStop() {
|
||||
tinyusb_msc_storage_deinit();
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
bool tusbIsSupported() { return false; }
|
||||
bool tusbStartMassStorageWithSdmmc() { return false; }
|
||||
void tusbStop() {}
|
||||
|
||||
#endif // TinyUSB enabled
|
||||
|
||||
#endif // ESP_PLATFORM
|
||||
Reference in New Issue
Block a user