kopia lustrzana https://github.com/meshtastic/firmware
Support for the ATGM336H series of GPS modules (#3610)
Co-authored-by: Ben Meadors <benmmeadors@gmail.com>pull/3645/head
David Ellefsen
GitHub
rodzic
9599549477
commit
55c9c3b298
|
|
@ -182,11 +182,11 @@ size_t RedirectablePrint::log(const char *logLevel, const char *format, ...)
|
|||
void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16_t len)
|
||||
{
|
||||
const char alphabet[17] = "0123456789abcdef";
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
log(logLevel, " |.0 .1 .2 .3 .4 .5 .6 .7 .8 .9 .a .b .c .d .e .f | | ASCII |\n");
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
log(logLevel, " |.0 .1 .2 .3 .4 .5 .6 .7 .8 .9 .a .b .c .d .e .f | | ASCII |\n");
|
||||
for (uint16_t i = 0; i < len; i += 16) {
|
||||
if (i % 128 == 0)
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
char s[] = "| | | |\n";
|
||||
uint8_t ix = 1, iy = 52;
|
||||
for (uint8_t j = 0; j < 16; j++) {
|
||||
|
|
@ -208,7 +208,7 @@ void RedirectablePrint::hexDump(const char *logLevel, unsigned char *buf, uint16
|
|||
log(logLevel, ".");
|
||||
log(logLevel, s);
|
||||
}
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
log(logLevel, " +------------------------------------------------+ +----------------+\n");
|
||||
}
|
||||
|
||||
std::string RedirectablePrint::mt_sprintf(const std::string fmt_str, ...)
|
||||
|
|
|
|||
163
src/gps/GPS.cpp
163
src/gps/GPS.cpp
|
|
@ -7,6 +7,8 @@
|
|||
|
||||
#include "main.h" // pmu_found
|
||||
#include "sleep.h"
|
||||
|
||||
#include "cas.h"
|
||||
#include "ubx.h"
|
||||
|
||||
#ifdef ARCH_PORTDUINO
|
||||
|
|
@ -51,6 +53,28 @@ void GPS::UBXChecksum(uint8_t *message, size_t length)
|
|||
message[length - 1] = CK_B;
|
||||
}
|
||||
|
||||
// Calculate the checksum for a CAS packet
|
||||
void GPS::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
|
||||
uint32_t *payload = (uint32_t *)(message + 6);
|
||||
for (size_t i = 0; i < (length - 10) / 4; i++) {
|
||||
uint32_t p = payload[i];
|
||||
cksum += p;
|
||||
}
|
||||
|
||||
// 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 ublox packet for editing in memory
|
||||
uint8_t GPS::makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg)
|
||||
{
|
||||
|
|
@ -72,6 +96,41 @@ uint8_t GPS::makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_siz
|
|||
return (payload_size + 8);
|
||||
}
|
||||
|
||||
// Function to create a CAS packet for editing in memory
|
||||
uint8_t GPS::makeCASPacket(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
|
||||
UBXscratch[0] = 0xBA; // header 1 (0xBA)
|
||||
UBXscratch[1] = 0xCE; // header 2 (0xCE)
|
||||
UBXscratch[2] = payload_size; // length 1
|
||||
UBXscratch[3] = 0; // length 2
|
||||
UBXscratch[4] = class_id; // class
|
||||
UBXscratch[5] = msg_id; // id
|
||||
|
||||
UBXscratch[6 + payload_size] = 0x00; // Checksum
|
||||
UBXscratch[7 + payload_size] = 0x00;
|
||||
UBXscratch[8 + payload_size] = 0x00;
|
||||
UBXscratch[9 + payload_size] = 0x00;
|
||||
|
||||
for (int i = 0; i < payload_size; i++) {
|
||||
UBXscratch[6 + i] = pgm_read_byte(&msg[i]);
|
||||
}
|
||||
CASChecksum(UBXscratch, (payload_size + 10));
|
||||
|
||||
#if defined(GPS_DEBUG) && defined(DEBUG_PORT)
|
||||
LOG_DEBUG("Constructed CAS packet: \n");
|
||||
DEBUG_PORT.hexDump(MESHTASTIC_LOG_LEVEL_DEBUG, UBXscratch, payload_size + 10);
|
||||
#endif
|
||||
return (payload_size + 10);
|
||||
}
|
||||
|
||||
GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
|
||||
{
|
||||
uint8_t buffer[768] = {0};
|
||||
|
|
@ -81,6 +140,7 @@ GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
|
|||
while (millis() < startTimeout) {
|
||||
if (_serial_gps->available()) {
|
||||
b = _serial_gps->read();
|
||||
|
||||
#ifdef GPS_DEBUG
|
||||
LOG_DEBUG("%02X", (char *)buffer);
|
||||
#endif
|
||||
|
|
@ -104,6 +164,67 @@ GPS_RESPONSE GPS::getACK(const char *message, uint32_t waitMillis)
|
|||
return GNSS_RESPONSE_NONE;
|
||||
}
|
||||
|
||||
GPS_RESPONSE GPS::getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
|
||||
{
|
||||
uint32_t startTime = millis();
|
||||
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 (millis() - startTime < waitMillis) {
|
||||
if (_serial_gps->available()) {
|
||||
buffer[bufferPos++] = _serial_gps->read();
|
||||
|
||||
// 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 %d millis.\n", class_id, msg_id, millis() - startTime);
|
||||
#endif
|
||||
return GNSS_RESPONSE_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 %d millis.\n", class_id, msg_id, millis() - startTime);
|
||||
#endif
|
||||
return GNSS_RESPONSE_NAK;
|
||||
}
|
||||
|
||||
// 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 GNSS_RESPONSE_NONE;
|
||||
}
|
||||
|
||||
GPS_RESPONSE GPS::getACK(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis)
|
||||
{
|
||||
uint8_t b;
|
||||
|
|
@ -313,6 +434,33 @@ bool GPS::setup()
|
|||
// Switch to Fitness Mode, for running and walking purpose with low speed (<5 m/s)
|
||||
_serial_gps->write("$PMTK886,1*29\r\n");
|
||||
delay(250);
|
||||
} else if (gnssModel == GNSS_MODEL_ATGM336H) {
|
||||
// Set the intial configuration of the device - these _should_ work for most AT6558 devices
|
||||
msglen = makeCASPacket(0x06, 0x07, sizeof(_message_CAS_CFG_NAVX_CONF), _message_CAS_CFG_NAVX_CONF);
|
||||
_serial_gps->write(UBXscratch, msglen);
|
||||
if (getACKCas(0x06, 0x07, 250) != GNSS_RESPONSE_OK) {
|
||||
LOG_WARN("ATGM336H - Could not set Configuration");
|
||||
}
|
||||
|
||||
// Set the update frequence to 1Hz
|
||||
msglen = makeCASPacket(0x06, 0x04, sizeof(_message_CAS_CFG_RATE_1HZ), _message_CAS_CFG_RATE_1HZ);
|
||||
_serial_gps->write(UBXscratch, msglen);
|
||||
if (getACKCas(0x06, 0x04, 250) != GNSS_RESPONSE_OK) {
|
||||
LOG_WARN("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 (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(0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet);
|
||||
_serial_gps->write(UBXscratch, msglen);
|
||||
if (getACKCas(0x06, 0x01, 250) != GNSS_RESPONSE_OK) {
|
||||
LOG_WARN("ATGM336H - Could not enable NMEA MSG: %d\n", fields[i]);
|
||||
}
|
||||
}
|
||||
} else if (gnssModel == GNSS_MODEL_UC6580) {
|
||||
// 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
|
||||
|
|
@ -948,10 +1096,18 @@ GnssModel_t GPS::probe(int serialSpeed)
|
|||
uint8_t buffer[768] = {0};
|
||||
delay(100);
|
||||
|
||||
// Close all NMEA sentences , Only valid for L76K MTK platform
|
||||
// Close all NMEA sentences, valid for L76K, ATGM336H (and likely other AT6558 devices)
|
||||
_serial_gps->write("$PCAS03,0,0,0,0,0,0,0,0,0,0,,,0,0*02\r\n");
|
||||
delay(20);
|
||||
|
||||
// Get version information
|
||||
clearBuffer();
|
||||
_serial_gps->write("$PCAS06,1*1A\r\n");
|
||||
if (getACK("$GPTXT,01,01,02,HW=ATGM336H", 500) == GNSS_RESPONSE_OK) {
|
||||
LOG_INFO("ATGM336H GNSS init succeeded, using ATGM336H Module\n");
|
||||
return GNSS_MODEL_ATGM336H;
|
||||
}
|
||||
|
||||
// Get version information
|
||||
clearBuffer();
|
||||
_serial_gps->write("$PCAS06,0*1B\r\n");
|
||||
|
|
@ -1216,6 +1372,11 @@ bool GPS::factoryReset()
|
|||
LOG_INFO("GNSS Factory Reset via PCAS10,3\n");
|
||||
_serial_gps->write("$PCAS10,3*1F\r\n");
|
||||
delay(100);
|
||||
} else if (gnssModel == GNSS_MODEL_ATGM336H) {
|
||||
LOG_INFO("Factory Reset via CAS-CFG-RST\n");
|
||||
uint8_t msglen = makeCASPacket(0x06, 0x02, sizeof(_message_CAS_CFG_RST_FACTORY), _message_CAS_CFG_RST_FACTORY);
|
||||
_serial_gps->write(UBXscratch, msglen);
|
||||
delay(100);
|
||||
} else {
|
||||
// fire this for good measure, if we have an L76B - won't harm other devices.
|
||||
_serial_gps->write("$PMTK104*37\r\n");
|
||||
|
|
|
|||
|
|
@ -22,7 +22,14 @@ struct uBloxGnssModelInfo {
|
|||
char extension[10][30];
|
||||
};
|
||||
|
||||
typedef enum { GNSS_MODEL_MTK, GNSS_MODEL_UBLOX, GNSS_MODEL_UC6580, GNSS_MODEL_UNKNOWN, GNSS_MODEL_MTK_L76B } GnssModel_t;
|
||||
typedef enum {
|
||||
GNSS_MODEL_ATGM336H,
|
||||
GNSS_MODEL_MTK,
|
||||
GNSS_MODEL_UBLOX,
|
||||
GNSS_MODEL_UC6580,
|
||||
GNSS_MODEL_UNKNOWN,
|
||||
GNSS_MODEL_MTK_L76B
|
||||
} GnssModel_t;
|
||||
|
||||
typedef enum {
|
||||
GNSS_RESPONSE_NONE,
|
||||
|
|
@ -133,6 +140,11 @@ class GPS : private concurrency::OSThread
|
|||
static const uint8_t _message_VALSET_DISABLE_SBAS_RAM[];
|
||||
static const uint8_t _message_VALSET_DISABLE_SBAS_BBR[];
|
||||
|
||||
// CASIC commands for ATGM336H
|
||||
static const uint8_t _message_CAS_CFG_RST_FACTORY[];
|
||||
static const uint8_t _message_CAS_CFG_NAVX_CONF[];
|
||||
static const uint8_t _message_CAS_CFG_RATE_1HZ[];
|
||||
|
||||
meshtastic_Position p = meshtastic_Position_init_default;
|
||||
|
||||
GPS() : concurrency::OSThread("GPS") {}
|
||||
|
|
@ -174,6 +186,7 @@ class GPS : private concurrency::OSThread
|
|||
|
||||
// Create a ublox packet for editing in memory
|
||||
uint8_t makeUBXPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
|
||||
uint8_t makeCASPacket(uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg);
|
||||
|
||||
// scratch space for creating ublox packets
|
||||
uint8_t UBXscratch[250] = {0};
|
||||
|
|
@ -184,6 +197,8 @@ class GPS : private concurrency::OSThread
|
|||
GPS_RESPONSE getACK(uint8_t c, uint8_t i, uint32_t waitMillis);
|
||||
GPS_RESPONSE getACK(const char *message, uint32_t waitMillis);
|
||||
|
||||
GPS_RESPONSE getACKCas(uint8_t class_id, uint8_t msg_id, uint32_t waitMillis);
|
||||
|
||||
/**
|
||||
* Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode
|
||||
*
|
||||
|
|
@ -243,6 +258,7 @@ class GPS : private concurrency::OSThread
|
|||
|
||||
// Calculate checksum
|
||||
void UBXChecksum(uint8_t *message, size_t length);
|
||||
void CASChecksum(uint8_t *message, size_t length);
|
||||
|
||||
/** Get how long we should stay looking for each aquisition
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -0,0 +1,63 @@
|
|||
#pragma once
|
||||
|
||||
// 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
|
||||
const uint8_t GPS::_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
|
||||
const uint8_t GPS::_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.
|
||||
const uint8_t GPS::_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
|
||||
};
|
||||
Ładowanie…
Reference in New Issue