Skip to content

GPS.cpp refactor - move Ublox and CASIC to separate files. #8516

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wants to merge 1 commit into
base: develop
Choose a base branch
from
Draft

GPS.cpp refactor - move Ublox and CASIC to separate files. #8516

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
118 changes: 13 additions & 105 deletions src/gps/GPS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down Expand Up @@ -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");
Expand All @@ -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]);
Expand Down Expand Up @@ -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");
Expand All @@ -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);
}

Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down
4 changes: 0 additions & 4 deletions src/gps/GPS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};

Expand Down
58 changes: 58 additions & 0 deletions src/gps/cas.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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);
}
7 changes: 7 additions & 0 deletions src/gps/cas.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#pragma once
#include <Arduino.h>

// CASIC binary message definitions
// Reference: https://www.icofchina.com/d/file/xiazai/2020-09-22/20f1b42b3a11ac52089caf3603b43fb5.pdf
Expand Down Expand Up @@ -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);
35 changes: 35 additions & 0 deletions src/gps/ubx.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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;
}
9 changes: 7 additions & 2 deletions src/gps/ubx.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,16 +1,18 @@
#pragma once
#include <Arduino.h>

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); \
} \
} 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
Expand Down Expand Up @@ -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);
Loading