Skip to content

Commit

Permalink
Support for the ATGM336H series of GPS modules (#3610)
Browse files Browse the repository at this point in the history 
Co-authored-by: Ben Meadors <benmmeadors@gmail.com>
  • Loading branch information
@titan098 @thebentern
titan098 and thebentern authored Apr 16, 2024
1 parent 9599549 commit 55c9c3b
Show file tree
Hide file tree
Showing 4 changed files with 246 additions and 6 deletions.
8 changes: 4 additions & 4 deletions src/RedirectablePrint.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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++) {
Expand All @@ -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, ...)
Expand Down
163 changes: 162 additions & 1 deletion src/gps/GPS.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@

#include "main.h" // pmu_found
#include "sleep.h"

#include "cas.h"
#include "ubx.h"

#ifdef ARCH_PORTDUINO
Expand Down Expand Up @@ -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)
{
Expand All @@ -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};
Expand All @@ -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
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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");
Expand Down
18 changes: 17 additions & 1 deletion src/gps/GPS.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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") {}
Expand Down Expand Up @@ -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};
Expand All @@ -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
*
Expand Down Expand Up @@ -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
*/
Expand Down
63 changes: 63 additions & 0 deletions src/gps/cas.h
View file
Original file line number Diff line number Diff line change
@@ -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
};

0 comments on commit 55c9c3b

Please sign in to comment.