From 55c9c3b29843a3301c5982ad6a46561d80d0fd94 Mon Sep 17 00:00:00 2001 From: David Ellefsen <93522+titan098@users.noreply.github.com> Date: Tue, 16 Apr 2024 16:03:51 +0200 Subject: [PATCH] Support for the ATGM336H series of GPS modules (#3610) Co-authored-by: Ben Meadors --- src/RedirectablePrint.cpp | 8 +- src/gps/GPS.cpp | 163 +++++++++++++++++++++++++++++++++++++- src/gps/GPS.h | 18 ++++- src/gps/cas.h | 63 +++++++++++++++ 4 files changed, 246 insertions(+), 6 deletions(-) create mode 100644 src/gps/cas.h diff --git a/src/RedirectablePrint.cpp b/src/RedirectablePrint.cpp index 16906e2e0..e09e5fe30 100644 --- a/src/RedirectablePrint.cpp +++ b/src/RedirectablePrint.cpp @@ -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, ...) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 6a0e3e44a..0d0bfd9a2 100644 --- a/src/gps/GPS.cpp +++ b/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"); diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 49f27e29f..77c6c0269 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -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 */ diff --git a/src/gps/cas.h b/src/gps/cas.h new file mode 100644 index 000000000..53d75cda9 --- /dev/null +++ b/src/gps/cas.h @@ -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 +}; \ No newline at end of file