diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 6c6cdba6de..7665c7e77e 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -128,99 +128,6 @@ static int32_t gpsSwitch() static concurrency::Periodic *gpsPeriodic; #endif -static void UBXChecksum(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; -} - -// Calculate the checksum for a CAS packet -static void CASChecksum(uint8_t *message, size_t length) -{ - uint32_t cksum = ((uint32_t)message[5] << 24); // Message ID - cksum += ((uint32_t)message[4]) << 16; // Class - cksum += message[2]; // Payload Len - - // Iterate over the payload as a series of uint32_t's and - // accumulate the cksum - for (size_t i = 0; i < (length - 10) / 4; i++) { - uint32_t pl = 0; - memcpy(&pl, (message + 6) + (i * sizeof(uint32_t)), sizeof(uint32_t)); // avoid pointer dereference - cksum += pl; - } - - // Place the checksum values in the message - message[length - 4] = (cksum & 0xFF); - message[length - 3] = (cksum & (0xFF << 8)) >> 8; - message[length - 2] = (cksum & (0xFF << 16)) >> 16; - message[length - 1] = (cksum & (0xFF << 24)) >> 24; -} - -// Function to create a 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) -{ - // Construct the UBX packet - UBXscratch[0] = 0xB5; // header - UBXscratch[1] = 0x62; // header - UBXscratch[2] = class_id; // class - UBXscratch[3] = msg_id; // id - UBXscratch[4] = payload_size; // length - UBXscratch[5] = 0x00; - - UBXscratch[6 + payload_size] = 0x00; // CK_A - UBXscratch[7 + payload_size] = 0x00; // CK_B - - for (int i = 0; i < payload_size; i++) { - UBXscratch[6 + i] = pgm_read_byte(&msg[i]); - } - UBXChecksum(UBXscratch, (payload_size + 8)); - 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("CAS packet: "); - 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}; @@ -585,14 +492,14 @@ bool GPS::setup() 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); + msglen = makeCASPacket(UBXscratch, 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 Config"); } // Set the update frequence to 1Hz - msglen = makeCASPacket(0x06, 0x04, sizeof(_message_CAS_CFG_RATE_1HZ), _message_CAS_CFG_RATE_1HZ); + msglen = makeCASPacket(UBXscratch, 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"); @@ -604,7 +511,7 @@ bool GPS::setup() for (unsigned int i = 0; i < sizeof(fields); i++) { // Construct a CAS-CFG-MSG packet uint8_t cas_cfg_msg_packet[] = {0x4e, fields[i], 0x01, 0x00}; - msglen = makeCASPacket(0x06, 0x01, sizeof(cas_cfg_msg_packet), cas_cfg_msg_packet); + msglen = makeCASPacket(UBXscratch, 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", fields[i]); @@ -672,7 +579,7 @@ bool GPS::setup() SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500); SEND_UBX_PACKET(0x06, 0x01, _message_AID, "disable UBX-AID", 500); - msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE); + msglen = makeUBXPacket(UBXscratch, 0x06, 0x09, sizeof(_message_SAVE), _message_SAVE); _serial_gps->write(UBXscratch, msglen); if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to save GNSS module config"); @@ -682,10 +589,10 @@ bool GPS::setup() } else if (IS_ONE_OF(gnssModel, GNSS_MODEL_UBLOX7, GNSS_MODEL_UBLOX8, GNSS_MODEL_UBLOX9)) { if (gnssModel == GNSS_MODEL_UBLOX7) { LOG_DEBUG("Set GPS+SBAS"); - msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_7), _message_GNSS_7); + msglen = makeUBXPacket(UBXscratch, 0x06, 0x3e, sizeof(_message_GNSS_7), _message_GNSS_7); _serial_gps->write(UBXscratch, msglen); } else { // 8,9 - msglen = makeUBXPacket(0x06, 0x3e, sizeof(_message_GNSS_8), _message_GNSS_8); + msglen = makeUBXPacket(UBXscratch, 0x06, 0x3e, sizeof(_message_GNSS_8), _message_GNSS_8); _serial_gps->write(UBXscratch, msglen); } @@ -741,7 +648,7 @@ bool GPS::setup() SEND_UBX_PACKET(0x06, 0x3B, _message_CFG_PM2, "enable powersave details for GPS", 500); } - msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE), _message_SAVE); + msglen = makeUBXPacket(UBXscratch, 0x06, 0x09, sizeof(_message_SAVE), _message_SAVE); _serial_gps->write(UBXscratch, msglen); if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to save GNSS module config"); @@ -789,7 +696,7 @@ bool GPS::setup() // 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. - msglen = makeUBXPacket(0x06, 0x09, sizeof(_message_SAVE_10), _message_SAVE_10); + msglen = makeUBXPacket(UBXscratch, 0x06, 0x09, sizeof(_message_SAVE_10), _message_SAVE_10); _serial_gps->write(UBXscratch, msglen); if (getACK(0x06, 0x09, 2000) != GNSS_RESPONSE_OK) { LOG_WARN("Unable to save GNSS module config"); @@ -975,14 +882,14 @@ void GPS::setPowerUBLOX(bool on, uint32_t sleepMs) _message_PMREQ[0 + i] = sleepMs >> (i * 8); // Record the message length - msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ), _message_PMREQ); + msglen = makeUBXPacket(gps->UBXscratch, 0x02, 0x41, sizeof(_message_PMREQ), _message_PMREQ); } else { // Encode the sleep time in millis into the packet for (int i = 0; i < 4; i++) _message_PMREQ_10[4 + i] = sleepMs >> (i * 8); // Record the message length - msglen = gps->makeUBXPacket(0x02, 0x41, sizeof(_message_PMREQ_10), _message_PMREQ_10); + msglen = makeUBXPacket(gps->UBXscratch, 0x02, 0x41, sizeof(_message_PMREQ_10), _message_PMREQ_10); } // Send the UBX packet @@ -1558,8 +1465,9 @@ GPS *GPS::createGps() gpsPeriodic = new concurrency::Periodic("GPSSwitch", gpsSwitch); #endif -// Currently disabled per issue #525 (TinyGPS++ crash bug) -// when fixed upstream, can be un-disabled to enable 3D FixType and PDOP + // Currently disabled per issue #525 (TinyGPS++ crash bug) + // when fixed upstream, can be un-disabled to enable 3D FixType and PDOP + #ifndef TINYGPS_OPTION_NO_CUSTOM_FIELDS // see NMEAGPS.h gsafixtype.begin(reader, NMEA_MSG_GXGSA, 2); diff --git a/src/gps/GPS.h b/src/gps/GPS.h index 8ba1ce0a60..230da2d4a7 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -198,10 +198,6 @@ class GPS : private concurrency::OSThread static HardwareSerial *_serial_gps; #endif - // 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}; diff --git a/src/gps/cas.cpp b/src/gps/cas.cpp new file mode 100644 index 0000000000..b94025777a --- /dev/null +++ b/src/gps/cas.cpp @@ -0,0 +1,58 @@ +#include "cas.h" + +// Calculate the checksum for a CAS packet +void CASChecksum(uint8_t *message, size_t length) +{ + uint32_t cksum = ((uint32_t)message[5] << 24); // Message ID + cksum += ((uint32_t)message[4]) << 16; // Class + cksum += message[2]; // Payload Len + + // Iterate over the payload as a series of uint32_t's and + // accumulate the cksum + for (size_t i = 0; i < (length - 10) / 4; i++) { + uint32_t pl = 0; + memcpy(&pl, (message + 6) + (i * sizeof(uint32_t)), sizeof(uint32_t)); // avoid pointer dereference + cksum += pl; + } + + // Place the checksum values in the message + message[length - 4] = (cksum & 0xFF); + message[length - 3] = (cksum & (0xFF << 8)) >> 8; + message[length - 2] = (cksum & (0xFF << 16)) >> 16; + message[length - 1] = (cksum & (0xFF << 24)) >> 24; +} + +// Function to create a CAS packet for editing in memory +uint8_t makeCASPacket(uint8_t *out, 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 + out[0] = 0xBA; // header 1 (0xBA) + out[1] = 0xCE; // header 2 (0xCE) + out[2] = payload_size; // length 1 + out[3] = 0; // length 2 + out[4] = class_id; // class + out[5] = msg_id; // id + + out[6 + payload_size] = 0x00; // Checksum + out[7 + payload_size] = 0x00; + out[8 + payload_size] = 0x00; + out[9 + payload_size] = 0x00; + + for (int i = 0; i < payload_size; i++) { + out[6 + i] = pgm_read_byte(&msg[i]); + } + CASChecksum(out, (payload_size + 10)); + +#if defined(GPS_DEBUG) && defined(DEBUG_PORT) + LOG_DEBUG("CAS packet: "); + DEBUG_PORT.hexDump(MESHTASTIC_LOG_LEVEL_DEBUG, out, payload_size + 10); +#endif + return (payload_size + 10); +} diff --git a/src/gps/cas.h b/src/gps/cas.h index 725fd07b3d..76a9de8445 100644 --- a/src/gps/cas.h +++ b/src/gps/cas.h @@ -1,4 +1,5 @@ #pragma once +#include // CASIC binary message definitions // Reference: https://www.icofchina.com/d/file/xiazai/2020-09-22/20f1b42b3a11ac52089caf3603b43fb5.pdf @@ -61,3 +62,9 @@ static const uint8_t _message_CAS_CFG_NAVX_CONF[] = { 0x00, 0x00, 0x00, 0x00, // Time Accuracy Max 0x00, 0x00, 0x00, 0x00 // Static Hold Threshold }; + +// CASIC checksum calculation +void CASChecksum(uint8_t *message, size_t length); + +// Create a CAS packet for editing in memory +uint8_t makeCASPacket(uint8_t *out, uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg); \ No newline at end of file diff --git a/src/gps/ubx.cpp b/src/gps/ubx.cpp new file mode 100644 index 0000000000..3846f27961 --- /dev/null +++ b/src/gps/ubx.cpp @@ -0,0 +1,35 @@ +#include "ubx.h" + +void UBXChecksum(uint8_t *message, size_t length) +{ + uint8_t CK_A = 0, CK_B = 0; + for (size_t i = 2; i < length - 2; i++) { + CK_A = (CK_A + message[i]) & 0xFF; + CK_B = (CK_B + CK_A) & 0xFF; + } + message[length - 2] = CK_A; + message[length - 1] = CK_B; +} + +uint8_t makeUBXPacket(uint8_t *out, uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg) +{ + // UBX header + out[0] = 0xB5; + out[1] = 0x62; + out[2] = class_id; + out[3] = msg_id; + out[4] = payload_size; // length LSB + out[5] = 0x00; // length MSB + + // Payload + for (int i = 0; i < payload_size; i++) { + out[6 + i] = pgm_read_byte(&msg[i]); + } + + // Reserve space for checksum and compute it + out[6 + payload_size] = 0x00; + out[7 + payload_size] = 0x00; + UBXChecksum(out, payload_size + 8); + + return payload_size + 8; +} diff --git a/src/gps/ubx.h b/src/gps/ubx.h index 0fe2f01fb4..ad1fad2d04 100644 --- a/src/gps/ubx.h +++ b/src/gps/ubx.h @@ -1,8 +1,11 @@ +#pragma once +#include + static const char *failMessage = "Unable to %s"; #define SEND_UBX_PACKET(TYPE, ID, DATA, ERRMSG, TIMEOUT) \ do { \ - msglen = makeUBXPacket(TYPE, ID, sizeof(DATA), DATA); \ + msglen = makeUBXPacket(UBXscratch, TYPE, ID, sizeof(DATA), DATA); \ _serial_gps->write(UBXscratch, msglen); \ if (getACK(TYPE, ID, TIMEOUT) != GNSS_RESPONSE_OK) { \ LOG_WARN(failMessage, #ERRMSG); \ @@ -10,7 +13,6 @@ static const char *failMessage = "Unable to %s"; } while (0) // Power Management - static uint8_t _message_PMREQ[] PROGMEM = { 0x00, 0x00, 0x00, 0x00, // 4 bytes duration of request task (milliseconds) 0x02, 0x00, 0x00, 0x00 // Bitfield, set backup = 1 @@ -478,3 +480,6 @@ b5 62 06 8a 0e 00 00 01 00 00 20 00 31 10 00 05 00 31 10 00 46 87 BBR layer config message: b5 62 06 8a 0e 00 00 02 00 00 20 00 31 10 00 05 00 31 10 00 47 94 */ + +void UBXChecksum(uint8_t *message, size_t length); +uint8_t makeUBXPacket(uint8_t *out, uint8_t class_id, uint8_t msg_id, uint8_t payload_size, const uint8_t *msg); \ No newline at end of file