Remove old HAL components and refactored GPS-related code (#583)
- Added generic GPS/GNSS support with device detection, configuration, and persistent settings. - Improved device and module lifecycle management. - Added flexible filesystem locking support for displays and storage. - Improved display-idle and keyboard backlight handling. - Updated architecture, driver, module, testing, and licensing documentation. - Removed old HAL device and related code.
This commit is contained in:
Ken Van Hoeylandt
committed by
GitHub
GitHub
parent
29e80cfd65
commit
2a2558b29a
@@ -0,0 +1,343 @@
|
||||
// SPDX-License-Identifier: GPL-3.0-or-later
|
||||
#include <gps/gps.h>
|
||||
#include <gps_generic/gps_generic.h>
|
||||
|
||||
#include <gps_generic/private/init.h>
|
||||
#include <gps_generic/private/probe.h>
|
||||
|
||||
#include <tactility/check.h>
|
||||
#include <tactility/concurrent/recursive_mutex.h>
|
||||
#include <tactility/concurrent/thread.h>
|
||||
#include <tactility/device.h>
|
||||
#include <tactility/driver.h>
|
||||
#include <tactility/drivers/uart_controller.h>
|
||||
#include <tactility/error.h>
|
||||
#include <tactility/log.h>
|
||||
#include <tactility/module.h>
|
||||
#include <tactility/time.h>
|
||||
|
||||
#include <minmea.h>
|
||||
|
||||
#include <cstdio>
|
||||
#include <cstdlib> // For calloc() in PC builds
|
||||
|
||||
constexpr auto* TAG = "gps-generic";
|
||||
|
||||
#define GET_CONFIG(device) (static_cast<const GpsConfig*>((device)->config))
|
||||
|
||||
constexpr uint32_t GPS_UART_BUFFER_SIZE = 256;
|
||||
constexpr TickType_t GPS_THREAD_STOP_TIMEOUT_TICKS = pdMS_TO_TICKS(5000);
|
||||
constexpr TickType_t GPS_THREAD_STOP_POLL_TICKS = pdMS_TO_TICKS(1000);
|
||||
|
||||
struct GpsInternal {
|
||||
RecursiveMutex mutex;
|
||||
Thread* thread;
|
||||
volatile bool interrupt_requested;
|
||||
GpsState state;
|
||||
// Mirrors GpsConfig::model, but overwritten with the autodetected model once probing succeeds.
|
||||
GpsModel model;
|
||||
// Singly-linked list of subscribers, guarded by `mutex`.
|
||||
GpsSubscription* subscribers;
|
||||
};
|
||||
|
||||
static const char* gpsModelToString(GpsModel model) {
|
||||
switch (model) {
|
||||
case GPS_MODEL_AG3335:
|
||||
return "AG3335";
|
||||
case GPS_MODEL_AG3352:
|
||||
return "AG3352";
|
||||
case GPS_MODEL_ATGM336H:
|
||||
return "ATGM336H";
|
||||
case GPS_MODEL_LS20031:
|
||||
return "LS20031";
|
||||
case GPS_MODEL_MTK:
|
||||
return "MTK";
|
||||
case GPS_MODEL_MTK_L76B:
|
||||
return "MTK L76B";
|
||||
case GPS_MODEL_MTK_PA1616S:
|
||||
return "MTK PA1616S";
|
||||
case GPS_MODEL_UBLOX6:
|
||||
return "U-blox 6";
|
||||
case GPS_MODEL_UBLOX7:
|
||||
return "U-blox 7";
|
||||
case GPS_MODEL_UBLOX8:
|
||||
return "U-blox 8";
|
||||
case GPS_MODEL_UBLOX9:
|
||||
return "U-blox 9";
|
||||
case GPS_MODEL_UBLOX10:
|
||||
return "U-blox 10";
|
||||
case GPS_MODEL_UC6580:
|
||||
return "UC6580";
|
||||
case GPS_MODEL_UNKNOWN:
|
||||
return "Auto-detect";
|
||||
default:
|
||||
return "Unknown";
|
||||
}
|
||||
}
|
||||
|
||||
// Pushes `event` to every current subscriber and wakes their waiting task. Safe to call from the
|
||||
// GPS thread's parsing loop.
|
||||
static void notify_subscribers(GpsInternal* internal, const GpsEvent& event) {
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
|
||||
for (GpsSubscription* sub = internal->subscribers; sub != nullptr; sub = sub->next) {
|
||||
sub->event = event;
|
||||
sub->sequence++;
|
||||
xTaskNotifyGive(sub->task);
|
||||
}
|
||||
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
}
|
||||
|
||||
static void set_state(GpsInternal* internal, GpsState state) {
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
internal->state = state;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
}
|
||||
|
||||
static bool is_interrupted(GpsInternal* internal) {
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
bool result = internal->interrupt_requested;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
return result;
|
||||
}
|
||||
|
||||
// region Driver lifecycle
|
||||
|
||||
static int32_t gps_thread_main(void* context) {
|
||||
auto* device = static_cast<Device*>(context);
|
||||
auto* internal = static_cast<GpsInternal*>(device_get_driver_data(device));
|
||||
const auto* config = GET_CONFIG(device);
|
||||
auto* uart = device_get_parent(device);
|
||||
check(uart);
|
||||
check(device_get_type(uart) == &UART_CONTROLLER_TYPE);
|
||||
|
||||
const UartConfig uart_config = {
|
||||
.baud_rate = config->baud_rate,
|
||||
.data_bits = UART_CONTROLLER_DATA_8_BITS,
|
||||
.parity = UART_CONTROLLER_PARITY_DISABLE,
|
||||
.stop_bits = UART_CONTROLLER_STOP_BITS_1
|
||||
};
|
||||
|
||||
if (uart_controller_set_config(uart, &uart_config) != ERROR_NONE) {
|
||||
LOG_E(TAG, "Failed to configure UART %s", uart->name);
|
||||
set_state(internal, GpsState::GPS_STATE_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
if (uart_controller_open(uart) != ERROR_NONE) {
|
||||
LOG_E(TAG, "Failed to open UART %s", uart->name);
|
||||
set_state(internal, GpsState::GPS_STATE_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
GpsModel model = internal->model;
|
||||
if (model == GpsModel::GPS_MODEL_UNKNOWN) {
|
||||
model = gps_probe(uart);
|
||||
if (model == GpsModel::GPS_MODEL_UNKNOWN) {
|
||||
LOG_E(TAG, "Probe failed");
|
||||
set_state(internal, GpsState::GPS_STATE_ERROR);
|
||||
return -1;
|
||||
}
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
internal->model = model;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
}
|
||||
|
||||
if (!gps_init(uart, model)) {
|
||||
LOG_E(TAG, "Init failed");
|
||||
set_state(internal, GpsState::GPS_STATE_ERROR);
|
||||
return -1;
|
||||
}
|
||||
|
||||
set_state(internal, GpsState::GPS_STATE_ON);
|
||||
|
||||
// Reference: https://gpsd.gitlab.io/gpsd/NMEA.html
|
||||
uint8_t buffer[GPS_UART_BUFFER_SIZE];
|
||||
while (!is_interrupted(internal)) {
|
||||
size_t bytes_read = 0;
|
||||
uart_controller_read_until(uart, buffer, sizeof(buffer), '\n', true, &bytes_read, pdMS_TO_TICKS(100));
|
||||
|
||||
// Thread might've been interrupted in the meanwhile
|
||||
if (is_interrupted(internal)) {
|
||||
break;
|
||||
}
|
||||
|
||||
if (bytes_read > 0U) {
|
||||
switch (minmea_sentence_id(reinterpret_cast<char*>(buffer), false)) {
|
||||
case MINMEA_SENTENCE_RMC: {
|
||||
GpsEvent event { .type = GPS_EVENT_MESSAGE_RMC };
|
||||
if (minmea_parse_rmc(&event.data.rmc, reinterpret_cast<char*>(buffer))) {
|
||||
notify_subscribers(internal, event);
|
||||
} else {
|
||||
LOG_E(TAG, "RMC parse error: %s", reinterpret_cast<const char*>(buffer));
|
||||
}
|
||||
break;
|
||||
}
|
||||
case MINMEA_SENTENCE_GGA: {
|
||||
GpsEvent event { .type = GPS_EVENT_MESSAGE_GGA };
|
||||
if (minmea_parse_gga(&event.data.gga, reinterpret_cast<char*>(buffer))) {
|
||||
notify_subscribers(internal, event);
|
||||
} else {
|
||||
LOG_E(TAG, "GGA parse error: %s", reinterpret_cast<const char*>(buffer));
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (uart_controller_close(uart) != ERROR_NONE) {
|
||||
LOG_W(TAG, "Failed to close UART %s", uart->name);
|
||||
}
|
||||
|
||||
// Wake any subscribers still awaiting an event so they don't block forever on a device that's
|
||||
// going away, then drop them - stop() is about to free `internal`.
|
||||
notify_subscribers(internal, GpsEvent { .type = GPS_EVENT_UNSUBSCRIBED });
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
internal->subscribers = nullptr;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
|
||||
set_state(internal, GPS_STATE_OFF);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static error_t start(Device* device) {
|
||||
const auto* config = GET_CONFIG(device);
|
||||
|
||||
auto* internal = static_cast<GpsInternal*>(calloc(1, sizeof(GpsInternal)));
|
||||
if (internal == nullptr) {
|
||||
return ERROR_OUT_OF_MEMORY;
|
||||
}
|
||||
|
||||
recursive_mutex_construct(&internal->mutex);
|
||||
internal->model = config->model;
|
||||
internal->state = GPS_STATE_PENDING_ON;
|
||||
internal->thread = thread_alloc_full("gps", 4096, gps_thread_main, device, -1);
|
||||
if (internal->thread == nullptr) {
|
||||
recursive_mutex_destruct(&internal->mutex);
|
||||
free(internal);
|
||||
return ERROR_OUT_OF_MEMORY;
|
||||
}
|
||||
thread_set_priority(internal->thread, THREAD_PRIORITY_HIGH);
|
||||
|
||||
device_set_driver_data(device, internal);
|
||||
|
||||
if (thread_start(internal->thread) != ERROR_NONE) {
|
||||
thread_free(internal->thread);
|
||||
recursive_mutex_destruct(&internal->mutex);
|
||||
free(internal);
|
||||
device_set_driver_data(device, nullptr);
|
||||
return ERROR_RESOURCE;
|
||||
}
|
||||
|
||||
return ERROR_NONE;
|
||||
}
|
||||
|
||||
static error_t stop(Device* device) {
|
||||
auto* internal = static_cast<GpsInternal*>(device_get_driver_data(device));
|
||||
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
internal->interrupt_requested = true;
|
||||
internal->state = GPS_STATE_PENDING_OFF;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
|
||||
if (thread_join(internal->thread, GPS_THREAD_STOP_TIMEOUT_TICKS, GPS_THREAD_STOP_POLL_TICKS) != ERROR_NONE) {
|
||||
LOG_E(TAG, "GPS thread for %s did not stop in time", device->name);
|
||||
return ERROR_RESOURCE_BUSY;
|
||||
}
|
||||
thread_free(internal->thread);
|
||||
|
||||
recursive_mutex_destruct(&internal->mutex);
|
||||
free(internal);
|
||||
device_set_driver_data(device, nullptr);
|
||||
return ERROR_NONE;
|
||||
}
|
||||
|
||||
// endregion
|
||||
|
||||
// region GpsApi
|
||||
|
||||
static error_t gps_api_event_subscribe(Device* device, GpsSubscription* sub) {
|
||||
auto* internal = static_cast<GpsInternal*>(device_get_driver_data(device));
|
||||
|
||||
sub->task = xTaskGetCurrentTaskHandle();
|
||||
sub->sequence = 0;
|
||||
sub->consumed_sequence = 0;
|
||||
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
sub->next = internal->subscribers;
|
||||
internal->subscribers = sub;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
|
||||
return ERROR_NONE;
|
||||
}
|
||||
|
||||
static error_t gps_api_event_unsubscribe(Device* device, GpsSubscription* sub) {
|
||||
auto* internal = static_cast<GpsInternal*>(device_get_driver_data(device));
|
||||
|
||||
error_t result = ERROR_NOT_FOUND;
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
for (GpsSubscription** link = &internal->subscribers; *link != nullptr; link = &(*link)->next) {
|
||||
if (*link == sub) {
|
||||
*link = sub->next;
|
||||
result = ERROR_NONE;
|
||||
break;
|
||||
}
|
||||
}
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
static error_t gps_api_event_await(Device*, GpsSubscription* sub, TickType_t timeout) {
|
||||
uint32_t old_sequence = sub->sequence;
|
||||
|
||||
while (sub->sequence == old_sequence) {
|
||||
if (ulTaskNotifyTake(pdTRUE, timeout) == 0) {
|
||||
return ERROR_TIMEOUT;
|
||||
}
|
||||
}
|
||||
|
||||
sub->consumed_sequence = sub->sequence;
|
||||
return ERROR_NONE;
|
||||
}
|
||||
|
||||
static GpsState gps_api_get_state(Device* device) {
|
||||
auto* internal = static_cast<GpsInternal*>(device_get_driver_data(device));
|
||||
recursive_mutex_lock(&internal->mutex);
|
||||
auto state = internal->state;
|
||||
recursive_mutex_unlock(&internal->mutex);
|
||||
return state;
|
||||
}
|
||||
|
||||
static error_t gps_api_get_model_name(Device* device, char* model_name, size_t buffer_size) {
|
||||
const auto* config = GET_CONFIG(device);
|
||||
const char* name_to_set = gpsModelToString(config->model);
|
||||
snprintf(model_name, buffer_size, "%s", name_to_set);
|
||||
return ERROR_NONE;
|
||||
}
|
||||
|
||||
// endregion
|
||||
|
||||
static const GpsApi generic_gps_api = {
|
||||
.event_subscribe = gps_api_event_subscribe,
|
||||
.event_unsubscribe = gps_api_event_unsubscribe,
|
||||
.event_await = gps_api_event_await,
|
||||
.get_state = gps_api_get_state,
|
||||
.get_model_name = gps_api_get_model_name
|
||||
};
|
||||
|
||||
extern Module gps_generic_module;
|
||||
|
||||
Driver generic_gps_driver = {
|
||||
.name = "gps-generic",
|
||||
.compatible = (const char*[]) { "tactility,gps-generic", nullptr },
|
||||
.start_device = start,
|
||||
.stop_device = stop,
|
||||
.api = &generic_gps_api,
|
||||
.device_type = &GPS_TYPE,
|
||||
.owner = &gps_generic_module
|
||||
};
|
||||
@@ -0,0 +1,288 @@
|
||||
// SPDX-License-Identifier: GPL-3.0-or-later
|
||||
#include <gps_generic/private/cas_messages.h>
|
||||
#include <gps_generic/private/init.h>
|
||||
#include <gps_generic/private/ublox.h>
|
||||
#include <gps_generic/private/gps_response.h>
|
||||
|
||||
#include <tactility/check.h>
|
||||
#include <tactility/delay.h>
|
||||
#include <tactility/device.h>
|
||||
#include <tactility/drivers/uart_controller.h>
|
||||
#include <tactility/log.h>
|
||||
#include <tactility/time.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
constexpr auto* TAG = "gps";
|
||||
|
||||
bool init_mtk(Device* uart);
|
||||
bool init_mtk_l76b(Device* uart);
|
||||
bool init_mtk_pa1616s(Device* uart);
|
||||
bool init_atgm336h(Device* uart);
|
||||
bool init_uc6580(Device* uart);
|
||||
bool init_ag33xx(Device* uart);
|
||||
|
||||
// region CAS
|
||||
|
||||
// Calculate the checksum for a CAS packet
|
||||
static void cas_checksum(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 make_cas_packet(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];
|
||||
}
|
||||
cas_checksum(buffer, (payload_size + 10));
|
||||
|
||||
return (payload_size + 10);
|
||||
}
|
||||
|
||||
static GpsResponse get_ack_cas(Device* uart, uint8_t class_id, uint8_t msg_id, uint32_t wait_millis) {
|
||||
uint32_t start_time = get_millis();
|
||||
uint8_t buffer[CAS_MESSAGE_ACK_NACK_SIZE] = {0};
|
||||
uint8_t buffer_pos = 0;
|
||||
TickType_t wait_ticks = pdMS_TO_TICKS(wait_millis);
|
||||
|
||||
// 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 (get_ticks() - start_time < wait_ticks) {
|
||||
size_t available = 0;
|
||||
uart_controller_get_available(uart, &available);
|
||||
if (available > 0) {
|
||||
uart_controller_read_byte(uart, &buffer[buffer_pos++], 1);
|
||||
|
||||
// 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 ((buffer_pos == 2) && !(buffer[0] == 0xBA && buffer[1] == 0xCE)) {
|
||||
buffer[0] = buffer[1];
|
||||
buffer[1] = 0;
|
||||
buffer_pos = 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 (buffer_pos == 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) {
|
||||
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) {
|
||||
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));
|
||||
buffer_pos = 0;
|
||||
}
|
||||
}
|
||||
return GpsResponse::None;
|
||||
}
|
||||
|
||||
// endregion
|
||||
|
||||
bool gps_init(Device* uart, GpsModel type) {
|
||||
switch (type) {
|
||||
case GPS_MODEL_UNKNOWN:
|
||||
check(false);
|
||||
case GPS_MODEL_AG3335:
|
||||
case GPS_MODEL_AG3352:
|
||||
return init_ag33xx(uart);
|
||||
case GPS_MODEL_ATGM336H:
|
||||
return init_atgm336h(uart);
|
||||
case GPS_MODEL_LS20031:
|
||||
return true;
|
||||
case GPS_MODEL_MTK:
|
||||
return init_mtk(uart);
|
||||
case GPS_MODEL_MTK_L76B:
|
||||
return init_mtk_l76b(uart);
|
||||
case GPS_MODEL_MTK_PA1616S:
|
||||
return init_mtk_pa1616s(uart);
|
||||
case GPS_MODEL_UBLOX6:
|
||||
case GPS_MODEL_UBLOX7:
|
||||
case GPS_MODEL_UBLOX8:
|
||||
case GPS_MODEL_UBLOX9:
|
||||
case GPS_MODEL_UBLOX10:
|
||||
return gps_ublox::init(uart, type);
|
||||
case GPS_MODEL_UC6580:
|
||||
return init_uc6580(uart);
|
||||
}
|
||||
|
||||
LOG_I(TAG, "Init not implemented %d", static_cast<int>(type));
|
||||
return false;
|
||||
}
|
||||
|
||||
bool init_ag33xx(Device* uart) {
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR066,1,0,1,0,0,1*3B\r\n", 25, 250); // Enable GPS+GALILEO+NAVIC
|
||||
|
||||
// Configure NMEA (sentences will output once per fix)
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,0,1*3F\r\n", 17, 250); // GGA ON
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,1,0*3F\r\n", 17, 250); // GLL OFF
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,2,0*3C\r\n", 17, 250); // GSA OFF
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,3,0*3D\r\n", 17, 250); // GSV OFF
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,4,1*3B\r\n", 17, 250); // RMC ON
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,5,0*3B\r\n", 17, 250); // VTG OFF
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR062,6,0*38\r\n", 17, 250); // ZDA ON
|
||||
|
||||
delay_millis(250);
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PAIR513*3D\r\n", 13, 250); // save configuration
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_uc6580(Device* uart) {
|
||||
// The Unicore UC6580 can use a lot of sat systems, enable it to
|
||||
// use GPS L1 & L5 + BDS B1I & B2a + GLONASS L1 + GALILEO E1 & E5a + SBAS + QZSS
|
||||
// This will reset the receiver, so wait a bit afterwards
|
||||
// The paranoid will wait for the OK*04 confirmation response after each command.
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$CFGSYS,h35155\r\n", 16, 250);
|
||||
delay_millis(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_controller_write_bytes(uart, (const uint8_t*)"$CFGMSG,0,3,0\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
// Turn off GSA messages, TinyGPS++ doesn't use this message.
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$CFGMSG,0,2,0\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
// Turn off NOTICE __TXT messages, these may provide Unicore some info but we don't care.
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$CFGMSG,6,0,0\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$CFGMSG,6,1,0\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_atgm336h(Device* uart) {
|
||||
uint8_t buffer[256];
|
||||
|
||||
// Set the intial configuration of the device - these _should_ work for most AT6558 devices
|
||||
int msglen = make_cas_packet(buffer, 0x06, 0x07, sizeof(CAS_MESSAGE_CFG_NAVX_CONF), CAS_MESSAGE_CFG_NAVX_CONF);
|
||||
uart_controller_write_bytes(uart, buffer, msglen, 250);
|
||||
if (get_ack_cas(uart, 0x06, 0x07, 250) != GpsResponse::Ok) {
|
||||
LOG_W(TAG, "ATGM336H: Could not set Config");
|
||||
}
|
||||
|
||||
// Set the update frequence to 1Hz
|
||||
msglen = make_cas_packet(buffer, 0x06, 0x04, sizeof(CAS_MESSAGE_CFG_RATE_1HZ), CAS_MESSAGE_CFG_RATE_1HZ);
|
||||
uart_controller_write_bytes(uart, buffer, msglen, 250);
|
||||
if (get_ack_cas(uart, 0x06, 0x04, 250) != GpsResponse::Ok) {
|
||||
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 = make_cas_packet(buffer, 0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet);
|
||||
uart_controller_write_bytes(uart, buffer, msglen, 250);
|
||||
if (get_ack_cas(uart, 0x06, 0x01, 250) != GpsResponse::Ok) {
|
||||
LOG_W(TAG, "ATGM336H: Could not enable NMEA MSG: %u", fields[i]);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_mtk_pa1616s(Device* uart) {
|
||||
// PA1616S is used in some GPS breakout boards from Adafruit
|
||||
// PA1616S does not have GLONASS capability. PA1616D does, but is not implemented here.
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK353,1,0,0,0,0*2A\r\n", 23, 250);
|
||||
// Above command will reset the GPS and takes longer before it will accept new commands
|
||||
delay_millis(1000);
|
||||
// Only ask for RMC and GGA (GNRMC and GNGGA)
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n", 51, 250);
|
||||
delay_millis(250);
|
||||
// Enable SBAS / WAAS
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK301,2*2E\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_mtk_l76b(Device* uart) {
|
||||
// Waveshare Pico-GPS hat uses the L76B with 9600 baud
|
||||
// Initialize the L76B Chip, use GPS + GLONASS
|
||||
// See note in L76_Series_GNSS_Protocol_Specification, chapter 3.29
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK353,1,1,0,0,0*2B\r\n", 23, 250);
|
||||
// Above command will reset the GPS and takes longer before it will accept new commands
|
||||
delay_millis(1000);
|
||||
// only ask for RMC and GGA (GNRMC and GNGGA)
|
||||
// See note in L76_Series_GNSS_Protocol_Specification, chapter 2.1
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n", 51, 250);
|
||||
delay_millis(250);
|
||||
// Enable SBAS
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK301,2*2E\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
// Enable PPS for 2D/3D fix only
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK285,3,100*3F\r\n", 19, 250);
|
||||
delay_millis(250);
|
||||
// Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PMTK886,1*29\r\n", 15, 250);
|
||||
delay_millis(250);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_mtk(Device* uart) {
|
||||
// Initialize the L76K Chip, use GPS + GLONASS + BEIDOU
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PCAS04,7*1E\r\n", 14, 250);
|
||||
delay_millis(250);
|
||||
// only ask for RMC and GGA
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PCAS03,1,0,0,0,1,0,0,0,0,0,,,0,0*02\r\n", 38, 250);
|
||||
delay_millis(250);
|
||||
// Switch to Vehicle Mode, since SoftRF enables Aviation < 2g
|
||||
uart_controller_write_bytes(uart, (const uint8_t*)"$PCAS11,3*1E\r\n", 14, 250);
|
||||
delay_millis(250);
|
||||
return true;
|
||||
}
|
||||
@@ -0,0 +1,19 @@
|
||||
// SPDX-License-Identifier: GPL-3.0-or-later
|
||||
#include <tactility/driver.h>
|
||||
#include <tactility/module.h>
|
||||
|
||||
extern "C" {
|
||||
|
||||
extern Driver generic_gps_driver;
|
||||
|
||||
static Driver* const gps_generic_drivers[] = {
|
||||
&generic_gps_driver,
|
||||
nullptr
|
||||
};
|
||||
|
||||
Module gps_generic_module = {
|
||||
.name = "gps-generic",
|
||||
.drivers = gps_generic_drivers
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,120 @@
|
||||
// SPDX-License-Identifier: GPL-3.0-or-later
|
||||
#include <gps_generic/private/gps_response.h>
|
||||
#include <gps_generic/private/probe.h>
|
||||
#include <gps_generic/private/ublox.h>
|
||||
|
||||
#include <tactility/delay.h>
|
||||
#include <tactility/device.h>
|
||||
#include <tactility/drivers/uart_controller.h>
|
||||
#include <tactility/log.h>
|
||||
#include <tactility/time.h>
|
||||
|
||||
#include <cstring>
|
||||
|
||||
constexpr auto* TAG = "Gps";
|
||||
|
||||
static char* probe_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);
|
||||
}
|
||||
|
||||
static GpsResponse get_ack(Device* uart, const char* message, uint32_t wait_millis) {
|
||||
uint8_t buffer[768] = {0};
|
||||
uint8_t b;
|
||||
int bytes_read = 0;
|
||||
uint32_t start_timeout = get_millis() + wait_millis;
|
||||
while (get_millis() < start_timeout) {
|
||||
size_t available = 0;
|
||||
uart_controller_get_available(uart, &available);
|
||||
if (available > 0) {
|
||||
uart_controller_read_byte(uart, &b, 1);
|
||||
|
||||
buffer[bytes_read] = b;
|
||||
bytes_read++;
|
||||
if ((bytes_read == 767) || (b == '\r')) {
|
||||
if (probe_strnstr((char*)buffer, message, bytes_read) != nullptr) {
|
||||
return GpsResponse::Ok;
|
||||
} else {
|
||||
bytes_read = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return GpsResponse::None;
|
||||
}
|
||||
|
||||
#define PROBE_SIMPLE(UART, CHIP, TOWRITE, RESPONSE, DRIVER, TIMEOUT, ...) \
|
||||
do { \
|
||||
LOG_I(TAG, "Probing for %s (%s)", CHIP, TOWRITE); \
|
||||
uart_controller_flush_input(UART); \
|
||||
uart_controller_write_bytes(UART, (const uint8_t*)(TOWRITE "\r\n"), strlen(TOWRITE "\r\n"), TIMEOUT); \
|
||||
if (get_ack(UART, RESPONSE, TIMEOUT) == GpsResponse::Ok) { \
|
||||
LOG_I(TAG, "Probe detected %s %s", CHIP, #DRIVER); \
|
||||
return DRIVER; \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
GpsModel gps_probe(Device* uart) {
|
||||
// Close all NMEA sentences
|
||||
// Valid for L76K, ATGM336H and likely other AT6558 devices
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n"), 40, 500);
|
||||
delay_millis(20);
|
||||
|
||||
// Close NMEA sequences on Ublox
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PUBX,40,GLL,0,0,0,0,0,0*5C\r\n"), 29, 500);
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PUBX,40,GSV,0,0,0,0,0,0*59\r\n"), 29, 500);
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PUBX,40,VTG,0,0,0,0,0,0*5E\r\n"), 29, 500);
|
||||
delay_millis(20);
|
||||
|
||||
// Unicore UFirebirdII Series: UC6580, UM620, UM621, UM670A, UM680A, or UM681A
|
||||
PROBE_SIMPLE(uart, "UC6580", "$PDTINFO", "UC6580", GpsModel::GPS_MODEL_UC6580, 500);
|
||||
PROBE_SIMPLE(uart, "UM600", "$PDTINFO", "UM600", GpsModel::GPS_MODEL_UC6580, 500);
|
||||
PROBE_SIMPLE(uart, "ATGM336H", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM336H", GpsModel::GPS_MODEL_ATGM336H, 500);
|
||||
|
||||
// ATGM332D series (-11(GPS), -21(BDS), -31(GPS+BDS), -51(GPS+GLONASS), -71-0(GPS+BDS+GLONASS)) based on AT6558
|
||||
PROBE_SIMPLE(uart, "ATGM332D", "$PCAS06,1*1A", "$GPTXT,01,01,02,HW=ATGM332D", GpsModel::GPS_MODEL_ATGM336H, 500);
|
||||
|
||||
// Airoha (Mediatek) AG3335A/M/S, A3352Q, Quectel L89 2.0, SimCom SIM65M
|
||||
// GSA OFF, reduce volume
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PAIR062,2,0*3C\r\n"), 17, 500);
|
||||
// GSV OFF, reduce volume
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PAIR062,3,0*3D\r\n"), 17, 500);
|
||||
// Save configuration
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PAIR513*3D\r\n"), 13, 500);
|
||||
PROBE_SIMPLE(uart, "AG3335", "$PAIR021*39", "$PAIR021,AG3335", GpsModel::GPS_MODEL_AG3335, 500);
|
||||
PROBE_SIMPLE(uart, "AG3352", "$PAIR021*39", "$PAIR021,AG3352", GpsModel::GPS_MODEL_AG3352, 500);
|
||||
PROBE_SIMPLE(uart, "LC86", "$PQTMVERNO*58", "$PQTMVERNO,LC86", GpsModel::GPS_MODEL_AG3352, 500);
|
||||
|
||||
PROBE_SIMPLE(uart, "L76K", "$PCAS06,0*1B", "$GPTXT,01,01,02,SW=", GpsModel::GPS_MODEL_MTK, 500);
|
||||
|
||||
// Close all NMEA sentences
|
||||
// Valid for L76B MTK
|
||||
uart_controller_write_bytes(uart, reinterpret_cast<const uint8_t*>("$PMTK514,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*2E\r\n"), 51, 500);
|
||||
delay_millis(20);
|
||||
|
||||
PROBE_SIMPLE(uart, "L76B", "$PMTK605*31", "Quectel-L76B", GpsModel::GPS_MODEL_MTK_L76B, 500);
|
||||
PROBE_SIMPLE(uart, "PA1616S", "$PMTK605*31", "1616S", GpsModel::GPS_MODEL_MTK_PA1616S, 500);
|
||||
|
||||
auto ublox_result = gps_ublox::probe(uart);
|
||||
if (ublox_result != GPS_MODEL_UNKNOWN) {
|
||||
return ublox_result;
|
||||
} else {
|
||||
LOG_W(TAG, "No GNSS Module");
|
||||
return GPS_MODEL_UNKNOWN;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,476 @@
|
||||
// SPDX-License-Identifier: GPL-3.0-or-later
|
||||
#include <gps_generic/private/ublox.h>
|
||||
#include <gps_generic/private/gps_response.h>
|
||||
#include <gps_generic/private/ublox_messages.h>
|
||||
|
||||
#include <gps/gps.h>
|
||||
|
||||
#include <tactility/delay.h>
|
||||
#include <tactility/device.h>
|
||||
#include <tactility/drivers/uart_controller.h>
|
||||
#include <tactility/log.h>
|
||||
#include <tactility/time.h>
|
||||
|
||||
#include <cstring>
|
||||
#include <cstdlib>
|
||||
|
||||
namespace gps_ublox {
|
||||
|
||||
constexpr auto* TAG = "Ublox";
|
||||
|
||||
bool init_ublox_6(Device* uart);
|
||||
bool init_ublox_789(Device* uart, GpsModel model);
|
||||
bool init_ublox_10(Device* uart);
|
||||
|
||||
#define SEND_UBX_PACKET(UART, BUFFER, TYPE, ID, DATA, ERRMSG, TIMEOUT_MILLIS) \
|
||||
do { \
|
||||
auto msglen = make_packet(TYPE, ID, DATA, sizeof(DATA), BUFFER); \
|
||||
uart_controller_write_bytes(UART, BUFFER, msglen, TIMEOUT_MILLIS / portTICK_PERIOD_MS); \
|
||||
if (get_ack(UART, TYPE, ID, TIMEOUT_MILLIS) != GpsResponse::Ok) { \
|
||||
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 make_packet(uint8_t class_id, uint8_t message_id, const uint8_t* payload, uint8_t payload_size, uint8_t* buffer_out) {
|
||||
// Construct the UBX packet
|
||||
buffer_out[0] = 0xB5U; // header
|
||||
buffer_out[1] = 0x62U; // header
|
||||
buffer_out[2] = class_id; // class
|
||||
buffer_out[3] = message_id; // id
|
||||
buffer_out[4] = payload_size; // length
|
||||
buffer_out[5] = 0x00U;
|
||||
|
||||
buffer_out[6 + payload_size] = 0x00U; // CK_A
|
||||
buffer_out[7 + payload_size] = 0x00U; // CK_B
|
||||
|
||||
for (int i = 0; i < payload_size; i++) {
|
||||
buffer_out[6 + i] = payload[i];
|
||||
}
|
||||
checksum(buffer_out, (payload_size + 8U));
|
||||
return (payload_size + 8U);
|
||||
}
|
||||
|
||||
GpsResponse get_ack(Device* uart, uint8_t class_id, uint8_t msg_id, uint32_t wait_millis) {
|
||||
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 start_time = get_ticks();
|
||||
TickType_t wait_ticks = pdMS_TO_TICKS(wait_millis);
|
||||
const char frame_errors[] = "More than 100 frame errors";
|
||||
int sCounter = 0;
|
||||
|
||||
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 (get_ticks() - start_time < wait_ticks) {
|
||||
if (ack > 9) {
|
||||
return GpsResponse::Ok; // ACK received
|
||||
}
|
||||
size_t available = 0;
|
||||
uart_controller_get_available(uart, &available);
|
||||
if (available > 0) {
|
||||
uart_controller_read_byte(uart, &b, 1);
|
||||
if (b == frame_errors[sCounter]) {
|
||||
sCounter++;
|
||||
if (sCounter == 26) {
|
||||
return GpsResponse::FrameErrors;
|
||||
}
|
||||
} else {
|
||||
sCounter = 0;
|
||||
}
|
||||
if (b == buf[ack]) {
|
||||
ack++;
|
||||
} else {
|
||||
if (ack == 3 && b == 0x00) { // UBX-ACK-NAK message
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
LOG_W(TAG, "No response for class %02X message %02X", class_id, msg_id);
|
||||
return GpsResponse::None; // No response received within timeout
|
||||
}
|
||||
|
||||
static int get_ack(Device* uart, uint8_t* buffer, uint16_t size, uint8_t requested_class, uint8_t requested_id, uint32_t timeout_millis) {
|
||||
uint16_t ubx_frame_counter = 0;
|
||||
TickType_t start_time = get_ticks();
|
||||
TickType_t timeout_ticks = pdMS_TO_TICKS(timeout_millis);
|
||||
uint16_t need_read = 0;
|
||||
|
||||
while ((get_ticks() - start_time) < timeout_ticks) {
|
||||
size_t available = 0;
|
||||
uart_controller_get_available(uart, &available);
|
||||
while (available > 0) {
|
||||
uint8_t c;
|
||||
uart_controller_read_byte(uart, &c, 1);
|
||||
available--;
|
||||
|
||||
switch (ubx_frame_counter) {
|
||||
case 0:
|
||||
if (c == 0xB5) {
|
||||
ubx_frame_counter++;
|
||||
}
|
||||
break;
|
||||
case 1:
|
||||
if (c == 0x62) {
|
||||
ubx_frame_counter++;
|
||||
} else {
|
||||
ubx_frame_counter = 0;
|
||||
}
|
||||
break;
|
||||
case 2:
|
||||
if (c == requested_class) {
|
||||
ubx_frame_counter++;
|
||||
} else {
|
||||
ubx_frame_counter = 0;
|
||||
}
|
||||
break;
|
||||
case 3:
|
||||
if (c == requested_id) {
|
||||
ubx_frame_counter++;
|
||||
} else {
|
||||
ubx_frame_counter = 0;
|
||||
}
|
||||
break;
|
||||
case 4:
|
||||
need_read = c;
|
||||
ubx_frame_counter++;
|
||||
break;
|
||||
case 5: {
|
||||
// Payload length msb
|
||||
need_read |= (c << 8);
|
||||
ubx_frame_counter++;
|
||||
// Check for buffer overflow
|
||||
if (need_read >= size) {
|
||||
ubx_frame_counter = 0;
|
||||
break;
|
||||
}
|
||||
auto read_bytes = 0U;
|
||||
uart_controller_read_bytes(uart, buffer, need_read, 250 / portTICK_PERIOD_MS);
|
||||
if (read_bytes != need_read) {
|
||||
ubx_frame_counter = 0;
|
||||
} else {
|
||||
// return payload length
|
||||
return need_read;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static struct UbloxGnssModelInfo {
|
||||
char swVersion[30];
|
||||
char hwVersion[10];
|
||||
uint8_t extensionNo;
|
||||
char extension[10][30];
|
||||
uint8_t protocol_version;
|
||||
} ublox_info;
|
||||
|
||||
GpsModel probe(Device* uart) {
|
||||
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_controller_flush_input(uart);
|
||||
uart_controller_write_bytes(uart, cfg_rate, sizeof(cfg_rate), 500 / portTICK_PERIOD_MS);
|
||||
// Check that the returned response class and message ID are correct
|
||||
GpsResponse response = get_ack(uart, 0x06, 0x08, 750);
|
||||
if (response == GpsResponse::None) {
|
||||
LOG_W(TAG, "No GNSS Module");
|
||||
return GpsModel::GPS_MODEL_UNKNOWN;
|
||||
} else if (response == GpsResponse::FrameErrors) {
|
||||
LOG_W(TAG, "UBlox Frame Errors");
|
||||
}
|
||||
|
||||
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_controller_flush_input(uart);
|
||||
uart_controller_write_bytes(uart, message_monver, sizeof(message_monver), 500);
|
||||
|
||||
uint16_t ack_response_len = get_ack(uart, buffer, sizeof(buffer), 0x0A, 0x04, 1200);
|
||||
if (ack_response_len) {
|
||||
uint16_t position = 0;
|
||||
for (char& i: ublox_info.swVersion) {
|
||||
i = buffer[position];
|
||||
position++;
|
||||
}
|
||||
for (char& i: ublox_info.hwVersion) {
|
||||
i = buffer[position];
|
||||
position++;
|
||||
}
|
||||
|
||||
while (ack_response_len >= position + 30) {
|
||||
for (int i = 0; i < 30; i++) {
|
||||
ublox_info.extension[ublox_info.extensionNo][i] = buffer[position];
|
||||
position++;
|
||||
}
|
||||
ublox_info.extensionNo++;
|
||||
if (ublox_info.extensionNo > 9)
|
||||
break;
|
||||
}
|
||||
|
||||
LOG_I(TAG, "Module Info:");
|
||||
LOG_I(TAG, "Soft version: %s", ublox_info.swVersion);
|
||||
LOG_I(TAG, "Hard version: %s", ublox_info.hwVersion);
|
||||
LOG_I(TAG, "Extensions: %u", ublox_info.extensionNo);
|
||||
for (int i = 0; i < ublox_info.extensionNo; i++) {
|
||||
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));
|
||||
LOG_I(TAG, "Protocol Version: %s", (char*)buffer);
|
||||
if (strlen((char*)buffer)) {
|
||||
ublox_info.protocol_version = strtoul((char*)buffer, &ptr, 10);
|
||||
LOG_I(TAG, "ProtVer=%u", ublox_info.protocol_version);
|
||||
} else {
|
||||
ublox_info.protocol_version = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
#define DETECTED_MESSAGE "%s detected, using %s Module"
|
||||
if (strncmp(ublox_info.hwVersion, "00040007", 8) == 0) {
|
||||
LOG_I(TAG, DETECTED_MESSAGE, "U-blox 6", "6");
|
||||
return GPS_MODEL_UBLOX6;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00070000", 8) == 0) {
|
||||
LOG_I(TAG, DETECTED_MESSAGE, "U-blox 7", "7");
|
||||
return GPS_MODEL_UBLOX7;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00080000", 8) == 0) {
|
||||
LOG_I(TAG, DETECTED_MESSAGE, "U-blox 8", "8");
|
||||
return GPS_MODEL_UBLOX8;
|
||||
} else if (strncmp(ublox_info.hwVersion, "00190000", 8) == 0) {
|
||||
LOG_I(TAG, DETECTED_MESSAGE, "U-blox 9", "9");
|
||||
return GPS_MODEL_UBLOX9;
|
||||
} else if (strncmp(ublox_info.hwVersion, "000A0000", 8) == 0) {
|
||||
LOG_I(TAG, DETECTED_MESSAGE, "U-blox 10", "10");
|
||||
return GPS_MODEL_UBLOX10;
|
||||
}
|
||||
}
|
||||
|
||||
return GPS_MODEL_UNKNOWN;
|
||||
}
|
||||
|
||||
bool init(Device* uart, GpsModel model) {
|
||||
LOG_I(TAG, "U-blox init");
|
||||
switch (model) {
|
||||
case GPS_MODEL_UBLOX6:
|
||||
return init_ublox_6(uart);
|
||||
case GPS_MODEL_UBLOX7:
|
||||
case GPS_MODEL_UBLOX8:
|
||||
case GPS_MODEL_UBLOX9:
|
||||
return init_ublox_789(uart, model);
|
||||
case GPS_MODEL_UBLOX10:
|
||||
return init_ublox_10(uart);
|
||||
default:
|
||||
LOG_E(TAG, "Unknown or unsupported U-blox model");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool init_ublox_10(Device* uart) {
|
||||
uint8_t buffer[256];
|
||||
delay_millis(1000);
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_RAM, "disable NMEA messages in M10 RAM", 300);
|
||||
delay_millis(750);
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_NMEA_BBR, "disable NMEA messages in M10 BBR", 300);
|
||||
delay_millis(750);
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_RAM, "disable Info messages for M10 GPS RAM", 300);
|
||||
delay_millis(750);
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_TXT_INFO_BBR, "disable Info messages for M10 GPS BBR", 300);
|
||||
delay_millis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_PM_RAM, "enable powersave for M10 GPS RAM", 300);
|
||||
delay_millis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_PM_BBR, "enable powersave for M10 GPS BBR", 300);
|
||||
delay_millis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ITFM_RAM, "enable jam detection M10 GPS RAM", 300);
|
||||
delay_millis(750);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ITFM_BBR, "enable jam detection M10 GPS BBR", 300);
|
||||
delay_millis(750);
|
||||
// Here is where the init commands should go to do further M10 initialization.
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_SBAS_RAM, "disable SBAS M10 GPS RAM", 300);
|
||||
delay_millis(750); // will cause a receiver restart so wait a bit
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_DISABLE_SBAS_BBR, "disable SBAS M10 GPS BBR", 300);
|
||||
delay_millis(750); // will cause a receiver restart so wait a bit
|
||||
|
||||
// Done with initialization
|
||||
|
||||
// Enable wanted NMEA messages in BBR layer so they will survive a periodic sleep
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ENABLE_NMEA_BBR, "enable messages for M10 GPS BBR", 300);
|
||||
delay_millis(750);
|
||||
// Enable wanted NMEA messages in RAM layer
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x8A, _message_VALSET_ENABLE_NMEA_RAM, "enable messages for M10 GPS RAM", 500);
|
||||
delay_millis(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 = make_packet(0x06, 0x09, _message_SAVE_10, sizeof(_message_SAVE_10), buffer);
|
||||
uart_controller_write_bytes(uart, buffer, packet_size, 2000 / portTICK_PERIOD_MS);
|
||||
if (get_ack(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
LOG_I(TAG, "GNSS module configuration saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_ublox_789(Device* uart, GpsModel model) {
|
||||
uint8_t buffer[256];
|
||||
if (model == GpsModel::GPS_MODEL_UBLOX7) {
|
||||
LOG_D(TAG, "Set GPS+SBAS");
|
||||
auto msglen = make_packet(0x06, 0x3e, _message_GNSS_7, sizeof(_message_GNSS_7), buffer);
|
||||
uart_controller_write_bytes(uart, buffer, msglen, 800 / portTICK_PERIOD_MS);
|
||||
} else { // 8,9
|
||||
auto msglen = make_packet(0x06, 0x3e, _message_GNSS_8, sizeof(_message_GNSS_8), buffer);
|
||||
uart_controller_write_bytes(uart, buffer, msglen, 800 / portTICK_PERIOD_MS);
|
||||
}
|
||||
|
||||
if (get_ack(uart, 0x06, 0x3e, 800) == GpsResponse::NotAck) {
|
||||
// It's not critical if the module doesn't acknowledge this configuration.
|
||||
LOG_D(TAG, "reconfigure GNSS - defaults maintained. Is this module GPS-only?");
|
||||
} else {
|
||||
if (model == GpsModel::GPS_MODEL_UBLOX7) {
|
||||
LOG_I(TAG, "GPS+SBAS configured");
|
||||
} else { // 8,9
|
||||
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
|
||||
delay_millis(1000);
|
||||
}
|
||||
|
||||
uart_controller_flush_input(uart);
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
|
||||
|
||||
if (model == GpsModel::GPS_MODEL_UBLOX8) { // 8
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_8, "enable interference resistance", 500);
|
||||
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5_8, "configure NAVX5_8 settings", 500);
|
||||
} else { // 6,7,9
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
|
||||
}
|
||||
|
||||
// Turn off unwanted NMEA messages, set update rate
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
|
||||
|
||||
if (ublox_info.protocol_version >= 18) {
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x86, _message_PMS, "enable powersave for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
|
||||
// For M8 we want to enable NMEA version 4.10 so we can see the additional satellites.
|
||||
if (model == GpsModel::GPS_MODEL_UBLOX8) {
|
||||
uart_controller_flush_input(uart);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x17, _message_NMEA, "enable NMEA 4.10", 500);
|
||||
}
|
||||
} else {
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x11, _message_CFG_RXM_PSM, "enable powersave mode for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
}
|
||||
|
||||
auto packet_size = make_packet(0x06, 0x09, _message_SAVE, sizeof(_message_SAVE), buffer);
|
||||
uart_controller_write_bytes(uart, buffer, packet_size, 2000 / portTICK_PERIOD_MS);
|
||||
if (get_ack(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
LOG_I(TAG, "GNSS module configuration saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool init_ublox_6(Device* uart) {
|
||||
uint8_t buffer[256];
|
||||
|
||||
uart_controller_flush_input(uart);
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x02, _message_DISABLE_TXT_INFO, "disable text info messages", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x39, _message_JAM_6_7, "enable interference resistance", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x23, _message_NAVX5, "configure NAVX5 settings", 500);
|
||||
|
||||
// Turn off unwanted NMEA messages, set update rate
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x08, _message_1HZ, "set GPS update rate", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GLL, "disable NMEA GLL", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSA, "enable NMEA GSA", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GSV, "disable NMEA GSV", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_VTG, "disable NMEA VTG", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_RMC, "enable NMEA RMC", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_GGA, "enable NMEA GGA", 500);
|
||||
|
||||
uart_controller_flush_input(uart);
|
||||
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x11, _message_CFG_RXM_ECO, "enable powersave ECO mode for Neo-6", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500);
|
||||
SEND_UBX_PACKET(uart, buffer, 0x06, 0x01, _message_AID, "disable UBX-AID", 500);
|
||||
|
||||
auto packet_size = make_packet(0x06, 0x09, _message_SAVE, sizeof(_message_SAVE), buffer);
|
||||
uart_controller_write_bytes(uart, buffer, packet_size, 2000);
|
||||
if (get_ack(uart, 0x06, 0x09, 2000) != GpsResponse::Ok) {
|
||||
LOG_W(TAG, "Unable to save GNSS module config");
|
||||
} else {
|
||||
LOG_I(TAG, "GNSS module config saved!");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user