mirror of
https://github.com/ByteWelder/Tactility.git
synced 2026-04-18 09:25:06 +00:00
Improved GPS Settings app and GPS service (#222)
- Fixes and improvements to `GpsSettings` app, `GpsDevice` and `GpsService` - Implemented location/GPS statusbar icon - Added app icon - Added support for other GPS models (based on Meshtastic code)
This commit is contained in:
Ken Van Hoeylandt
GitHub
parent
ad2cad3bf1
commit
5055fa7822
@ -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
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
@ -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.
|
||||
|
||||
BIN
Data/system/service/Statusbar/location.png
Normal file
BIN
Data/system/service/Statusbar/location.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 528 B |
42
Data/system_sources/location.svg
Normal file
42
Data/system_sources/location.svg
Normal file
@ -0,0 +1,42 @@
|
||||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<svg
|
||||
height="16"
|
||||
viewBox="0 -960 640 640"
|
||||
width="16"
|
||||
fill="#e8eaed"
|
||||
version="1.1"
|
||||
id="svg1"
|
||||
sodipodi:docname="location.svg"
|
||||
inkscape:version="1.4 (e7c3feb100, 2024-10-09)"
|
||||
inkscape:export-filename="location.png"
|
||||
inkscape:export-xdpi="96"
|
||||
inkscape:export-ydpi="96"
|
||||
xmlns:inkscape="http://www.inkscape.org/namespaces/inkscape"
|
||||
xmlns:sodipodi="http://sodipodi.sourceforge.net/DTD/sodipodi-0.dtd"
|
||||
xmlns="http://www.w3.org/2000/svg"
|
||||
xmlns:svg="http://www.w3.org/2000/svg">
|
||||
<defs
|
||||
id="defs1" />
|
||||
<sodipodi:namedview
|
||||
id="namedview1"
|
||||
pagecolor="#505050"
|
||||
bordercolor="#eeeeee"
|
||||
borderopacity="1"
|
||||
inkscape:showpageshadow="0"
|
||||
inkscape:pageopacity="0"
|
||||
inkscape:pagecheckerboard="0"
|
||||
inkscape:deskcolor="#d1d1d1"
|
||||
inkscape:zoom="47.5"
|
||||
inkscape:cx="12"
|
||||
inkscape:cy="12"
|
||||
inkscape:window-width="2560"
|
||||
inkscape:window-height="1368"
|
||||
inkscape:window-x="0"
|
||||
inkscape:window-y="0"
|
||||
inkscape:window-maximized="1"
|
||||
inkscape:current-layer="svg1" />
|
||||
<path
|
||||
d="M 347.36841,-361.6842 259.36842,-579.36842 41.684211,-667.36841 V -710.5965 L 597.47368,-917.47368 390.59649,-361.6842 Z m 20.07018,-114.24563 125.05263,-336.5614 -336.5614,125.05264 151.29825,60.21052 z m -60.21052,-151.29824 z"
|
||||
id="path1"
|
||||
style="stroke-width:0.771924" />
|
||||
</svg>
|
||||
|
After Width: | Height: | Size: 1.4 KiB |
@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" height="20" viewBox="0 -960 960 960" width="20"><path d="M360-528h72v-144h-72v144Zm120 0h72v-144h-72v144Zm120 0h72v-144h-72v144ZM263.717-96Q234-96 213-117.15T192-168v-504l192-192h312q29.7 0 50.85 21.15Q768-821.7 768-792v624q0 29.7-21.162 50.85Q725.676-96 695.96-96H263.717Zm.283-72h432v-624H414L264-642v474Zm0 0h432-432Z"/></svg>
|
||||
|
Before Width: | Height: | Size: 369 B |
@ -1 +0,0 @@
|
||||
<svg xmlns="http://www.w3.org/2000/svg" height="20" viewBox="0 -960 960 960" width="20"><path d="M263.717-96Q234-96 213-117.15T192-168v-504l192-192h312q29.7 0 50.85 21.15Q768-821.7 768-792v624q0 29.7-21.162 50.85Q725.676-96 695.96-96H263.717Zm.283-72h432v-624H414L264-642v474Zm215.789-120Q495-288 505.5-298.289q10.5-10.29 10.5-25.5Q516-339 505.711-349.5q-10.29-10.5-25.5-10.5Q465-360 454.5-349.711q-10.5 10.29-10.5 25.5Q444-309 454.289-298.5q10.29 10.5 25.5 10.5ZM444-432h72v-192h-72v192ZM264-168h432-432Z"/></svg>
|
||||
|
Before Width: | Height: | Size: 514 B |
@ -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 <Tactility/service/gps/Gps.h>
|
||||
|
||||
#include <format>
|
||||
#include <lvgl.h>
|
||||
|
||||
#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> timer;
|
||||
std::shared_ptr<GpsSettingsApp*> appReference = std::make_shared<GpsSettingsApp*>(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<void> context) {
|
||||
auto appPtr = std::static_pointer_cast<GpsSettingsApp*>(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::gps::GpsDevice>(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>(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<GpsSettingsApp>
|
||||
};
|
||||
|
||||
@ -3,6 +3,7 @@
|
||||
|
||||
#include "Tactility/hal/power/PowerDevice.h"
|
||||
#include "Tactility/hal/sdcard/SdCardDevice.h"
|
||||
#include "Tactility/service/gps/Gps.h"
|
||||
#include <Tactility/Mutex.h>
|
||||
#include <Tactility/Tactility.h>
|
||||
#include <Tactility/TactilityHeadless.h>
|
||||
@ -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<Timer> 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();
|
||||
|
||||
@ -58,6 +58,11 @@ std::basic_string<T> lowercase(const std::basic_string<T>& 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.
|
||||
*/
|
||||
|
||||
@ -1,6 +1,7 @@
|
||||
#include "Tactility/StringUtils.h"
|
||||
|
||||
#include <cstring>
|
||||
#include <ranges>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
|
||||
@ -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
|
||||
|
||||
@ -2,7 +2,6 @@
|
||||
|
||||
#include "../Device.h"
|
||||
#include "../uart/Uart.h"
|
||||
#include "GpsDeviceInitDefault.h"
|
||||
#include "Satellites.h"
|
||||
|
||||
#include <Tactility/Mutex.h>
|
||||
@ -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<bool(uart_port_t)> initFunction = initGpsDefault;
|
||||
GpsModel model;
|
||||
};
|
||||
|
||||
enum class State {
|
||||
PendingOn,
|
||||
On,
|
||||
Error,
|
||||
PendingOff,
|
||||
Off
|
||||
};
|
||||
|
||||
private:
|
||||
|
||||
struct SatelliteSubscription {
|
||||
SatelliteSubscriptionId id;
|
||||
std::shared_ptr<std::function<void(Device::Id id, const minmea_sat_info&)>> onData;
|
||||
struct GgaSubscription {
|
||||
GgaSubscriptionId id;
|
||||
std::shared_ptr<std::function<void(Device::Id id, const minmea_sentence_gga&)>> onData;
|
||||
};
|
||||
|
||||
struct LocationSubscription {
|
||||
LocationSubscriptionId id;
|
||||
struct RmcSubscription {
|
||||
RmcSubscriptionId id;
|
||||
std::shared_ptr<std::function<void(Device::Id id, const minmea_sentence_rmc&)>> onData;
|
||||
};
|
||||
|
||||
const Configuration configuration;
|
||||
Mutex mutex;
|
||||
Mutex mutex = Mutex(Mutex::Type::Recursive);
|
||||
std::unique_ptr<Thread> thread;
|
||||
bool threadInterrupted = false;
|
||||
std::vector<SatelliteSubscription> satelliteSubscriptions;
|
||||
std::vector<LocationSubscription> locationSubscriptions;
|
||||
SatelliteSubscriptionId lastSatelliteSubscriptionId = 0;
|
||||
LocationSubscriptionId lastLocationSubscriptionId = 0;
|
||||
std::vector<GgaSubscription> ggaSubscriptions;
|
||||
std::vector<RmcSubscription> 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<void(Device::Id deviceId, const minmea_sat_info&)>& onData) {
|
||||
satelliteSubscriptions.push_back({
|
||||
GgaSubscriptionId subscribeGga(const std::function<void(Device::Id deviceId, const minmea_sentence_gga&)>& onData) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
ggaSubscriptions.push_back({
|
||||
.id = ++lastSatelliteSubscriptionId,
|
||||
.onData = std::make_shared<std::function<void(Device::Id, const minmea_sat_info&)>>(onData)
|
||||
.onData = std::make_shared<std::function<void(Device::Id, const minmea_sentence_gga&)>>(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<void(Device::Id deviceId, const minmea_sentence_rmc&)>& onData) {
|
||||
locationSubscriptions.push_back({
|
||||
.id = ++lastLocationSubscriptionId,
|
||||
RmcSubscriptionId subscribeRmc(const std::function<void(Device::Id deviceId, const minmea_sentence_rmc&)>& onData) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
rmcSubscriptions.push_back({
|
||||
.id = ++lastRmcSubscriptionId,
|
||||
.onData = std::make_shared<std::function<void(Device::Id, const minmea_sentence_rmc&)>>(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;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@ -1,9 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <Tactility/hal/uart/Uart.h>
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
bool initGpsDefault(uart_port_t port);
|
||||
|
||||
}
|
||||
@ -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
|
||||
|
||||
@ -1,6 +1,9 @@
|
||||
#pragma once
|
||||
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "GpsState.h"
|
||||
|
||||
#include <Tactility/PubSub.h>
|
||||
|
||||
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<PubSub> getStatePubsub();
|
||||
|
||||
}
|
||||
|
||||
12
TactilityHeadless/Include/Tactility/service/gps/GpsState.h
Normal file
12
TactilityHeadless/Include/Tactility/service/gps/GpsState.h
Normal file
@ -0,0 +1,12 @@
|
||||
#pragma once
|
||||
|
||||
namespace tt::service::gps {
|
||||
|
||||
enum class State {
|
||||
OnPending,
|
||||
On,
|
||||
OffPending,
|
||||
Off
|
||||
};
|
||||
|
||||
}
|
||||
@ -76,7 +76,7 @@ struct ApRecord {
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Get wifi pubsub
|
||||
* @brief Get wifi pubsub that broadcasts Event objects
|
||||
* @return PubSub
|
||||
*/
|
||||
std::shared_ptr<PubSub> getPubsub();
|
||||
|
||||
68
TactilityHeadless/Private/Tactility/hal/gps/Cas.h
Normal file
68
TactilityHeadless/Private/Tactility/hal/gps/Cas.h
Normal file
@ -0,0 +1,68 @@
|
||||
/**
|
||||
* Source: https://raw.githubusercontent.com/meshtastic/firmware/3b0232de1b6282eacfbff6e50b68fca7e67b8511/src/gps/cas.h
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
// 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
|
||||
};
|
||||
@ -11,4 +11,9 @@ namespace tt::hal::gps {
|
||||
*/
|
||||
bool init(const std::vector<GpsDevice::Configuration>& configurations);
|
||||
|
||||
/**
|
||||
* Init sequence on UART for a specific GPS model.
|
||||
*/
|
||||
bool init(uart_port_t port, GpsModel type);
|
||||
|
||||
}
|
||||
|
||||
12
TactilityHeadless/Private/Tactility/hal/gps/Probe.h
Normal file
12
TactilityHeadless/Private/Tactility/hal/gps/Probe.h
Normal file
@ -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);
|
||||
|
||||
}
|
||||
27
TactilityHeadless/Private/Tactility/hal/gps/Ublox.h
Normal file
27
TactilityHeadless/Private/Tactility/hal/gps/Ublox.h
Normal file
@ -0,0 +1,27 @@
|
||||
#pragma once
|
||||
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/hal/uart/Uart.h"
|
||||
|
||||
#include <cstdint>
|
||||
#include <cstddef>
|
||||
|
||||
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<size_t DataSize>
|
||||
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
|
||||
467
TactilityHeadless/Private/Tactility/hal/gps/UbloxMessages.h
Normal file
467
TactilityHeadless/Private/Tactility/hal/gps/UbloxMessages.h
Normal file
@ -0,0 +1,467 @@
|
||||
#pragma once
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
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
|
||||
*/
|
||||
|
||||
}
|
||||
@ -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<hal::gps::GpsDevice> 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<GpsDeviceRecord> deviceRecords;
|
||||
bool receiving = false;
|
||||
std::shared_ptr<PubSub> statePubSub = std::make_shared<PubSub>();
|
||||
State state = State::Off;
|
||||
|
||||
bool startGpsDevice(GpsDeviceRecord& deviceRecord);
|
||||
static bool stopGpsDevice(GpsDeviceRecord& deviceRecord);
|
||||
|
||||
GpsDeviceRecord* _Nullable findGpsRecord(const std::shared_ptr<hal::gps::GpsDevice>& 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<PubSub> getStatePubsub() const { return statePubSub; }
|
||||
};
|
||||
|
||||
} // tt::hal::gps
|
||||
|
||||
@ -1,12 +1,48 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include <minmea.h>
|
||||
#include "Tactility/hal/gps/GpsInit.h"
|
||||
#include "Tactility/hal/gps/Probe.h"
|
||||
#include <cstring>
|
||||
#include <minmea.h>
|
||||
|
||||
#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<Thread>(
|
||||
"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
|
||||
|
||||
@ -1,214 +0,0 @@
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/hal/uart/Uart.h>
|
||||
#include <Tactility/kernel/Kernel.h>
|
||||
|
||||
#define TAG "gps"
|
||||
#define GPS_UART_BUFFER_SIZE 256
|
||||
|
||||
using namespace tt;
|
||||
using namespace tt::hal;
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
static int ackGps(uart_port_t port, uint8_t* buffer, uint16_t size, uint8_t requestedClass, uint8_t requestedId) {
|
||||
uint16_t ubxFrameCounter = 0;
|
||||
uint32_t startTime = kernel::getTicks();
|
||||
uint16_t needRead;
|
||||
|
||||
while (kernel::getTicks() - startTime < 800) {
|
||||
while (uart::available(port)) {
|
||||
uint8_t c;
|
||||
uart::readByte(port, &c);
|
||||
switch (ubxFrameCounter) {
|
||||
case 0:
|
||||
if (c == 0xB5) {
|
||||
ubxFrameCounter++;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (c == 0x62) {
|
||||
ubxFrameCounter++;
|
||||
} else {
|
||||
ubxFrameCounter = 0;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
if (c == requestedClass) {
|
||||
ubxFrameCounter++;
|
||||
} else {
|
||||
ubxFrameCounter = 0;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if (c == requestedId) {
|
||||
ubxFrameCounter++;
|
||||
} else {
|
||||
ubxFrameCounter = 0;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
needRead = c;
|
||||
ubxFrameCounter++;
|
||||
break;
|
||||
case 5:
|
||||
needRead |= (c << 8);
|
||||
ubxFrameCounter++;
|
||||
break;
|
||||
case 6:
|
||||
if (needRead >= size) {
|
||||
ubxFrameCounter = 0;
|
||||
break;
|
||||
}
|
||||
if (uart::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
|
||||
@ -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 <cstring>
|
||||
|
||||
#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<GpsDevice::Configuration>& configurations) {
|
||||
for (auto& configuration : configurations) {
|
||||
auto device = std::make_shared<GpsDevice>(configuration);
|
||||
@ -12,4 +144,157 @@ bool init(const std::vector<GpsDevice::Configuration>& 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<int>(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
|
||||
|
||||
140
TactilityHeadless/Source/hal/gps/Probe.cpp
Normal file
140
TactilityHeadless/Source/hal/gps/Probe.cpp
Normal file
@ -0,0 +1,140 @@
|
||||
#include "Tactility/hal/gps/GpsDevice.h"
|
||||
#include "Tactility/hal/gps/Ublox.h"
|
||||
#include <Tactility/Log.h>
|
||||
#include <Tactility/hal/uart/Uart.h>
|
||||
#include <Tactility/kernel/Kernel.h>
|
||||
#include <cstring>
|
||||
|
||||
#define TAG "gps"
|
||||
#define GPS_UART_BUFFER_SIZE 256
|
||||
|
||||
using namespace tt;
|
||||
using namespace tt::hal;
|
||||
|
||||
namespace tt::hal::gps {
|
||||
|
||||
/**
|
||||
* From: https://github.com/meshtastic/firmware/blob/3b0232de1b6282eacfbff6e50b68fca7e67b8511/src/meshUtils.cpp#L40
|
||||
*/
|
||||
char* strnstr(const char* s, const char* find, size_t slen) {
|
||||
char c;
|
||||
if ((c = *find++) != '\0') {
|
||||
char sc;
|
||||
size_t len;
|
||||
|
||||
len = strlen(find);
|
||||
do {
|
||||
do {
|
||||
if (slen-- < 1 || (sc = *s++) == '\0')
|
||||
return (nullptr);
|
||||
} while (sc != c);
|
||||
if (len > slen)
|
||||
return (nullptr);
|
||||
} while (strncmp(s, find, len) != 0);
|
||||
s--;
|
||||
}
|
||||
return ((char*)s);
|
||||
}
|
||||
|
||||
/**
|
||||
* From: https://github.com/meshtastic/firmware/blob/f81d3b045dd1b7e3ca7870af3da915ff4399ea98/src/gps/GPS.cpp
|
||||
*/
|
||||
GpsResponse getAck(uart_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
|
||||
479
TactilityHeadless/Source/hal/gps/Ublox.cpp
Normal file
479
TactilityHeadless/Source/hal/gps/Ublox.cpp
Normal file
@ -0,0 +1,479 @@
|
||||
#include "Tactility/hal/gps/Ublox.h"
|
||||
#include "Tactility/hal/gps/UbloxMessages.h"
|
||||
#include <cstring>
|
||||
|
||||
#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
|
||||
@ -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<size_t>(buffer_write_ptr) - reinterpret_cast<size_t>(buffer) - 1UL;
|
||||
} else {
|
||||
return {};
|
||||
return reinterpret_cast<size_t>(buffer_write_ptr) - reinterpret_cast<size_t>(buffer);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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<GpsService> findGpsService() {
|
||||
return std::static_pointer_cast<GpsService>(service);
|
||||
}
|
||||
|
||||
|
||||
void addGpsDevice(const std::shared_ptr<GpsDevice>& 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<PubSub> getStatePubsub() {
|
||||
return findGpsService()->getStatePubsub();
|
||||
}
|
||||
|
||||
} // namespace tt::service::gps
|
||||
|
||||
@ -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<GpsDevice>& device) {
|
||||
auto lock = mutex.asScopedLock();
|
||||
lock.lock();
|
||||
@ -27,7 +31,7 @@ void GpsService::addGpsDevice(const std::shared_ptr<GpsDevice>& 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<GpsDevice>& 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 = {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user