diff --git a/Boards/LilygoTdeck/Source/LilygoTdeck.cpp b/Boards/LilygoTdeck/Source/LilygoTdeck.cpp index 9ba410be..fddef29a 100644 --- a/Boards/LilygoTdeck/Source/LilygoTdeck.cpp +++ b/Boards/LilygoTdeck/Source/LilygoTdeck.cpp @@ -11,17 +11,19 @@ bool tdeckInit(); -extern const tt::hal::Configuration lilygo_tdeck = { +using namespace tt::hal; + +extern const Configuration lilygo_tdeck = { .initBoot = tdeckInit, .createDisplay = createDisplay, .createKeyboard = createKeyboard, .sdcard = createTdeckSdCard(), .power = tdeck_get_power, .i2c = { - tt::hal::i2c::Configuration { + i2c::Configuration { .name = "Internal", .port = I2C_NUM_0, - .initMode = tt::hal::i2c::InitMode::ByTactility, + .initMode = i2c::InitMode::ByTactility, .canReinit = false, .hasMutableConfiguration = false, .config = (i2c_config_t) { @@ -36,10 +38,10 @@ extern const tt::hal::Configuration lilygo_tdeck = { .clk_flags = 0 } }, - tt::hal::i2c::Configuration { + i2c::Configuration { .name = "External", .port = I2C_NUM_1, - .initMode = tt::hal::i2c::InitMode::Disabled, + .initMode = i2c::InitMode::Disabled, .canReinit = true, .hasMutableConfiguration = true, .config = (i2c_config_t) { @@ -56,7 +58,7 @@ extern const tt::hal::Configuration lilygo_tdeck = { } }, .spi { - tt::hal::spi::Configuration { + spi::Configuration { .device = SPI2_HOST, .dma = SPI_DMA_CH_AUTO, .config = { @@ -75,16 +77,16 @@ extern const tt::hal::Configuration lilygo_tdeck = { .isr_cpu_id = ESP_INTR_CPU_AFFINITY_AUTO, .intr_flags = 0 }, - .initMode = tt::hal::spi::InitMode::ByTactility, + .initMode = spi::InitMode::ByTactility, .canReinit = false, .hasMutableConfiguration = false, .lock = tt::lvgl::getSyncLock() // esp_lvgl_port owns the lock for the display } }, .uart { - tt::hal::uart::Configuration { + uart::Configuration { .port = UART_NUM_1, - .initMode = tt::hal::uart::InitMode::Disabled, // Let GPS driver control this interface + .initMode = uart::InitMode::Disabled, // Let GPS driver control this interface .canReinit = true, .hasMutableConfiguration = false, .rxPin = GPIO_NUM_44, @@ -109,10 +111,11 @@ extern const tt::hal::Configuration lilygo_tdeck = { } }, .gps = { - tt::hal::gps::GpsDevice::Configuration { + gps::GpsDevice::Configuration { .name = "Internal", .uartPort = UART_NUM_1, - .baudRate = 38400 + .baudRate = 38400, + .model = gps::GpsModel::UBLOX10 } } }; diff --git a/COPYRIGHT.md b/COPYRIGHT.md index 3a8ecd44..671b0ceb 100644 --- a/COPYRIGHT.md +++ b/COPYRIGHT.md @@ -45,6 +45,14 @@ Website: https://github.com/kosma/minmea License: [WTFPL](https://github.com/kosma/minmea/blob/master/LICENSE.grants), [LGPL 3.0](https://github.com/kosma/minmea/blob/master/LICENSE.LGPL-3.0), [MIT](https://github.com/kosma/minmea/blob/master/LICENSE.MIT) +### Meshtastic Firmware + +Parts of the Meshtastic firmware were copied and modified for Tactility. + +Website: https://github.com/meshtastic/firmware + +License: [GPL v3.0](https://github.com/meshtastic/firmware/blob/master/LICENSE) + ### Other Components See `/components` for the respective projects and their licenses. diff --git a/Data/system/service/Statusbar/location.png b/Data/system/service/Statusbar/location.png new file mode 100644 index 00000000..1dfe5c31 Binary files /dev/null and b/Data/system/service/Statusbar/location.png differ diff --git a/Data/system_sources/location.svg b/Data/system_sources/location.svg new file mode 100644 index 00000000..97b675d4 --- /dev/null +++ b/Data/system_sources/location.svg @@ -0,0 +1,42 @@ + + + + + + diff --git a/Data/system_sources/sdcard.svg b/Data/system_sources/sdcard.svg deleted file mode 100644 index d250daab..00000000 --- a/Data/system_sources/sdcard.svg +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/Data/system_sources/sdcard_alert.svg b/Data/system_sources/sdcard_alert.svg deleted file mode 100644 index 565c6856..00000000 --- a/Data/system_sources/sdcard_alert.svg +++ /dev/null @@ -1 +0,0 @@ - \ No newline at end of file diff --git a/Tactility/Source/app/gpssettings/GpsSettings.cpp b/Tactility/Source/app/gpssettings/GpsSettings.cpp index bb830b56..11b1555f 100644 --- a/Tactility/Source/app/gpssettings/GpsSettings.cpp +++ b/Tactility/Source/app/gpssettings/GpsSettings.cpp @@ -1,11 +1,16 @@ +#include "Tactility/TactilityHeadless.h" +#include "Tactility/Timer.h" #include "Tactility/app/AppManifest.h" +#include "Tactility/lvgl/LvglSync.h" #include "Tactility/lvgl/Toolbar.h" +#include "Tactility/service/gps/GpsUtil.h" #include "Tactility/service/loader/Loader.h" #include +#include #include -#define TAG "text_viewer" +#define TAG "gps_settings" namespace tt::app::gpssettings { @@ -15,35 +20,175 @@ class GpsSettingsApp final : public App { private: - static void onGpsToggled(lv_event_t* event) { - auto* widget = lv_event_get_target_obj(event); + std::unique_ptr timer; + std::shared_ptr appReference = std::make_shared(this); + lv_obj_t* statusLabelWidget = nullptr; + lv_obj_t* switchWidget = nullptr; + lv_obj_t* spinnerWidget = nullptr; + lv_obj_t* infoContainerWidget = nullptr; + bool hasSetInfo = false; + PubSub::SubscriptionHandle serviceStateSubscription = nullptr; - bool wants_on = lv_obj_has_state(widget, LV_STATE_CHECKED); - bool is_on = service::gps::isReceiving(); + static void onUpdateCallback(TT_UNUSED std::shared_ptr context) { + auto appPtr = std::static_pointer_cast(context); + auto app = *appPtr; + app->updateViews(); + } + + static void onServiceStateChangedCallback(const void* data, void* context) { + auto* app = (GpsSettingsApp*)context; + app->onServiceStateChanged(); + } + + void onServiceStateChanged() { + auto lock = lvgl::getSyncLock()->asScopedLock(); + if (lock.lock(100 / portTICK_PERIOD_MS)) { + if (!updateTimerState()) { + updateViews(); + } + } + } + + static void onGpsToggledCallback(lv_event_t* event) { + auto* app = (GpsSettingsApp*)lv_event_get_user_data(event); + app->onGpsToggled(event); + } + + void startReceivingUpdates() { + timer->start(kernel::secondsToTicks(1)); + updateViews(); + } + + void stopReceivingUpdates() { + timer->stop(); + updateViews(); + } + + void createInfoView(hal::gps::GpsModel model) { + auto* label = lv_label_create(infoContainerWidget); + lv_label_set_text_fmt(label, "Model: %s", toString(model)); + } + + void updateViews() { + auto lock = lvgl::getSyncLock()->asScopedLock(); + if (lock.lock(100 / portTICK_PERIOD_MS)) { + auto state = service::gps::getState(); + + // Update toolbar + switch (state) { + case service::gps::State::OnPending: + TT_LOG_I(TAG, "OnPending"); + lv_obj_remove_flag(spinnerWidget, LV_OBJ_FLAG_HIDDEN); + lv_obj_add_state(switchWidget, LV_STATE_CHECKED); + lv_obj_add_state(switchWidget, LV_STATE_DISABLED); + break; + case service::gps::State::On: + TT_LOG_I(TAG, "On"); + lv_obj_add_flag(spinnerWidget, LV_OBJ_FLAG_HIDDEN); + lv_obj_add_state(switchWidget, LV_STATE_CHECKED); + lv_obj_remove_state(switchWidget, LV_STATE_DISABLED); + break; + case service::gps::State::OffPending: + TT_LOG_I(TAG, "OffPending"); + lv_obj_remove_flag(spinnerWidget, LV_OBJ_FLAG_HIDDEN); + lv_obj_remove_state(switchWidget, LV_STATE_CHECKED); + lv_obj_add_state(switchWidget, LV_STATE_DISABLED); + break; + case service::gps::State::Off: + TT_LOG_I(TAG, "Off"); + lv_obj_add_flag(spinnerWidget, LV_OBJ_FLAG_HIDDEN); + lv_obj_remove_state(switchWidget, LV_STATE_CHECKED); + lv_obj_remove_state(switchWidget, LV_STATE_DISABLED); + break; + } + + // Update status label and device info + if (state == service::gps::State::On) { + if (!hasSetInfo) { + auto devices = hal::findDevices(hal::Device::Type::Gps); + for (auto& device : devices) { + createInfoView(device->getModel()); + hasSetInfo = true; + } + } + + minmea_sentence_rmc rmc; + if (service::gps::getCoordinates(rmc)) { + minmea_float latitude = { rmc.latitude.value, rmc.latitude.scale }; + minmea_float longitude = { rmc.longitude.value, rmc.longitude.scale }; + auto label_text = std::format("LAT {}\nLON {}", minmea_tocoord(&latitude), minmea_tocoord(&longitude)); + lv_label_set_text(statusLabelWidget, label_text.c_str()); + } else { + lv_label_set_text(statusLabelWidget, "Acquiring lock..."); + } + lv_obj_remove_flag(statusLabelWidget, LV_OBJ_FLAG_HIDDEN); + } else { + if (hasSetInfo) { + lv_obj_clean(infoContainerWidget); + hasSetInfo = false; + } + + lv_obj_add_flag(statusLabelWidget, LV_OBJ_FLAG_HIDDEN); + } + } + } + + /** @return true if the views were updated */ + bool updateTimerState() { + bool is_on = service::gps::getState() == service::gps::State::On; + if (is_on && !timer->isRunning()) { + startReceivingUpdates(); + return true; + } else if (!is_on && timer->isRunning()) { + stopReceivingUpdates(); + return true; + } else { + return false; + } + } + + void onGpsToggled(TT_UNUSED lv_event_t* event) { + bool wants_on = lv_obj_has_state(switchWidget, LV_STATE_CHECKED); + auto state = service::gps::getState(); + bool is_on = (state == service::gps::State::On) || (state == service::gps::State::OnPending); if (wants_on != is_on) { + // start/stop are potentially blocking calls, so we use a dispatcher to not block the UI if (wants_on) { - if (!service::gps::startReceiving()) { - TT_LOG_E(TAG, "Failed to toggle GPS on"); - lv_obj_remove_state(widget, LV_STATE_CHECKED); - } + getMainDispatcher().dispatch([](TT_UNUSED auto _) { + service::gps::startReceiving(); + }, nullptr); } else { - service::gps::stopReceiving(); + getMainDispatcher().dispatch([](TT_UNUSED auto _) { + service::gps::stopReceiving(); + }, nullptr); } } } public: + GpsSettingsApp() { + timer = std::make_unique(Timer::Type::Periodic, onUpdateCallback, appReference); + } + void onShow(AppContext& app, lv_obj_t* parent) final { lv_obj_set_flex_flow(parent, LV_FLEX_FLOW_COLUMN); - lvgl::toolbar_create(parent, app); + auto* toolbar = lvgl::toolbar_create(parent, app); + + spinnerWidget = lvgl::toolbar_add_spinner_action(toolbar); + lv_obj_add_flag(spinnerWidget, LV_OBJ_FLAG_HIDDEN); + + switchWidget = lvgl::toolbar_add_switch_action(toolbar); + lv_obj_add_event_cb(switchWidget, onGpsToggledCallback, LV_EVENT_VALUE_CHANGED, this); auto* main_wrapper = lv_obj_create(parent); lv_obj_set_flex_flow(main_wrapper, LV_FLEX_FLOW_COLUMN); lv_obj_set_width(main_wrapper, LV_PCT(100)); lv_obj_set_flex_grow(main_wrapper, 1); + lv_obj_set_style_border_width(main_wrapper, 0, 0); + lv_obj_set_style_pad_all(main_wrapper, 0, 0); auto* top_wrapper = lv_obj_create(main_wrapper); lv_obj_set_width(top_wrapper, LV_PCT(100)); @@ -51,23 +196,33 @@ public: lv_obj_set_style_pad_all(top_wrapper, 0, 0); lv_obj_set_style_border_width(top_wrapper, 0, 0); - auto* toggle_label = lv_label_create(top_wrapper); - lv_label_set_text(toggle_label, "GPS receiver"); + statusLabelWidget = lv_label_create(top_wrapper); + lv_obj_align(statusLabelWidget, LV_ALIGN_TOP_LEFT, 0, 0); - auto* toggle_switch = lv_switch_create(top_wrapper); - lv_obj_align(toggle_switch, LV_ALIGN_TOP_RIGHT, 0, 0); + infoContainerWidget = lv_obj_create(top_wrapper); + lv_obj_align_to(infoContainerWidget, statusLabelWidget, LV_ALIGN_OUT_BOTTOM_LEFT, 0, 20); + lv_obj_set_size(infoContainerWidget, LV_PCT(100), LV_SIZE_CONTENT); + lv_obj_set_flex_flow(infoContainerWidget, LV_FLEX_FLOW_COLUMN); + lv_obj_set_style_border_width(infoContainerWidget, 0, 0); + lv_obj_set_style_pad_all(infoContainerWidget, 0, 0); + hasSetInfo = false; - if (service::gps::isReceiving()) { - lv_obj_add_state(toggle_switch, LV_STATE_CHECKED); - } + updateTimerState(); + updateViews(); - lv_obj_add_event_cb(toggle_switch, onGpsToggled, LV_EVENT_VALUE_CHANGED, nullptr); + serviceStateSubscription = service::gps::getStatePubsub()->subscribe(onServiceStateChangedCallback, this); + } + + void onHide(AppContext& app) final { + service::gps::getStatePubsub()->unsubscribe(serviceStateSubscription); + serviceStateSubscription = nullptr; } }; extern const AppManifest manifest = { .id = "GpsSettings", .name = "GPS", + .icon = LV_SYMBOL_GPS, .type = Type::Settings, .createApp = create }; diff --git a/Tactility/Source/service/statusbar/Statusbar.cpp b/Tactility/Source/service/statusbar/Statusbar.cpp index 8161371a..d0170144 100644 --- a/Tactility/Source/service/statusbar/Statusbar.cpp +++ b/Tactility/Source/service/statusbar/Statusbar.cpp @@ -3,6 +3,7 @@ #include "Tactility/hal/power/PowerDevice.h" #include "Tactility/hal/sdcard/SdCardDevice.h" +#include "Tactility/service/gps/Gps.h" #include #include #include @@ -39,6 +40,9 @@ namespace tt::service::statusbar { #define STATUSBAR_ICON_POWER_90 "power_90.png" #define STATUSBAR_ICON_POWER_100 "power_100.png" +// GPS +#define STATUSBAR_ICON_GPS "location.png" + extern const ServiceManifest manifest; const char* getWifiStatusIconForRssi(int rssi) { @@ -130,6 +134,8 @@ private: Mutex mutex; std::unique_ptr updateTimer; + int8_t gps_icon_id = lvgl::statusbar_icon_add(); + bool gps_last_state = false; int8_t wifi_icon_id = lvgl::statusbar_icon_add(); const char* wifi_last_icon = nullptr; int8_t sdcard_icon_id = lvgl::statusbar_icon_add(); @@ -147,6 +153,21 @@ private: mutex.unlock(); } + void updateGpsIcon() { + auto gps_state = gps::getState(); + bool show_icon = (gps_state == gps::State::OnPending) || (gps_state == gps::State::On); + if (gps_last_state != show_icon) { + if (show_icon) { + auto icon_path = paths->getSystemPathLvgl(STATUSBAR_ICON_GPS); + lvgl::statusbar_icon_set_image(gps_icon_id, icon_path); + lvgl::statusbar_icon_set_visibility(gps_icon_id, true); + } else { + lvgl::statusbar_icon_set_visibility(gps_icon_id, false); + } + gps_last_state = show_icon; + } + } + void updateWifiIcon() { wifi::RadioState radio_state = wifi::getRadioState(); bool is_secure = wifi::isConnectionSecure(); @@ -196,6 +217,7 @@ private: void update() { // TODO: Make thread-safe for LVGL + updateGpsIcon(); updateWifiIcon(); updateSdCardIcon(); updatePowerStatusIcon(); diff --git a/TactilityCore/Include/Tactility/StringUtils.h b/TactilityCore/Include/Tactility/StringUtils.h index e8ef04ea..0e03b574 100644 --- a/TactilityCore/Include/Tactility/StringUtils.h +++ b/TactilityCore/Include/Tactility/StringUtils.h @@ -58,6 +58,11 @@ std::basic_string lowercase(const std::basic_string& input) { return std::move(output); } +/** + * @return true when input only has hex characters: [a-z], [A-Z], [0-9] + */ +bool isAsciiHexString(const std::string& input); + /** * @return the first part of a file name right up (and excluding) the first period character. */ diff --git a/TactilityCore/Source/StringUtils.cpp b/TactilityCore/Source/StringUtils.cpp index 558ba342..945bef3e 100644 --- a/TactilityCore/Source/StringUtils.cpp +++ b/TactilityCore/Source/StringUtils.cpp @@ -1,6 +1,7 @@ #include "Tactility/StringUtils.h" #include +#include #include #include @@ -80,4 +81,19 @@ std::string removeFileExtension(const std::string& input) { } } +bool isAsciiHexString(const std::string& input) { + // Find invalid characters + return std::ranges::views::filter(input, [](char character){ + if ( + (('0' <= character) && ('9' >= character)) || + (('a' <= character) && ('f' >= character)) || + (('A' <= character) && ('F' >= character)) + ) { + return false; + } else { + return true; + } + }).empty(); +} + } // namespace diff --git a/TactilityHeadless/Include/Tactility/hal/gps/GpsDevice.h b/TactilityHeadless/Include/Tactility/hal/gps/GpsDevice.h index 3acea544..0c4aa8e4 100644 --- a/TactilityHeadless/Include/Tactility/hal/gps/GpsDevice.h +++ b/TactilityHeadless/Include/Tactility/hal/gps/GpsDevice.h @@ -2,7 +2,6 @@ #include "../Device.h" #include "../uart/Uart.h" -#include "GpsDeviceInitDefault.h" #include "Satellites.h" #include @@ -13,51 +12,88 @@ namespace tt::hal::gps { + +enum class GpsResponse { + None, + NotAck, + FrameErrors, + Ok, +}; + +enum class GpsModel { + Unknown = 0, + AG3335, + AG3352, + ATGM336H, + LS20031, + MTK, + MTK_L76B, + MTK_PA1616S, + UBLOX6, + UBLOX7, + UBLOX8, + UBLOX9, + UBLOX10, + UC6580, +}; + +const char* toString(GpsModel model); + class GpsDevice : public Device { public: - typedef int SatelliteSubscriptionId; - typedef int LocationSubscriptionId; + typedef int GgaSubscriptionId; + typedef int RmcSubscriptionId; struct Configuration { std::string name; uart_port_t uartPort; uint32_t baudRate; - std::function initFunction = initGpsDefault; + GpsModel model; + }; + + enum class State { + PendingOn, + On, + Error, + PendingOff, + Off }; private: - struct SatelliteSubscription { - SatelliteSubscriptionId id; - std::shared_ptr> onData; + struct GgaSubscription { + GgaSubscriptionId id; + std::shared_ptr> onData; }; - struct LocationSubscription { - LocationSubscriptionId id; + struct RmcSubscription { + RmcSubscriptionId id; std::shared_ptr> onData; }; const Configuration configuration; - Mutex mutex; + Mutex mutex = Mutex(Mutex::Type::Recursive); std::unique_ptr thread; bool threadInterrupted = false; - std::vector satelliteSubscriptions; - std::vector locationSubscriptions; - SatelliteSubscriptionId lastSatelliteSubscriptionId = 0; - LocationSubscriptionId lastLocationSubscriptionId = 0; + std::vector ggaSubscriptions; + std::vector rmcSubscriptions; + GgaSubscriptionId lastSatelliteSubscriptionId = 0; + RmcSubscriptionId lastRmcSubscriptionId = 0; + GpsModel model = GpsModel::Unknown; + State state = State::Off; static int32_t threadMainStatic(void* parameter); int32_t threadMain(); bool isThreadInterrupted() const; + void setState(State newState); + public: - explicit GpsDevice(Configuration configuration) : configuration(std::move(configuration)) { - assert(this->configuration.initFunction != nullptr); - } + explicit GpsDevice(Configuration configuration) : configuration(std::move(configuration)) {} ~GpsDevice() override = default; @@ -69,31 +105,41 @@ public: bool start(); bool stop(); - bool isStarted() const; - - SatelliteSubscriptionId subscribeSatellites(const std::function& onData) { - satelliteSubscriptions.push_back({ + GgaSubscriptionId subscribeGga(const std::function& onData) { + auto lock = mutex.asScopedLock(); + lock.lock(); + ggaSubscriptions.push_back({ .id = ++lastSatelliteSubscriptionId, - .onData = std::make_shared>(onData) + .onData = std::make_shared>(onData) }); return lastSatelliteSubscriptionId; } - void unsubscribeSatellites(SatelliteSubscriptionId subscriptionId) { - std::erase_if(satelliteSubscriptions, [subscriptionId](auto& subscription) { return subscription.id == subscriptionId; }); + void unsubscribeGga(GgaSubscriptionId subscriptionId) { + auto lock = mutex.asScopedLock(); + lock.lock(); + std::erase_if(ggaSubscriptions, [subscriptionId](auto& subscription) { return subscription.id == subscriptionId; }); } - LocationSubscriptionId subscribeLocations(const std::function& onData) { - locationSubscriptions.push_back({ - .id = ++lastLocationSubscriptionId, + RmcSubscriptionId subscribeRmc(const std::function& onData) { + auto lock = mutex.asScopedLock(); + lock.lock(); + rmcSubscriptions.push_back({ + .id = ++lastRmcSubscriptionId, .onData = std::make_shared>(onData) }); - return lastLocationSubscriptionId; + return lastRmcSubscriptionId; } - void unsubscribeLocations(SatelliteSubscriptionId subscriptionId) { - std::erase_if(locationSubscriptions, [subscriptionId](auto& subscription) { return subscription.id == subscriptionId; }); + void unsubscribeRmc(GgaSubscriptionId subscriptionId) { + auto lock = mutex.asScopedLock(); + lock.lock(); + std::erase_if(rmcSubscriptions, [subscriptionId](auto& subscription) { return subscription.id == subscriptionId; }); } + + GpsModel getModel() const; + + State getState() const; }; } diff --git a/TactilityHeadless/Include/Tactility/hal/gps/GpsDeviceInitDefault.h b/TactilityHeadless/Include/Tactility/hal/gps/GpsDeviceInitDefault.h deleted file mode 100644 index 9aef4b5d..00000000 --- a/TactilityHeadless/Include/Tactility/hal/gps/GpsDeviceInitDefault.h +++ /dev/null @@ -1,9 +0,0 @@ -#pragma once - -#include - -namespace tt::hal::gps { - -bool initGpsDefault(uart_port_t port); - -} diff --git a/TactilityHeadless/Include/Tactility/hal/uart/Uart.h b/TactilityHeadless/Include/Tactility/hal/uart/Uart.h index de301147..e630359b 100644 --- a/TactilityHeadless/Include/Tactility/hal/uart/Uart.h +++ b/TactilityHeadless/Include/Tactility/hal/uart/Uart.h @@ -111,7 +111,10 @@ void flushInput(uart_port_t port); */ std::string readStringUntil(uart_port_t port, char untilChar, TickType_t timeout = defaultTimeout); -/** Read a buffer as a byte array until the specified character (the "untilChar" is included in the result) */ -bool readUntil(uart_port_t port, uint8_t* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout = defaultTimeout); +/** + * Read a buffer as a byte array until the specified character (the "untilChar" is included in the result) + * @return the amount of bytes read from UART + */ +size_t readUntil(uart_port_t port, uint8_t* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout = defaultTimeout, bool addNullTerminator = true); } // namespace tt::hal::uart diff --git a/TactilityHeadless/Include/Tactility/service/gps/Gps.h b/TactilityHeadless/Include/Tactility/service/gps/Gps.h index b8d4ca13..2e230191 100644 --- a/TactilityHeadless/Include/Tactility/service/gps/Gps.h +++ b/TactilityHeadless/Include/Tactility/service/gps/Gps.h @@ -1,6 +1,9 @@ #pragma once #include "Tactility/hal/gps/GpsDevice.h" +#include "GpsState.h" + +#include namespace tt::service::gps { @@ -16,7 +19,13 @@ bool startReceiving(); /** Turn GPS receiving off */ void stopReceiving(); -/** @return true when GPS receiver is on and 1 or more devices are active */ -bool isReceiving(); +bool hasCoordinates(); + +bool getCoordinates(minmea_sentence_rmc& rmc); + +State getState(); + +/** @return GPS service pubsub that broadcasts State* objects */ +std::shared_ptr getStatePubsub(); } diff --git a/TactilityHeadless/Include/Tactility/service/gps/GpsState.h b/TactilityHeadless/Include/Tactility/service/gps/GpsState.h new file mode 100644 index 00000000..dae2195d --- /dev/null +++ b/TactilityHeadless/Include/Tactility/service/gps/GpsState.h @@ -0,0 +1,12 @@ +#pragma once + +namespace tt::service::gps { + +enum class State { + OnPending, + On, + OffPending, + Off +}; + +} diff --git a/TactilityHeadless/Include/Tactility/service/wifi/Wifi.h b/TactilityHeadless/Include/Tactility/service/wifi/Wifi.h index 0e6f6290..a43e7799 100644 --- a/TactilityHeadless/Include/Tactility/service/wifi/Wifi.h +++ b/TactilityHeadless/Include/Tactility/service/wifi/Wifi.h @@ -76,7 +76,7 @@ struct ApRecord { }; /** - * @brief Get wifi pubsub + * @brief Get wifi pubsub that broadcasts Event objects * @return PubSub */ std::shared_ptr getPubsub(); diff --git a/TactilityHeadless/Private/Tactility/hal/gps/Cas.h b/TactilityHeadless/Private/Tactility/hal/gps/Cas.h new file mode 100644 index 00000000..d203ab15 --- /dev/null +++ b/TactilityHeadless/Private/Tactility/hal/gps/Cas.h @@ -0,0 +1,68 @@ +/** + * Source: https://raw.githubusercontent.com/meshtastic/firmware/3b0232de1b6282eacfbff6e50b68fca7e67b8511/src/gps/cas.h + */ +#pragma once + +#include + +// CASIC binary message definitions +// Reference: https://www.icofchina.com/d/file/xiazai/2020-09-22/20f1b42b3a11ac52089caf3603b43fb5.pdf +// ATGM33H-5N: https://www.icofchina.com/pro/mokuai/2016-08-01/4.html +// (https://www.icofchina.com/d/file/xiazai/2016-12-05/b5c57074f4b1fcc62ba8c7868548d18a.pdf) + +// NEMA (Class ID - 0x4e) message IDs +#define CAS_NEMA_GGA 0x00 +#define CAS_NEMA_GLL 0x01 +#define CAS_NEMA_GSA 0x02 +#define CAS_NEMA_GSV 0x03 +#define CAS_NEMA_RMC 0x04 +#define CAS_NEMA_VTG 0x05 +#define CAS_NEMA_GST 0x07 +#define CAS_NEMA_ZDA 0x08 +#define CAS_NEMA_DHV 0x0D + +// Size of a CAS-ACK-(N)ACK message (14 bytes) +#define CAS_ACK_NACK_MSG_SIZE 0x0E + +// CFG-RST (0x06, 0x02) +// Factory reset +constexpr uint8_t _message_CAS_CFG_RST_FACTORY[] = { + 0xFF, 0x03, // Fields to clear + 0x01, // Reset Mode: Controlled Software reset + 0x03 // Startup Mode: Factory +}; + +// CFG_RATE (0x06, 0x01) +// 1HZ update rate, this should always be the case after +// factory reset but update it regardless +constexpr uint8_t _message_CAS_CFG_RATE_1HZ[] = { + 0xE8, 0x03, // Update Rate: 0x03E8 = 1000ms + 0x00, 0x00 // Reserved +}; + +// CFG-NAVX (0x06, 0x07) +// Initial ATGM33H-5N configuration, Updates for Dynamic Mode, Fix Mode, and SV system +// Qwirk: The ATGM33H-5N-31 should only support GPS+BDS, however it will happily enable +// and use GPS+BDS+GLONASS iff the correct CFG_NAVX command is used. +constexpr uint8_t _message_CAS_CFG_NAVX_CONF[] = { + 0x03, 0x01, 0x00, 0x00, // Update Mask: Dynamic Mode, Fix Mode, Nav Settings + 0x03, // Dynamic Mode: Automotive + 0x03, // Fix Mode: Auto 2D/3D + 0x00, // Min SV + 0x00, // Max SVs + 0x00, // Min CNO + 0x00, // Reserved1 + 0x00, // Init 3D fix + 0x00, // Min Elevation + 0x00, // Dr Limit + 0x07, // Nav System: 2^0 = GPS, 2^1 = BDS 2^2 = GLONASS: 2^3 + // 3=GPS+BDS, 7=GPS+BDS+GLONASS + 0x00, 0x00, // Rollover Week + 0x00, 0x00, 0x00, 0x00, // Fix Altitude + 0x00, 0x00, 0x00, 0x00, // Fix Height Error + 0x00, 0x00, 0x00, 0x00, // PDOP Maximum + 0x00, 0x00, 0x00, 0x00, // TDOP Maximum + 0x00, 0x00, 0x00, 0x00, // Position Accuracy Max + 0x00, 0x00, 0x00, 0x00, // Time Accuracy Max + 0x00, 0x00, 0x00, 0x00 // Static Hold Threshold +}; diff --git a/TactilityHeadless/Private/Tactility/hal/gps/GpsInit.h b/TactilityHeadless/Private/Tactility/hal/gps/GpsInit.h index 8f57845c..56c0d192 100644 --- a/TactilityHeadless/Private/Tactility/hal/gps/GpsInit.h +++ b/TactilityHeadless/Private/Tactility/hal/gps/GpsInit.h @@ -11,4 +11,9 @@ namespace tt::hal::gps { */ bool init(const std::vector& configurations); +/** + * Init sequence on UART for a specific GPS model. + */ +bool init(uart_port_t port, GpsModel type); + } diff --git a/TactilityHeadless/Private/Tactility/hal/gps/Probe.h b/TactilityHeadless/Private/Tactility/hal/gps/Probe.h new file mode 100644 index 00000000..b8dcdcf9 --- /dev/null +++ b/TactilityHeadless/Private/Tactility/hal/gps/Probe.h @@ -0,0 +1,12 @@ +#pragma once + +#include "Tactility/hal/gps/GpsDevice.h" +#include "Tactility/hal/uart/Uart.h" + +namespace tt::hal::gps { + +struct GpsInfo; + +GpsModel probe(uart_port_t port); + +} diff --git a/TactilityHeadless/Private/Tactility/hal/gps/Ublox.h b/TactilityHeadless/Private/Tactility/hal/gps/Ublox.h new file mode 100644 index 00000000..15015576 --- /dev/null +++ b/TactilityHeadless/Private/Tactility/hal/gps/Ublox.h @@ -0,0 +1,27 @@ +#pragma once + +#include "Tactility/hal/gps/GpsDevice.h" +#include "Tactility/hal/uart/Uart.h" + +#include +#include + +namespace tt::hal::gps::ublox { + +void checksum(uint8_t* message, size_t length); + +// From https://github.com/meshtastic/firmware/blob/7648391f91f2b84e367ae2b38220b30936fb45b1/src/gps/GPS.cpp#L128 +uint8_t makePacket(uint8_t classId, uint8_t messageId, const uint8_t* payload, uint8_t payloadSize, uint8_t* bufferOut); + +template +inline void sendPacket(uart_port_t port, uint8_t type, uint8_t id, uint8_t data[DataSize], const char* errorMessage, TickType_t timeout) { + static uint8_t buffer[250] = {0}; + size_t length = makePacket(type, id, data, DataSize, buffer); + hal::uart::writeBytes(port, buffer, length); +} + +GpsModel probe(uart_port_t port); + +bool init(uart_port_t port, GpsModel model); + +} // namespace tt::service::gps diff --git a/TactilityHeadless/Private/Tactility/hal/gps/UbloxMessages.h b/TactilityHeadless/Private/Tactility/hal/gps/UbloxMessages.h new file mode 100644 index 00000000..97846684 --- /dev/null +++ b/TactilityHeadless/Private/Tactility/hal/gps/UbloxMessages.h @@ -0,0 +1,467 @@ +#pragma once + +#include + +namespace tt::hal::gps::ublox { + +// Power Management + +constexpr uint8_t _message_PMREQ[] = { + 0x00, 0x00, 0x00, 0x00, // 4 bytes duration of request task (milliseconds) + 0x02, 0x00, 0x00, 0x00 // Bitfield, set backup = 1 +}; + +constexpr uint8_t _message_PMREQ_10[] = { + 0x00, // version (0 for this version) + 0x00, 0x00, 0x00, // Reserved 1 + 0x00, 0x00, 0x00, 0x00, // 4 bytes duration of request task (milliseconds) + 0x06, 0x00, 0x00, 0x00, // Bitfield, set backup =1 and force =1 + 0x08, 0x00, 0x00, 0x00 // wakeupSources Wake on uartrx +}; + +constexpr uint8_t _message_CFG_RXM_PSM[] = { + 0x08, // Reserved + 0x01 // Power save mode +}; + +// only for Neo-6 +constexpr uint8_t _message_CFG_RXM_ECO[] = { + 0x08, // Reserved + 0x04 // eco mode +}; + +constexpr uint8_t _message_CFG_PM2[] = { + 0x01, // version + 0x00, // Reserved 1, set to 0x06 by u-Center + 0x00, // Reserved 2 + 0x00, // Reserved 1 + 0x00, 0x11, 0x03, 0x00, // flags-> cyclic mode, wait for normal fix ok, do not wake to update RTC, doNotEnterOff, + // LimitPeakCurrent + 0xE8, 0x03, 0x00, 0x00, // update period 1000 ms + 0x10, 0x27, 0x00, 0x00, // search period 10s + 0x00, 0x00, 0x00, 0x00, // Grid offset 0 + 0x01, 0x00, // onTime 1 second + 0x00, 0x00, // min search time 0 + 0x00, 0x00, // 0x2C, 0x01, // reserved 4 + 0x00, 0x00, // 0x00, 0x00, // reserved 5 + 0x00, 0x00, 0x00, 0x00, // 0x4F, 0xC1, 0x03, 0x00, // reserved 6 + 0x00, 0x00, 0x00, 0x00, // 0x87, 0x02, 0x00, 0x00, // reserved 7 + 0x00, // 0xFF, // reserved 8 + 0x00, // 0x00, // reserved 9 + 0x00, 0x00, // 0x00, 0x00, // reserved 10 + 0x00, 0x00, 0x00, 0x00 // 0x64, 0x40, 0x01, 0x00 // reserved 11 +}; + +// Constallation setup, none required for Neo-6 + +// For Neo-7 GPS & SBAS +constexpr uint8_t _message_GNSS_7[] = { + 0x00, // msgVer (0 for this version) + 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) + 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) + 0x02, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems + // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags + 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x00, 0x01, // GPS + 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x00, 0x01 // SBAS +}; + +// It's not critical if the module doesn't acknowledge this configuration. +// The module should operate adequately with its factory or previously saved settings. +// It appears that there is a firmware bug in some GPS modules: When an attempt is made +// to overwrite a saved state with identical values, no ACK/NAK is received, contrary to +// what is specified in the Ublox documentation. +// There is also a possibility that the module may be GPS-only. + +// For M8 GPS, GLONASS, Galileo, SBAS, QZSS +constexpr uint8_t _message_GNSS_8[] = { + 0x00, // msgVer (0 for this version) + 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) + 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) + 0x05, // numConfigBlocks (number of GNSS systems) + // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags + 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS + 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS + 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, // Galileo + 0x05, 0x00, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // QZSS + 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS +}; +/* +// For M8 GPS, GLONASS, BeiDou, SBAS, QZSS +constexpr uint8_t _message_GNSS_8_B[] = { + 0x00, // msgVer (0 for this version) + 0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0) + 0xff, // numTrkChUse (max number of channels to use, 0xff = max available) read only for protocol >23 + 0x05, // numConfigBlocks (number of GNSS systems) + // GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags + 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS + 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS + 0x03, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // BeiDou + 0x05, 0x00, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // QZSS + 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 // GLONASS +}; +*/ + +// For M8 we want to enable NMEA version 4.10 messages to allow for Galileo and or BeiDou +constexpr uint8_t _message_NMEA[] { + 0x00, // filter flags + 0x41, // NMEA Version + 0x00, // Max number of SVs to report per TaklerId + 0x02, // flags + 0x00, 0x00, 0x00, 0x00, // gnssToFilter + 0x00, // svNumbering + 0x00, // mainTalkerId + 0x00, // gsvTalkerId + 0x01, // Message version + 0x00, 0x00, // bdsTalkerId 2 chars 0=default + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 // Reserved +}; +// Enable jamming/interference monitor + +// For Neo-6, Max-7 and Neo-7 +constexpr uint8_t _message_JAM_6_7[] = { + 0xf3, 0xac, 0x62, 0xad, // config1 bbThreshold = 3, cwThreshold = 15, enable = 1, reserved bits 0x16B156 + 0x1e, 0x03, 0x00, 0x00 // config2 antennaSetting Unknown = 0, reserved 3, = 0x00,0x00, reserved 2 = 0x31E +}; + +// For M8 +constexpr uint8_t _message_JAM_8[] = { + 0xf3, 0xac, 0x62, 0xad, // config1 bbThreshold = 3, cwThreshold = 15, enable1 = 1, reserved bits 0x16B156 + 0x1e, 0x43, 0x00, 0x00 // config2 antennaSetting Unknown = 0, enable2 = 1, generalBits = 0x31E +}; + +// Configure navigation engine expert settings: +// there are many variations of what were Reserved fields for the Neo-6 in later versions +// ToDo: check UBX-MON-VER for module type and protocol version + +// For the Neo-6 +constexpr uint8_t _message_NAVX5[] = { + 0x00, 0x00, // msgVer (0 for this version) + 0x4c, 0x66, // mask1 + 0x00, 0x00, 0x00, 0x00, // Reserved 0 + 0x00, // Reserved 1 + 0x00, // Reserved 2 + 0x03, // minSVs (Minimum number of satellites for navigation) = 3 + 0x10, // maxSVs (Maximum number of satellites for navigation) = 16 + 0x06, // minCNO (Minimum satellite signal level for navigation) = 6 dBHz + 0x00, // Reserved 5 + 0x00, // iniFix3D (Initial fix must be 3D) (0 = false 1 = true) + 0x00, // Reserved 6 + 0x00, // Reserved 7 + 0x00, // Reserved 8 + 0x00, 0x00, // wknRollover 0 = firmware default + 0x00, 0x00, 0x00, 0x00, // Reserved 9 + 0x00, // Reserved 10 + 0x00, // Reserved 11 + 0x00, // usePPP (Precice Point Positioning) (0 = false, 1 = true) + 0x01, // useAOP (AssistNow Autonomous configuration) = 1 (enabled) + 0x00, // Reserved 12 + 0x00, // Reserved 13 + 0x00, 0x00, // aopOrbMaxErr = 0 to reset to firmware default + 0x00, // Reserved 14 + 0x00, // Reserved 15 + 0x00, 0x00, // Reserved 3 + 0x00, 0x00, 0x00, 0x00 // Reserved 4 +}; +// For the M8 +constexpr uint8_t _message_NAVX5_8[] = { + 0x02, 0x00, // msgVer (2 for this version) + 0x4c, 0x66, // mask1 + 0x00, 0x00, 0x00, 0x00, // mask2 + 0x00, 0x00, // Reserved 1 + 0x03, // minSVs (Minimum number of satellites for navigation) = 3 + 0x10, // maxSVs (Maximum number of satellites for navigation) = 16 + 0x06, // minCNO (Minimum satellite signal level for navigation) = 6 dBHz + 0x00, // Reserved 2 + 0x00, // iniFix3D (Initial fix must be 3D) (0 = false 1 = true) + 0x00, 0x00, // Reserved 3 + 0x00, // ackAiding + 0x00, 0x00, // wknRollover 0 = firmware default + 0x00, // sigAttenCompMode + 0x00, // Reserved 4 + 0x00, 0x00, // Reserved 5 + 0x00, 0x00, // Reserved 6 + 0x00, // usePPP (Precice Point Positioning) (0 = false, 1 = true) + 0x01, // aopCfg (AssistNow Autonomous configuration) = 1 (enabled) + 0x00, 0x00, // Reserved 7 + 0x00, 0x00, // aopOrbMaxErr = 0 to reset to firmware default + 0x00, 0x00, 0x00, 0x00, // Reserved 8 + 0x00, 0x00, 0x00, // Reserved 9 + 0x00 // useAdr +}; + +// Set GPS update rate to 1Hz +// Lowering the update rate helps to save power. +// Additionally, for some new modules like the M9/M10, an update rate lower than 5Hz +// is recommended to avoid a known issue with satellites disappearing. +// The module defaults for M8, M9, M10 are the same as we use here so no update is necessary +constexpr uint8_t _message_1HZ[] = { + 0xE8, 0x03, // Measurement Rate (1000ms for 1Hz) + 0x01, 0x00, // Navigation rate, always 1 in GPS mode + 0x01, 0x00 // Time reference +}; + +// Disable GLL. GLL - Geographic position (latitude and longitude), which provides the current geographical +// coordinates. +constexpr uint8_t _message_GLL[] = { + 0xF0, 0x01, // NMEA ID for GLL + 0x00, // Rate for DDC + 0x00, // Rate for UART1 + 0x00, // Rate for UART2 + 0x00, // Rate for USB + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Disable GSA. GSA - GPS DOP and active satellites, used for detailing the satellites used in the positioning and +// the DOP (Dilution of Precision) +constexpr uint8_t _message_GSA[] = { + 0xF0, 0x02, // NMEA ID for GSA + 0x00, // Rate for DDC + 0x00, // Rate for UART1 + 0x00, // Rate for UART2 + 0x00, // Rate for USB usefull for native linux + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Disable GSV. GSV - Satellites in view, details the number and location of satellites in view. +constexpr uint8_t _message_GSV[] = { + 0xF0, 0x03, // NMEA ID for GSV + 0x00, // Rate for DDC + 0x00, // Rate for UART1 + 0x00, // Rate for UART2 + 0x00, // Rate for USB + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Disable VTG. VTG - Track made good and ground speed, which provides course and speed information relative to +// the ground. +constexpr uint8_t _message_VTG[] = { + 0xF0, 0x05, // NMEA ID for VTG + 0x00, // Rate for DDC + 0x00, // Rate for UART1 + 0x00, // Rate for UART2 + 0x00, // Rate for USB + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Enable RMC. RMC - Recommended Minimum data, the essential gps pvt (position, velocity, time) data. +constexpr uint8_t _message_RMC[] = { + 0xF0, 0x04, // NMEA ID for RMC + 0x00, // Rate for DDC + 0x01, // Rate for UART1 + 0x00, // Rate for UART2 + 0x01, // Rate for USB usefull for native linux + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Enable GGA. GGA - Global Positioning System Fix Data, which provides 3D location and accuracy data. +constexpr uint8_t _message_GGA[] = { + 0xF0, 0x00, // NMEA ID for GGA + 0x00, // Rate for DDC + 0x01, // Rate for UART1 + 0x00, // Rate for UART2 + 0x01, // Rate for USB, usefull for native linux + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Disable UBX-AID-ALPSRV as it may confuse TinyGPS. The Neo-6 seems to send this message +// whether the AID Autonomous is enabled or not +constexpr uint8_t _message_AID[] = { + 0x0B, 0x32, // NMEA ID for UBX-AID-ALPSRV + 0x00, // Rate for DDC + 0x00, // Rate for UART1 + 0x00, // Rate for UART2 + 0x00, // Rate for USB + 0x00, // Rate for SPI + 0x00 // Reserved +}; + +// Turn off TEXT INFO Messages for all but M10 series + +// B5 62 06 02 0A 00 01 00 00 00 03 03 00 03 03 00 1F 20 +constexpr uint8_t _message_DISABLE_TXT_INFO[] = { + 0x01, // Protocol ID for NMEA + 0x00, 0x00, 0x00, // Reserved + 0x03, // I2C + 0x03, // I/O Port 1 + 0x00, // I/O Port 2 + 0x03, // USB + 0x03, // SPI + 0x00 // Reserved +}; + +// The Power Management configuration allows the GPS module to operate in different power modes for optimized +// power consumption. The modes supported are: 0x00 = Full power: The module operates at full power with no power +// saving. 0x01 = Balanced: The module dynamically adjusts the tracking behavior to balance power consumption. +// 0x02 = Interval: The module operates in a periodic mode, cycling between tracking and power saving states. +// 0x03 = Aggressive with 1 Hz: The module operates in a power saving mode with a 1 Hz update rate. +// 0x04 = Aggressive with 2 Hz: The module operates in a power saving mode with a 2 Hz update rate. +// 0x05 = Aggressive with 4 Hz: The module operates in a power saving mode with a 4 Hz update rate. +// The 'period' field specifies the position update and search period. It is only valid when the powerSetupValue +// is set to Interval; otherwise, it must be set to '0'. The 'onTime' field specifies the duration of the ON phase +// and must be smaller than the period. It is only valid when the powerSetupValue is set to Interval; otherwise, +// it must be set to '0'. +// This command applies to M8 products +constexpr uint8_t _message_PMS[] = { + 0x00, // Version (0) + 0x03, // Power setup value 3 = Agresssive 1Hz + 0x00, 0x00, // period: not applicable, set to 0 + 0x00, 0x00, // onTime: not applicable, set to 0 + 0x00, 0x00 // reserved, generated by u-center +}; + +constexpr uint8_t _message_SAVE[] = { + 0x00, 0x00, 0x00, 0x00, // clearMask: no sections cleared + 0xFF, 0xFF, 0x00, 0x00, // saveMask: save all sections + 0x00, 0x00, 0x00, 0x00, // loadMask: no sections loaded + 0x17 // deviceMask: BBR, Flash, EEPROM, and SPI Flash +}; + +constexpr uint8_t _message_SAVE_10[] = { + 0x00, 0x00, 0x00, 0x00, // clearMask: no sections cleared + 0xFF, 0xFF, 0x00, 0x00, // saveMask: save all sections + 0x00, 0x00, 0x00, 0x00, // loadMask: no sections loaded + 0x01 // deviceMask: only save to BBR +}; + +// 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. +// for all configurations using sleep / low power modes, V_BCKP needs to be hooked to permanent power for fast aquisition after +// sleep + +// VALSET Commands for M10 +// Please refer to the M10 Protocol Specification: +// https://content.u-blox.com/sites/default/files/u-blox-M10-SPG-5.10_InterfaceDescription_UBX-21035062.pdf +// Where the VALSET/VALGET/VALDEL commands are described in detail. +// and: +// https://content.u-blox.com/sites/default/files/u-blox-M10-ROM-5.10_ReleaseNotes_UBX-22001426.pdf +// for interesting insights. +// +// Integration manual: +// https://content.u-blox.com/sites/default/files/documents/SAM-M10Q_IntegrationManual_UBX-22020019.pdf +// has details on low-power modes + +/* +OPERATEMODE E1 2 (0 | 1 | 2) +POSUPDATEPERIOD U4 5 +ACQPERIOD U4 10 +GRIDOFFSET U4 0 +ONTIME U2 1 +MINACQTIME U1 0 +MAXACQTIME U1 0 +DONOTENTEROFF L 1 +WAITTIMEFIX L 1 +UPDATEEPH L 1 +EXTINTWAKE L 0 no ext ints +EXTINTBACKUP L 0 no ext ints +EXTINTINACTIVE L 0 no ext ints +EXTINTACTIVITY U4 0 no ext ints +LIMITPEAKCURRENT L 1 + +// Ram layer config message: +// b5 62 06 8a 26 00 00 01 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0 +// 10 01 8b de + +// BBR layer config message: +// b5 62 06 8a 26 00 00 02 00 00 01 00 d0 20 02 02 00 d0 40 05 00 00 00 05 00 d0 30 01 00 08 00 d0 10 01 09 00 d0 10 01 10 00 d0 +// 10 01 8c 03 +*/ +constexpr uint8_t _message_VALSET_PM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40, 0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0, 0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01}; +constexpr uint8_t _message_VALSET_PM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x01, 0x00, 0xd0, 0x20, 0x02, 0x02, 0x00, 0xd0, 0x40, 0x05, 0x00, 0x00, 0x00, 0x05, 0x00, 0xd0, 0x30, 0x01, 0x00, 0x08, 0x00, 0xd0, 0x10, 0x01, 0x09, 0x00, 0xd0, 0x10, 0x01, 0x10, 0x00, 0xd0, 0x10, 0x01}; + +/* +CFG-ITFM replaced by 5 valset messages which can be combined into one for RAM and one for BBR + +20410001 bbthreshold U1 3 +20410002 cwthreshold U1 15 +1041000d enable L 0 -> 1 +20410010 ant E1 0 +10410013 enable aux L 0 -> 1 + + +b5 62 06 8a 0e 00 00 01 00 00 0d 00 41 10 01 13 00 41 10 01 63 c6 +*/ +constexpr uint8_t _message_VALSET_ITFM_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x0d, 0x00, 0x41, 0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01}; +constexpr uint8_t _message_VALSET_ITFM_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x0d, 0x00, 0x41, 0x10, 0x01, 0x13, 0x00, 0x41, 0x10, 0x01}; + +// Turn off all NMEA messages: +// Ram layer config message: +// b5 62 06 8a 22 00 00 01 00 00 c0 00 91 20 00 ca 00 91 20 00 c5 00 91 20 00 ac 00 91 20 00 b1 00 91 20 00 bb 00 91 20 00 40 8f + +// Disable GLL, GSV, VTG messages in BBR layer +// BBR layer config message: +// b5 62 06 8a 13 00 00 02 00 00 ca 00 91 20 00 c5 00 91 20 00 b1 00 91 20 00 f8 4e + +constexpr uint8_t _message_VALSET_DISABLE_NMEA_RAM[] = { + /*0x00, 0x01, 0x00, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00 */ + 0x00, 0x01, 0x00, 0x00, 0xc0, 0x00, 0x91, 0x20, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, + 0x20, 0x00, 0xac, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x00 +}; + +constexpr uint8_t _message_VALSET_DISABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x00, 0xca, 0x00, 0x91, 0x20, 0x00, 0xc5, 0x00, 0x91, 0x20, 0x00, 0xb1, 0x00, 0x91, 0x20, 0x00}; + +// Turn off text info messages: +// Ram layer config message: +// b5 62 06 8a 09 00 00 01 00 00 07 00 92 20 06 59 50 + +// BBR layer config message: +// b5 62 06 8a 09 00 00 02 00 00 07 00 92 20 06 5a 58 + +// Turn NMEA GGA, RMC messages on: +// Layer config messages: +// RAM: +// b5 62 06 8a 0e 00 00 01 00 00 bb 00 91 20 01 ac 00 91 20 01 6a 8f +// BBR: +// b5 62 06 8a 0e 00 00 02 00 00 bb 00 91 20 01 ac 00 91 20 01 6b 9c +// FLASH: +// b5 62 06 8a 0e 00 00 04 00 00 bb 00 91 20 01 ac 00 91 20 01 6d b6 +// Doing this for the FLASH layer isn't really required since we save the config to flash later + +constexpr uint8_t _message_VALSET_DISABLE_TXT_INFO_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03}; +constexpr uint8_t _message_VALSET_DISABLE_TXT_INFO_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x07, 0x00, 0x92, 0x20, 0x03}; + +constexpr uint8_t _message_VALSET_ENABLE_NMEA_RAM[] = {0x00, 0x01, 0x00, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01}; +constexpr uint8_t _message_VALSET_ENABLE_NMEA_BBR[] = {0x00, 0x02, 0x00, 0x00, 0xbb, 0x00, 0x91, 0x20, 0x01, 0xac, 0x00, 0x91, 0x20, 0x01}; +constexpr uint8_t _message_VALSET_DISABLE_SBAS_RAM[] = {0x00, 0x01, 0x00, 0x00, 0x20, 0x00, 0x31, 0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00}; +constexpr uint8_t _message_VALSET_DISABLE_SBAS_BBR[] = {0x00, 0x02, 0x00, 0x00, 0x20, 0x00, 0x31, 0x10, 0x00, 0x05, 0x00, 0x31, 0x10, 0x00}; + +/* +Operational issues with the M10: + +PowerSave doesn't work with SBAS, seems like you can have SBAS enabled, but it will never lock +onto the SBAS sats. +PowerSave doesn't work with BDS B1C, u-blox says use B1l instead. +BDS B1l cannot be enabled with BDS B1C or GLONASS L1OF, so GLONASS will work with B1C, but not B1l +So no powersave with GLONASS and BDS B1l enabled. +So disable GLONASS and use BDS B1l, which is part of the default M10 config. + +GNSS configuration: + +Default GNSS configuration is: GPS, Galileo, BDS B1l, with QZSS and SBAS enabled. +The PMREQ puts the receiver to sleep and wakeup re-acquires really fast and seems to not need +the PM config. Lets try without it. +PMREQ sort of works with SBAS, but the awake time is too short to re-acquire any SBAS sats. +The defination of "Got Fix" doesn't seem to include SBAS. Much more too this... +Even if it was, it can take minutes (up to 12.5), +even under good sat visibility conditions to re-acquire the SBAS data. + +Another effect fo the quick transition to sleep is that no other sats will be acquired so the +sat count will tend to remain at what the initial fix was. +*/ + +// GNSS disable SBAS as recommended by u-blox if using GNSS defaults and power save mode +/* +Ram layer config message: +b5 62 06 8a 0e 00 00 01 00 00 20 00 31 10 00 05 00 31 10 00 46 87 + +BBR layer config message: +b5 62 06 8a 0e 00 00 02 00 00 20 00 31 10 00 05 00 31 10 00 47 94 +*/ + +} \ No newline at end of file diff --git a/TactilityHeadless/Private/Tactility/service/gps/GpsService.h b/TactilityHeadless/Private/Tactility/service/gps/GpsService.h index 31a50dbd..9130aebd 100644 --- a/TactilityHeadless/Private/Tactility/service/gps/GpsService.h +++ b/TactilityHeadless/Private/Tactility/service/gps/GpsService.h @@ -1,8 +1,10 @@ #pragma once #include "Tactility/Mutex.h" -#include "Tactility/service/Service.h" +#include "Tactility/PubSub.h" #include "Tactility/hal/gps/GpsDevice.h" +#include "Tactility/service/Service.h" +#include "Tactility/service/gps/GpsState.h" namespace tt::service::gps { @@ -12,22 +14,29 @@ private: struct GpsDeviceRecord { std::shared_ptr device = nullptr; - hal::gps::GpsDevice::SatelliteSubscriptionId satelliteSubscriptionId = -1; - hal::gps::GpsDevice::LocationSubscriptionId locationSubscriptionId = -1; + hal::gps::GpsDevice::GgaSubscriptionId satelliteSubscriptionId = -1; + hal::gps::GpsDevice::RmcSubscriptionId rmcSubscriptionId = -1; }; + minmea_sentence_rmc rmcRecord; + TickType_t rmcTime = 0; + Mutex mutex = Mutex(Mutex::Type::Recursive); + Mutex stateMutex; std::vector deviceRecords; - bool receiving = false; + std::shared_ptr statePubSub = std::make_shared(); + State state = State::Off; bool startGpsDevice(GpsDeviceRecord& deviceRecord); static bool stopGpsDevice(GpsDeviceRecord& deviceRecord); GpsDeviceRecord* _Nullable findGpsRecord(const std::shared_ptr& record); - void onSatelliteInfo(hal::Device::Id deviceId, const minmea_sat_info& info); + void onGgaSentence(hal::Device::Id deviceId, const minmea_sentence_gga& gga); void onRmcSentence(hal::Device::Id deviceId, const minmea_sentence_rmc& rmc); + void setState(State newState); + public: void onStart(tt::service::ServiceContext &serviceContext) final; @@ -38,7 +47,13 @@ public: bool startReceiving(); void stopReceiving(); - bool isReceiving(); + State getState() const; + + bool hasCoordinates() const; + bool getCoordinates(minmea_sentence_rmc& rmc) const; + + /** @return GPS service pubsub that broadcasts State* objects */ + std::shared_ptr getStatePubsub() const { return statePubSub; } }; } // tt::hal::gps diff --git a/TactilityHeadless/Source/hal/gps/GpsDevice.cpp b/TactilityHeadless/Source/hal/gps/GpsDevice.cpp index da488e69..382edeee 100644 --- a/TactilityHeadless/Source/hal/gps/GpsDevice.cpp +++ b/TactilityHeadless/Source/hal/gps/GpsDevice.cpp @@ -1,12 +1,48 @@ #include "Tactility/hal/gps/GpsDevice.h" -#include +#include "Tactility/hal/gps/GpsInit.h" +#include "Tactility/hal/gps/Probe.h" #include +#include #define TAG "gps" #define GPS_UART_BUFFER_SIZE 256 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); + } +} + int32_t GpsDevice::threadMainStatic(void* parameter) { auto* gps_device = (GpsDevice*)parameter; return gps_device->threadMain(); @@ -20,49 +56,66 @@ int32_t GpsDevice::threadMain() { return -1; } - if (!configuration.initFunction(configuration.uartPort)) { - TT_LOG_E(TAG, "Failed to init"); + + GpsModel model = configuration.model; + if (model == GpsModel::Unknown) { + model = probe(configuration.uartPort); + if (model == GpsModel::Unknown) { + TT_LOG_E(TAG, "Probe failed"); + setState(State::Error); + return -1; + } + } + + mutex.lock(); + this->model = model; + mutex.unlock(); + + if (!init(configuration.uartPort, 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()) { - if (uart::readUntil(configuration.uartPort, (uint8_t*)buffer, GPS_UART_BUFFER_SIZE, '\n', 100 / portTICK_PERIOD_MS) > 0) { - TT_LOG_D(TAG, "RX: %s", buffer); + size_t bytes_read = uart::readUntil(configuration.uartPort, (uint8_t*)buffer, GPS_UART_BUFFER_SIZE, '\n', 100 / portTICK_PERIOD_MS); + if (bytes_read > 0U) { + + // Thread might've been interrupted in the meanwhile + if (isThreadInterrupted()) { + break; + } + + TT_LOG_D(TAG, "%s", buffer); + switch (minmea_sentence_id((char*)buffer, false)) { case MINMEA_SENTENCE_RMC: - minmea_sentence_rmc frame; - if (minmea_parse_rmc(&frame, (char*)buffer)) { - for (auto& subscription : locationSubscriptions) { - (*subscription.onData)(getId(), frame); + minmea_sentence_rmc rmc_frame; + if (minmea_parse_rmc(&rmc_frame, (char*)buffer)) { + mutex.lock(); + for (auto& subscription : rmcSubscriptions) { + (*subscription.onData)(getId(), rmc_frame); } - TT_LOG_D(TAG, "RX RMC %f lat, %f lon, %f m/s", minmea_tocoord(&frame.latitude), minmea_tocoord(&frame.longitude), minmea_tofloat(&frame.speed)); + 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, "RX RMC parse error: %s", buffer); + 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)) { - TT_LOG_D(TAG, "RX GGA %f lat, %f lon", minmea_tocoord(&gga_frame.latitude), minmea_tocoord(&gga_frame.longitude)); - } else { - TT_LOG_W(TAG, "RX GGA parse error: %s", buffer); - } - break; - case MINMEA_SENTENCE_GSV: - minmea_sentence_gsv gsv_frame; - if (minmea_parse_gsv(&gsv_frame, (char*)buffer)) { - for (auto& sat : gsv_frame.sats) { - if (sat.nr != 0 && sat.elevation != 0 && sat.snr != 0) { - for (auto& subscription : satelliteSubscriptions) { - (*subscription.onData)(getId(), sat); - } - } - TT_LOG_D(TAG, "Satellite: id %d, elevation %d, azimuth %d, snr %d", sat.nr, sat.elevation, sat.azimuth, sat.snr); + mutex.lock(); + for (auto& subscription : ggaSubscriptions) { + (*subscription.onData)(getId(), gga_frame); } - TT_LOG_D(TAG, "RX GGA %f lat, %f lon", minmea_tocoord(&gga_frame.latitude), minmea_tocoord(&gga_frame.longitude)); + 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, "RX GGA parse error: %s", buffer); + TT_LOG_W(TAG, "GGA parse error: %s", buffer); } break; default: @@ -95,6 +148,9 @@ bool GpsDevice::start() { threadInterrupted = false; + TT_LOG_I(TAG, "Starting thread"); + setState(State::PendingOn); + thread = std::make_unique( "gps", 4096, @@ -104,12 +160,15 @@ bool GpsDevice::start() { 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(portMAX_DELAY); + lock.lock(); + + setState(State::PendingOff); if (thread != nullptr) { threadInterrupted = true; @@ -130,23 +189,38 @@ bool GpsDevice::stop() { if (uart::isStarted(configuration.uartPort)) { if (!uart::stop(configuration.uartPort)) { TT_LOG_E(TAG, "UART %d failed to stop", configuration.uartPort); + setState(State::Error); return false; } } - return true; -} + setState(State::Off); -bool GpsDevice::isStarted() const { - auto lock = mutex.asScopedLock(); - lock.lock(); - return thread != nullptr && thread->getState() != Thread::State::Stopped; + return true; } bool GpsDevice::isThreadInterrupted() const { auto lock = mutex.asScopedLock(); - lock.lock(portMAX_DELAY); + 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 diff --git a/TactilityHeadless/Source/hal/gps/GpsDeviceInitDefault.cpp b/TactilityHeadless/Source/hal/gps/GpsDeviceInitDefault.cpp deleted file mode 100644 index bc7ffaab..00000000 --- a/TactilityHeadless/Source/hal/gps/GpsDeviceInitDefault.cpp +++ /dev/null @@ -1,214 +0,0 @@ -#include -#include -#include - -#define TAG "gps" -#define GPS_UART_BUFFER_SIZE 256 - -using namespace tt; -using namespace tt::hal; - -namespace tt::hal::gps { - -static int ackGps(uart_port_t port, uint8_t* buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedId) { - uint16_t ubxFrameCounter = 0; - uint32_t startTime = kernel::getTicks(); - uint16_t needRead; - - while (kernel::getTicks() - startTime < 800) { - while (uart::available(port)) { - uint8_t c; - uart::readByte(port, &c); - switch (ubxFrameCounter) { - case 0: - if (c == 0xB5) { - ubxFrameCounter++; - } - break; - case 1: - if (c == 0x62) { - ubxFrameCounter++; - } else { - ubxFrameCounter = 0; - } - break; - case 2: - if (c == requestedClass) { - ubxFrameCounter++; - } else { - ubxFrameCounter = 0; - } - break; - case 3: - if (c == requestedId) { - ubxFrameCounter++; - } else { - ubxFrameCounter = 0; - } - break; - case 4: - needRead = c; - ubxFrameCounter++; - break; - case 5: - needRead |= (c << 8); - ubxFrameCounter++; - break; - case 6: - if (needRead >= size) { - ubxFrameCounter = 0; - break; - } - if (uart::readBytes(port, buffer, needRead) != needRead) { - ubxFrameCounter = 0; - } else { - return needRead; - } - break; - - default: - break; - } - } - } - return 0; -} - -static bool configureGps(uart_port_t port, uint8_t* buffer, size_t bufferSize) { - // According to the reference implementation, L76K GPS uses 9600 baudrate, but the default in the developer device was 38400 - // https://github.com/Xinyuan-LilyGO/T-Deck/blob/master/examples/GPSShield/GPSShield.ino - bool result = false; - uint32_t startTimeout; - for (int i = 0; i < 3; ++i) { - if (!uart::writeString(port, "$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n")) { - TT_LOG_E(TAG, "Failed to write init string"); - } - - kernel::delayMillis(5U); - // Get version information - startTimeout = kernel::getMillis() + 1000; - - TT_LOG_I(TAG, "Manual flushing of input"); -#ifdef ESP_PLATFORM - esp_rom_printf("Waiting..."); -#endif - while (uart::available(port)) { -#ifdef ESP_PLATFORM - esp_rom_printf("."); -#endif - uart::readUntil(port, buffer, bufferSize, '\n'); - if (kernel::getMillis() > startTimeout) { - TT_LOG_E(TAG, "NMEA timeout"); - return false; - } - }; -#ifdef ESP_PLATFORM - esp_rom_printf("\n"); -#endif - uart::flushInput(port); - kernel::delayMillis(200); - - if (!uart::writeString(port, "$PCAS06,0*1B\r\n")) { - TT_LOG_E(TAG, "Failed to write PCAS06"); - } - - startTimeout = kernel::getMillis() + 500; - while (!uart::available(port)) { - if (kernel::getMillis() > startTimeout) { - TT_LOG_E(TAG, "L76K timeout"); - return false; - } - } - auto ver = uart::readStringUntil(port, '\n'); - if (ver.starts_with("$GPTXT,01,01,02")) { - TT_LOG_I(TAG, "L76K GNSS init success"); - result = true; - break; - } - kernel::delayMillis(500); - } - // Initialize the L76K Chip, use GPS + GLONASS - uart::writeString(port, "$PCAS04,5*1C\r\n"); - kernel::delayMillis(250); - uart::writeString(port, "$PCAS03,1,1,1,1,1,1,1,1,1,1,,,0,0*26\r\n"); - kernel::delayMillis(250); - - // Switch to Vehicle Mode, since SoftRF enables Aviation < 2g - uart::writeString(port, "$PCAS11,3*1E\r\n"); - - return result; -} - -static bool recoverGps(uart_port_t port, uint8_t* buffer, size_t bufferSize) { - uint8_t cfg_clear1[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x1C, 0xA2}; - uint8_t cfg_clear2[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x1B, 0xA1}; - uint8_t cfg_clear3[] = {0xB5, 0x62, 0x06, 0x09, 0x0D, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0x00, 0x00, 0x03, 0x1D, 0xB3}; - - if (!uart::writeBytes(port, cfg_clear1, sizeof(cfg_clear1), 10)) { - return false; - TT_LOG_E(TAG, "Failed to send ack 1"); - } - - if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) { - TT_LOG_I(TAG, "Ack 1"); - } else { - TT_LOG_W(TAG, "Ack 1 failed"); - } - - if (!uart::writeBytes(port, cfg_clear2, sizeof(cfg_clear2))) { - return false; - TT_LOG_E(TAG, "Failed to send ack 2"); - } - - if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) { - TT_LOG_I(TAG, "Ack 2"); - } else { - TT_LOG_W(TAG, "Ack 2 failed"); - } - - if (!uart::writeBytes(port, cfg_clear3, sizeof(cfg_clear3))) { - TT_LOG_E(TAG, "Failed to send ack 3"); - return false; - } - - if (ackGps(port, buffer, bufferSize, 0x05, 0x01)) { - TT_LOG_I(TAG, "Ack 3"); - } else { - TT_LOG_W(TAG, "Ack 3 failed"); - } - - // UBX-CFG-RATE, Size 8, 'Navigation/measurement rate settings' - uint8_t cfg_rate[] = {0xB5, 0x62, 0x06, 0x08, 0x00, 0x00, 0x0E, 0x30}; - uart::writeBytes(port, cfg_rate, sizeof(cfg_rate)); - if (ackGps(port, buffer, bufferSize, 0x06, 0x08)) { - TT_LOG_I(TAG, "Ack completed"); - } else { - return false; - } - return true; -} - -bool initGpsDefault(uart_port_t port) { - uint8_t buffer[GPS_UART_BUFFER_SIZE]; - if (!configureGps(port, buffer, GPS_UART_BUFFER_SIZE)) { - if (!recoverGps(port, buffer, GPS_UART_BUFFER_SIZE)) { - uint32_t initial_baud_rate = std::max(uart::getBaudRate(port), (uint32_t)9600U); - uart::setBaudRate(port, 9600U); - if (!recoverGps(port, buffer, GPS_UART_BUFFER_SIZE)) { - TT_LOG_E(TAG, "Recovery repeatedly failed"); - return false; - } else { - TT_LOG_I(TAG, "Recovery 2 complete"); - } - uart::setBaudRate(port, initial_baud_rate); - } else { - TT_LOG_I(TAG, "Recovery 1 complete"); - } - } - - TT_LOG_I(TAG, "Init complete"); - - return true; -} - -} // namespace tt::hal::gps diff --git a/TactilityHeadless/Source/hal/gps/GpsInit.cpp b/TactilityHeadless/Source/hal/gps/GpsInit.cpp index 99807f1f..955e3a74 100644 --- a/TactilityHeadless/Source/hal/gps/GpsInit.cpp +++ b/TactilityHeadless/Source/hal/gps/GpsInit.cpp @@ -1,8 +1,140 @@ +#include "Tactility/hal/gps/Cas.h" #include "Tactility/hal/gps/GpsDevice.h" +#include "Tactility/hal/gps/Ublox.h" #include "Tactility/service/Service.h" +#include + +#define TAG "gps" namespace tt::hal::gps { +bool initMtk(uart_port_t port); +bool initMtkL76b(uart_port_t port); +bool initMtkPa1616s(uart_port_t port); +bool initAtgm336h(uart_port_t port); +bool initUc6580(uart_port_t port); +bool initAg33xx(uart_port_t port); + +// 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_port_t port, 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(port)) { + uart::readByte(port, &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 + + +/** Initialize the HAL with the provided configuration */ bool init(const std::vector& configurations) { for (auto& configuration : configurations) { auto device = std::make_shared(configuration); @@ -12,4 +144,157 @@ bool init(const std::vector& configurations) { return true; } +bool init(uart_port_t port, GpsModel type) { + switch (type) { + case GpsModel::Unknown: + tt_crash(); + case GpsModel::AG3335: + case GpsModel::AG3352: + return initAg33xx(port); + case GpsModel::ATGM336H: + return initAtgm336h(port); + case GpsModel::LS20031: + break; + case GpsModel::MTK: + return initMtk(port); + case GpsModel::MTK_L76B: + return initMtkL76b(port); + case GpsModel::MTK_PA1616S: + return initMtkPa1616s(port); + case GpsModel::UBLOX6: + case GpsModel::UBLOX7: + case GpsModel::UBLOX8: + case GpsModel::UBLOX9: + case GpsModel::UBLOX10: + return ublox::init(port, type); + case GpsModel::UC6580: + return initUc6580(port); + } + + TT_LOG_I(TAG, "Init not implemented %d", static_cast(type)); + return false; +} + +bool initAg33xx(uart_port_t port) { + uart::writeString(port, "$PAIR066,1,0,1,0,0,1*3B\r\n"); // Enable GPS+GALILEO+NAVIC + + // Configure NMEA (sentences will output once per fix) + uart::writeString(port, "$PAIR062,0,1*3F\r\n"); // GGA ON + uart::writeString(port, "$PAIR062,1,0*3F\r\n"); // GLL OFF + uart::writeString(port, "$PAIR062,2,0*3C\r\n"); // GSA OFF + uart::writeString(port, "$PAIR062,3,0*3D\r\n"); // GSV OFF + uart::writeString(port, "$PAIR062,4,1*3B\r\n"); // RMC ON + uart::writeString(port, "$PAIR062,5,0*3B\r\n"); // VTG OFF + uart::writeString(port, "$PAIR062,6,0*38\r\n"); // ZDA ON + + kernel::delayMillis(250); + uart::writeString(port, "$PAIR513*3D\r\n"); // save configuration + return true; +} + +bool initUc6580(uart_port_t port) { + // 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(port, "$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(port, "$CFGMSG,0,3,0\r\n"); + kernel::delayMillis(250); + // Turn off GSA messages, TinyGPS++ doesn't use this message. + uart::writeString(port, "$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(port, "$CFGMSG,6,0,0\r\n"); + kernel::delayMillis(250); + uart::writeString(port, "$CFGMSG,6,1,0\r\n"); + kernel::delayMillis(250); + return true; +} + +bool initAtgm336h(uart_port_t port) { + 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(port, buffer, msglen); + if (getACKCas(port, 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(port, buffer, msglen); + if (getACKCas(port, 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(port, buffer, msglen); + if (getACKCas(port, 0x06, 0x01, 250) != GpsResponse::Ok) { + TT_LOG_W(TAG, "ATGM336H: Could not enable NMEA MSG: %d", fields[i]); + } + } + return true; +} + +bool initMtkPa1616s(uart_port_t port) { + // 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(port, "$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(port, "$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(port, "$PMTK301,2*2E\r\n"); + kernel::delayMillis(250); + return true; +} + +bool initMtkL76b(uart_port_t port) { + // 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(port, "$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(port, "$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(port, "$PMTK301,2*2E\r\n"); + kernel::delayMillis(250); + // Enable PPS for 2D/3D fix only + uart::writeString(port, "$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(port, "$PMTK886,1*29\r\n"); + kernel::delayMillis(250); + return true; +} + +bool initMtk(uart_port_t port) { + // Initialize the L76K Chip, use GPS + GLONASS + BEIDOU + uart::writeString(port, "$PCAS04,7*1E\r\n"); + kernel::delayMillis(250); + // only ask for RMC and GGA + uart::writeString(port, "$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(port, "$PCAS11,3*1E\r\n"); + kernel::delayMillis(250); + return true; +} + } // namespace tt::hal::gps diff --git a/TactilityHeadless/Source/hal/gps/Probe.cpp b/TactilityHeadless/Source/hal/gps/Probe.cpp new file mode 100644 index 00000000..46b94310 --- /dev/null +++ b/TactilityHeadless/Source/hal/gps/Probe.cpp @@ -0,0 +1,140 @@ +#include "Tactility/hal/gps/GpsDevice.h" +#include "Tactility/hal/gps/Ublox.h" +#include +#include +#include +#include + +#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_port_t port, 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(port)) { + uart::readByte(port, &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(PORT, CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...) \ + do { \ + TT_LOG_I(TAG, "Probing for %s (%s)", CHIP, TOWRITE); \ + uart::flushInput(PORT); \ + uart::writeString(PORT, TOWRITE "\r\n", TIMEOUT); \ + if (getAck(PORT, 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_port_t port) { + // Close all NMEA sentences, valid for L76K, ATGM336H (and likely other AT6558 devices) + uart::writeString(port, "$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(port, "$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n"); + uart::writeString(port, "$PUBX,40,GSV,0,0,0,0,0,0*59\r\n"); + uart::writeString(port, "$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(port, "UC6580", "$PDTINFO", "UC6580", GpsModel::UC6580, 500); + PROBE_SIMPLE(port, "UM600", "$PDTINFO", "UM600", GpsModel::UC6580, 500); + PROBE_SIMPLE(port, "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(port, "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(port, "$PAIR062,2,0*3C\r\n"); // GSA OFF to reduce volume + uart::writeString(port, "$PAIR062,3,0*3D\r\n"); // GSV OFF to reduce volume + uart::writeString(port, "$PAIR513*3D\r\n"); // save configuration + PROBE_SIMPLE(port, "AG3335", "$PAIR021*39", "$PAIR021,AG3335", GpsModel::AG3335, 500); + PROBE_SIMPLE(port, "AG3352", "$PAIR021*39", "$PAIR021,AG3352", GpsModel::AG3352, 500); + PROBE_SIMPLE(port, "LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GpsModel::AG3352, 500); + + PROBE_SIMPLE(port, "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(port, "$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(port, "L76B", "$PMTK605*31", "Quectel-L76B", GpsModel::MTK_L76B, 500); + PROBE_SIMPLE(port, "PA1616S", "$PMTK605*31", "1616S", GpsModel::MTK_PA1616S, 500); + + auto ublox_result = ublox::probe(port); + if (ublox_result != GpsModel::Unknown) { + return ublox_result; + } else { + TT_LOG_W(TAG, "No GNSS Module (baudrate %lu)", uart::getBaudRate(port)); + return GpsModel::Unknown; + } +} + +} // namespace tt::hal::gps diff --git a/TactilityHeadless/Source/hal/gps/Ublox.cpp b/TactilityHeadless/Source/hal/gps/Ublox.cpp new file mode 100644 index 00000000..6ffbc775 --- /dev/null +++ b/TactilityHeadless/Source/hal/gps/Ublox.cpp @@ -0,0 +1,479 @@ +#include "Tactility/hal/gps/Ublox.h" +#include "Tactility/hal/gps/UbloxMessages.h" +#include + +#define TAG "ublox" + +namespace tt::hal::gps::ublox { + +bool initUblox6(uart_port_t port); +bool initUblox789(uart_port_t port, GpsModel model); +bool initUblox10(uart_port_t port); + +#define SEND_UBX_PACKET(PORT, BUFFER, TYPE, ID, DATA, ERRMSG, TIMEOUT) \ + do { \ + auto msglen = makePacket(TYPE, ID, DATA, sizeof(DATA), BUFFER); \ + uart::writeBytes(PORT, BUFFER, sizeof(BUFFER)); \ + if (getAck(PORT, 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_port_t port, 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(port)) { + uart::readByte(port, &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_port_t port, 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(port)) { + uint8_t c; + uart::readByte(port, &c); + + switch (ubxFrameCounter) { + case 0: + if (c == 0xB5) { + ubxFrameCounter++; + } + break; + case 1: + if (c == 0x62) { + ubxFrameCounter++; + } else { + ubxFrameCounter = 0; + } + break; + case 2: + if (c == requestedClass) { + ubxFrameCounter++; + } else { + ubxFrameCounter = 0; + } + break; + case 3: + if (c == requestedId) { + ubxFrameCounter++; + } else { + ubxFrameCounter = 0; + } + break; + case 4: + needRead = c; + ubxFrameCounter++; + break; + case 5: { + // Payload length msb + needRead |= (c << 8); + ubxFrameCounter++; + // Check for buffer overflow + if (needRead >= size) { + ubxFrameCounter = 0; + break; + } + auto read_bytes = uart::readBytes(port, 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_port_t port) { + 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(port); + uart::writeBytes(port, cfg_rate, sizeof(cfg_rate)); + // Check that the returned response class and message ID are correct + GpsResponse response = getAck(port, 0x06, 0x08, 750); + if (response == GpsResponse::None) { + TT_LOG_W(TAG, "No GNSS Module (baudrate %lu)", uart::getBaudRate(port)); + return GpsModel::Unknown; + } else if (response == GpsResponse::FrameErrors) { + TT_LOG_W(TAG, "UBlox Frame Errors (baudrate %lu)", uart::getBaudRate(port)); + } + + 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(port); + uart::writeBytes(port, _message_MONVER, sizeof(_message_MONVER)); + + uint16_t ack_response_len = getAck(port, 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_port_t port, GpsModel model) { + TT_LOG_I(TAG, "U-blox init"); + switch (model) { + case GpsModel::UBLOX6: + return initUblox6(port); + case GpsModel::UBLOX7: + case GpsModel::UBLOX8: + case GpsModel::UBLOX9: + return initUblox789(port, model); + case GpsModel::UBLOX10: + return initUblox10(port); + default: + TT_LOG_E(TAG, "Unknown or unsupported U-blox model"); + return false; + } +} + +bool initUblox10(uart_port_t port) { + uint8_t buffer[256]; + kernel::delayMillis(1000); + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_RAM, "disable NMEA messages in M10 RAM", 300); + kernel::delayMillis(750); + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_BBR, "disable NMEA messages in M10 BBR", 300); + kernel::delayMillis(750); + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_RAM, "disable Info messages for M10 GPS RAM", 300); + kernel::delayMillis(750); + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_BBR, "disable Info messages for M10 GPS BBR", 300); + kernel::delayMillis(750); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_PM_RAM, "enable powersave for M10 GPS RAM", 300); + kernel::delayMillis(750); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_PM_BBR, "enable powersave for M10 GPS BBR", 300); + kernel::delayMillis(750); + SEND_UBX_PACKET(port, buffer, 0x06, 0x8A, _message_VALSET_ITFM_RAM, "enable jam detection M10 GPS RAM", 300); + kernel::delayMillis(750); + SEND_UBX_PACKET(port, 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(port, 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(port, 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(port, 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(port, 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(port, buffer, packet_size); + if (getAck(port, 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_port_t port, 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(port, buffer, msglen); + } else { // 8,9 + auto msglen = makePacket(0x06, 0x3e, _message_GNSS_8, sizeof(_message_GNSS_8), buffer); + uart::writeBytes(port, buffer, msglen); + } + + if (getAck(port, 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(port); + + SEND_UBX_PACKET(port, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500); + + if (model == GpsModel::UBLOX8) { // 8 + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x39, _message_JAM_8, "enable interference resistance", 500); + + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x23, _message_NAVX5_8, "configure NAVX5_8 settings", 500); + } else { // 6,7,9 + SEND_UBX_PACKET(port, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500); + } + + // Turn off unwanted NMEA messages, set update rate + SEND_UBX_PACKET(port, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500); + + if (ublox_info.protocol_version >= 18) { + uart::flushInput(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x86, _message_PMS, "enable powersave for GPS", 500); + SEND_UBX_PACKET(port, 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(port); + SEND_UBX_PACKET(port, buffer, 0x06, 0x17, _message_NMEA, "enable NMEA 4.10", 500); + } + } else { + SEND_UBX_PACKET(port, buffer, 0x06, 0x11, _message_CFG_RXM_PSM, "enable powersave mode for GPS", 500); + SEND_UBX_PACKET(port, 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(port, buffer, packet_size); + if (getAck(port, 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_port_t port) { + uint8_t buffer[256]; + + uart::flushInput(port); + + SEND_UBX_PACKET(port, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500); + + // Turn off unwanted NMEA messages, set update rate + SEND_UBX_PACKET(port, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500); + + uart::flushInput(port); + + SEND_UBX_PACKET(port, buffer, 0x06, 0x11, _message_CFG_RXM_ECO, "enable powersave ECO mode for Neo-6", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500); + SEND_UBX_PACKET(port, buffer, 0x06, 0x01, _message_AID, "disable UBX-AID", 500); + + auto packet_size = makePacket(0x06, 0x09, _message_SAVE, sizeof(_message_SAVE), buffer); + uart::writeBytes(port, buffer, packet_size); + if (getAck(port, 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 diff --git a/TactilityHeadless/Source/hal/uart/Uart.cpp b/TactilityHeadless/Source/hal/uart/Uart.cpp index ba04ab33..3b71f0ba 100644 --- a/TactilityHeadless/Source/hal/uart/Uart.cpp +++ b/TactilityHeadless/Source/hal/uart/Uart.cpp @@ -286,39 +286,58 @@ bool setBaudRate(uart_port_t port, uint32_t baudRate, TickType_t timeout) { #endif // ESP_PLATFORM } -bool readUntil(uart_port_t port, uint8_t* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout) { - bool success = false; - size_t index = 0; - size_t index_limit = bufferSize - 1; - while (readByte(port, buffer, timeout) && index < index_limit) { - if (*buffer == untilByte) { - success = true; - // We have the extra space because index < index_limit - buffer++; - *buffer = 0; +// #define DEBUG_READ_UNTIL + +size_t readUntil(uart_port_t port, uint8_t* buffer, size_t bufferSize, uint8_t untilByte, TickType_t timeout, bool addNullTerminator) { + TickType_t start_time = kernel::getTicks(); + uint8_t* buffer_write_ptr = buffer; + uint8_t* buffer_limit = buffer + bufferSize - 1; // Keep 1 extra char as mull terminator + TickType_t timeout_left = timeout; + while (readByte(port, 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; } - buffer++; - } - return success; -} -std::string readStringUntil(uart_port_t port, char untilChar, TickType_t timeout) { - std::stringstream output; - char buffer; - bool success = false; - while (readByte(port, (uint8_t*)&buffer, timeout)) { - if (buffer == untilChar) { - success = true; +#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); } - output << buffer; } - if (success) { - return output.str(); +#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 > buffer)) { + return reinterpret_cast(buffer_write_ptr) - reinterpret_cast(buffer) - 1UL; } else { - return {}; + return reinterpret_cast(buffer_write_ptr) - reinterpret_cast(buffer); } } diff --git a/TactilityHeadless/Source/service/gps/Gps.cpp b/TactilityHeadless/Source/service/gps/Gps.cpp index d9af350c..44a43a12 100644 --- a/TactilityHeadless/Source/service/gps/Gps.cpp +++ b/TactilityHeadless/Source/service/gps/Gps.cpp @@ -1,3 +1,4 @@ +#include "Tactility/PubSub.h" #include "Tactility/service/ServiceManifest.h" #include "Tactility/service/ServiceRegistry.h" #include "Tactility/service/gps/GpsService.h" @@ -14,7 +15,6 @@ static std::shared_ptr findGpsService() { return std::static_pointer_cast(service); } - void addGpsDevice(const std::shared_ptr& device) { return findGpsService()->addGpsDevice(device); } @@ -31,8 +31,20 @@ void stopReceiving() { findGpsService()->stopReceiving(); } -bool isReceiving() { - return findGpsService()->isReceiving(); +bool hasCoordinates() { + return findGpsService()->hasCoordinates(); +} + +bool getCoordinates(minmea_sentence_rmc& rmc) { + return findGpsService()->getCoordinates(rmc); +} + +State getState() { + return findGpsService()->getState(); +} + +std::shared_ptr getStatePubsub() { + return findGpsService()->getStatePubsub(); } } // namespace tt::service::gps diff --git a/TactilityHeadless/Source/service/gps/GpsService.cpp b/TactilityHeadless/Source/service/gps/GpsService.cpp index 4ef52050..d8cd0a27 100644 --- a/TactilityHeadless/Source/service/gps/GpsService.cpp +++ b/TactilityHeadless/Source/service/gps/GpsService.cpp @@ -7,6 +7,10 @@ using tt::hal::gps::GpsDevice; namespace tt::service::gps { +constexpr inline bool hasTimeElapsed(TickType_t now, TickType_t timeInThePast, TickType_t expireTimeInTicks) { + return (TickType_t)(now - timeInThePast) >= expireTimeInTicks; +} + GpsService::GpsDeviceRecord* _Nullable GpsService::findGpsRecord(const std::shared_ptr& device) { auto lock = mutex.asScopedLock(); lock.lock(); @@ -27,7 +31,7 @@ void GpsService::addGpsDevice(const std::shared_ptr& device) { GpsDeviceRecord record = { .device = device }; - if (isReceiving()) { + if (getState() == State::On) { // Ignore during OnPending due to risk of data corruptiohn startGpsDevice(record); } @@ -40,7 +44,7 @@ void GpsService::removeGpsDevice(const std::shared_ptr& device) { GpsDeviceRecord* record = findGpsRecord(device); - if (isReceiving()) { + if (getState() == State::On) { // Ignore during OnPending due to risk of data corruptiohn stopGpsDevice(*record); } @@ -63,7 +67,7 @@ void GpsService::onStart(tt::service::ServiceContext &serviceContext) { } void GpsService::onStop(tt::service::ServiceContext &serviceContext) { - if (isReceiving()) { + if (getState() == State::On) { stopReceiving(); } } @@ -81,12 +85,20 @@ bool GpsService::startGpsDevice(GpsDeviceRecord& record) { return false; } - record.satelliteSubscriptionId = device->subscribeSatellites([this](hal::Device::Id deviceId, auto& record) { - onSatelliteInfo(deviceId, record); + record.satelliteSubscriptionId = device->subscribeGga([this](hal::Device::Id deviceId, auto& record) { + mutex.lock(); + onGgaSentence(deviceId, record); + mutex.unlock(); }); - record.locationSubscriptionId = device->subscribeLocations([this](hal::Device::Id deviceId, auto& record) { + record.rmcSubscriptionId = device->subscribeRmc([this](hal::Device::Id deviceId, auto& record) { + mutex.lock(); + if (record.longitude.value != 0 && record.longitude.scale != 0) { + rmcRecord = record; + rmcTime = kernel::getTicks(); + } onRmcSentence(deviceId, record); + mutex.unlock(); }); return true; @@ -97,23 +109,25 @@ bool GpsService::stopGpsDevice(GpsDeviceRecord& record) { auto device = record.device; + device->unsubscribeGga(record.satelliteSubscriptionId); + device->unsubscribeRmc(record.rmcSubscriptionId); + + record.satelliteSubscriptionId = -1; + record.rmcSubscriptionId = -1; + if (!device->stop()) { TT_LOG_E(TAG, "[device %lu] stopping failed", record.device->getId()); return false; } - device->unsubscribeSatellites(record.satelliteSubscriptionId); - device->unsubscribeLocations(record.locationSubscriptionId); - - record.satelliteSubscriptionId = -1; - record.locationSubscriptionId = -1; - return true; } bool GpsService::startReceiving() { TT_LOG_I(TAG, "Start receiving"); + setState(State::OnPending); + auto lock = mutex.asScopedLock(); lock.lock(); @@ -123,14 +137,22 @@ bool GpsService::startReceiving() { started_one_or_more |= startGpsDevice(record); } - receiving = started_one_or_more; + rmcTime = 0; - return receiving; + if (started_one_or_more) { + setState(State::On); + return true; + } else { + setState(State::Off); + return false; + } } void GpsService::stopReceiving() { TT_LOG_I(TAG, "Stop receiving"); + setState(State::OffPending); + auto lock = mutex.asScopedLock(); lock.lock(); @@ -138,21 +160,46 @@ void GpsService::stopReceiving() { stopGpsDevice(record); } - receiving = false; + rmcTime = 0; + + setState(State::Off); } -bool GpsService::isReceiving() { - auto lock = mutex.asScopedLock(); - lock.lock(); - return receiving; -} - -void GpsService::onSatelliteInfo(hal::Device::Id deviceId, const minmea_sat_info& info) { - TT_LOG_I(TAG, "[device %lu] satellite %d", deviceId, info.nr); +void GpsService::onGgaSentence(hal::Device::Id deviceId, const minmea_sentence_gga& gga) { + TT_LOG_D(TAG, "[device %lu] LAT %f LON %f, satellites: %d", deviceId, minmea_tocoord(&gga.latitude), minmea_tocoord(&gga.longitude), gga.satellites_tracked); } void GpsService::onRmcSentence(hal::Device::Id deviceId, const minmea_sentence_rmc& rmc) { - TT_LOG_I(TAG, "[device %lu] coordinates %f %f", deviceId, minmea_tofloat(&rmc.longitude), minmea_tofloat(&rmc.latitude)); + TT_LOG_D(TAG, "[device %lu] LAT %f LON %f, speed: %.2f", deviceId, minmea_tocoord(&rmc.latitude), minmea_tocoord(&rmc.longitude), minmea_tofloat(&rmc.speed)); +} + +State GpsService::getState() const { + auto lock = stateMutex.asScopedLock(); + lock.lock(); + return state; +} + +void GpsService::setState(State newState) { + auto lock = stateMutex.asScopedLock(); + lock.lock(); + state = newState; + lock.unlock(); + statePubSub->publish(&state); +} + +bool GpsService::hasCoordinates() const { + auto lock = mutex.asScopedLock(); + lock.lock(); + return getState() == State::On && rmcTime != 0 && !hasTimeElapsed(kernel::getTicks(), rmcTime, kernel::secondsToTicks(10)); +} + +bool GpsService::getCoordinates(minmea_sentence_rmc& rmc) const { + if (hasCoordinates()) { + rmc = rmcRecord; + return true; + } else { + return false; + } } extern const ServiceManifest manifest = {