GPS: Performance improvment for U-Blox hardware (#2574)

* Add proper configuration procedure for U-Blox modules

* More human friendly getACK

* Fix checksum calculation and payload

* GPS: move unsigned int check

* Introduce UBX protocol payload checksuming

* Fix missed checksums calculation for UBX-CFG-CFG
This commit is contained in:
Dmitry Galenko 2023-07-02 02:20:40 +02:00 committed by GitHub
parent c120549215
commit 4ef61f0f15
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 164 additions and 8 deletions

View File

@ -21,11 +21,26 @@ GPS *gps;
/// only init that port once.
static bool didSerialInit;
bool GPS::getACK(uint8_t c, uint8_t i)
void GPS::UBXChecksum(byte *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;
}
bool GPS::getACK(uint8_t class_id, uint8_t msg_id)
{
uint8_t b;
uint8_t ack = 0;
const uint8_t ackP[2] = {c, i};
const uint8_t ackP[2] = {class_id, msg_id};
uint8_t buf[10] = {0xB5, 0x62, 0x05, 0x01, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00};
unsigned long startTime = millis();
@ -42,17 +57,23 @@ bool GPS::getACK(uint8_t c, uint8_t i)
while (1) {
if (ack > 9) {
return true;
// LOG_INFO("Got ACK for class %02X message %02X\n", class_id, msg_id);
return true; // ACK received
}
if (millis() - startTime > 1000) {
return false;
if (millis() - startTime > 1500) {
LOG_WARN("No response for class %02X message %02X\n", class_id, msg_id);
return false; // No response received within 1.5 second
}
if (_serial_gps->available()) {
b = _serial_gps->read();
if (b == buf[ack]) {
ack++;
} else {
ack = 0;
ack = 0; // Reset the acknowledgement counter
if (buf[3] == 0x00) { // UBX-ACK-NAK message
LOG_WARN("Got NAK for class %02X message %02X\n", class_id, msg_id);
return false; // NAK received
}
}
}
}
@ -192,6 +213,114 @@ bool GPS::setupGPS()
delay(250);
} else if (gnssModel == GNSS_MODEL_UBLOX) {
uint8_t CK_A = 0, CK_B = 0; // checksum bytes
// Configure GNSS system to GPS+SBAS+GLONASS (Module may restart after this command)
// We need set it because by default it is GPS only, and we want to use GLONASS too
// Also we need SBAS for better accuracy and extra features
// ToDo: Dynamic configure GNSS systems depending of LoRa region
byte _message_GNSS[36] = {
0xb5, 0x62, // Sync message for UBX protocol
0x06, 0x3e, // Message class and ID (UBX-CFG-GNSS)
0x1c, 0x00, // Length of payload (28 bytes)
0x00, // msgVer (0 for this version)
0x00, // numTrkChHw (max number of hardware channels, read only, so it's always 0)
0xff, // numTrkChUse (max number of channels to use, 0xff = max available)
0x03, // numConfigBlocks (number of GNSS systems), most modules support maximum 3 GNSS systems
// GNSS config format: gnssId, resTrkCh, maxTrkCh, reserved1, flags
0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, // GPS
0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, // SBAS
0x06, 0x08, 0x0e, 0x00, 0x01, 0x00, 0x01, 0x01, // GLONASS
0x00, 0x00 // Checksum (to be calculated below)
};
// Calculate the checksum and update the message.
UBXChecksum(_message_GNSS, sizeof(_message_GNSS));
// Send the message to the module
_serial_gps->write(_message_GNSS, sizeof(_message_GNSS));
if (!getACK(0x06, 0x3e)) {
LOG_WARN("Unable to reconfigure GNSS, keep factory defaults\n");
} else {
LOG_INFO("GNSS set to GPS+SBAS+GLONASS, waiting before sending next command (0.75s)\n");
delay(750);
}
// Enable interference resistance, because we are using LoRa, WiFi and Bluetoot on same board,
// and we need to reduce interference from them
byte _message_JAM[16] = {
0xB5, 0x62, // UBX protocol sync characters
0x06, 0x39, // Message class and ID (UBX-CFG-ITFM)
0x08, 0x00, // Length of payload (8 bytes)
// bbThreshold (Broadband jamming detection threshold) is set to 0x3F (63 in decimal)
// cwThreshold (CW jamming detection threshold) is set to 0x10 (16 in decimal)
// algorithmBits (Reserved algorithm settings) is set to 0x16B156 as recommended
// enable (Enable interference detection) is set to 1 (enabled)
0x3F, 0x10, 0xB1, 0x56, // config: Interference config word
// generalBits (General settings) is set to 0x31E as recommended
// antSetting (Antenna setting, 0=unknown, 1=passive, 2=active) is set to 0 (unknown)
// ToDo: Set to 1 (passive) or 2 (active) if known, for example from UBX-MON-HW, or from board info
// enable2 (Set to 1 to scan auxiliary bands, u-blox 8 / u-blox M8 only, otherwise ignored) is set to 1 (enabled)
0x1E, 0x03, 0x00, 0x01, // config2: Extra settings for jamming/interference monitor
0x00, 0x00 // Checksum (calculated below)
};
// Calculate the checksum and update the message.
UBXChecksum(_message_JAM, sizeof(_message_JAM));
// Send the message to the module
_serial_gps->write(_message_JAM, sizeof(_message_JAM));
if (!getACK(0x06, 0x39)) {
LOG_WARN("Unable to enable interference resistance.\n");
}
// Configure navigation engine expert settings:
byte _message_NAVX5[48] = {
0xb5, 0x62, // UBX protocol sync characters
0x06, 0x23, // Message class and ID (UBX-CFG-NAVX5)
0x28, 0x00, // Length of payload (40 bytes)
0x00, 0x00, // msgVer (0 for this version)
// minMax flag = 1: apply min/max SVs settings
// minCno flag = 1: apply minimum C/N0 setting
// initial3dfix flag = 0: apply initial 3D fix settings
// aop flag = 1: apply aopCfg (useAOP flag) settings (AssistNow Autonomous)
0x1B, 0x00, // mask1 (First parameters bitmask)
// adr flag = 0: apply ADR sensor fusion on/off setting (useAdr flag)
// If firmware is not ADR/UDR, enabling this flag will fail configuration
// ToDo: check this with UBX-MON-VER
0x00, 0x00, 0x00, 0x00, // mask2 (Second parameters bitmask)
0x00, 0x00, // Reserved
0x03, // minSVs (Minimum number of satellites for navigation) = 3
0x10, // maxSVs (Maximum number of satellites for navigation) = 16
0x06, // minCNO (Minimum satellite signal level for navigation) = 6 dBHz
0x00, // Reserved
0x00, // iniFix3D (Initial fix must be 3D) = 0 (disabled)
0x00, 0x00, // Reserved
0x00, // ackAiding (Issue acknowledgements for assistance message input) = 0 (disabled)
0x00, 0x00, // Reserved
0x00, 0x00, 0x00, 0x00, 0x00, 0x00, // Reserved
0x00, // Reserved
0x01, // aopCfg (AssistNow Autonomous configuration) = 1 (enabled)
0x00, 0x00, // Reserved
0x00, 0x00, // Reserved
0x00, 0x00, 0x00, 0x00, // Reserved
0x00, 0x00, 0x00, // Reserved
0x01, // useAdr (Enable/disable ADR sensor fusion) = 1 (enabled)
0x00, 0x00 // Checksum (calculated below)
};
// Calculate the checksum and update the message.
UBXChecksum(_message_NAVX5, sizeof(_message_NAVX5));
// Send the message to the module
_serial_gps->write(_message_NAVX5, sizeof(_message_NAVX5));
if (!getACK(0x06, 0x23)) {
LOG_WARN("Unable to configure extra settings.\n");
}
/*
tips: NMEA Only should not be set here, otherwise initializing Ublox gnss module again after
setting will not output command messages in UART1, resulting in unrecognized module information
@ -260,6 +389,30 @@ bool GPS::setupGPS()
if (!getACK(0x06, 0x01)) {
LOG_WARN("Unable to enable NMEA GGA.\n");
}
// We need save configuration to flash to make our config changes persistent
byte _message_SAVE[21] = {
0xB5, 0x62, // UBX protocol header
0x06, 0x09, // UBX class ID (Configuration Input Messages), message ID (UBX-CFG-CFG)
0x0D, 0x00, // Length of payload (13 bytes)
0x00, 0x00, 0x00, 0x00, // clearMask: no sections cleared
0xFF, 0xFF, 0x00, 0x00, // saveMask: save all sections
0x00, 0x00, 0x00, 0x00, // loadMask: no sections loaded
0x0F, // deviceMask: BBR, Flash, EEPROM, and SPI Flash
0x00, 0x00 // Checksum (calculated below)
};
// Calculate the checksum and update the message.
UBXChecksum(_message_SAVE, sizeof(_message_SAVE));
// Send the message to the module
_serial_gps->write(_message_SAVE, sizeof(_message_SAVE));
if (!getACK(0x06, 0x09)) {
LOG_WARN("Unable to save GNSS module configuration.\n");
} else {
LOG_INFO("GNSS module configuration saved!\n");
}
}
}
@ -688,4 +841,4 @@ GPS *createGps()
}
return nullptr;
#endif
}
}

View File

@ -139,6 +139,9 @@ class GPS : private concurrency::OSThread
/// always returns 0 to indicate okay to sleep
int prepareDeepSleep(void *unused);
// Calculate checksum
void UBXChecksum(byte *message, size_t length);
/**
* Switch the GPS into a mode where we are actively looking for a lock, or alternatively switch GPS into a low power mode
*
@ -179,4 +182,4 @@ class GPS : private concurrency::OSThread
// Returns the new instance or null if the GPS is not present.
GPS *createGps();
extern GPS *gps;
extern GPS *gps;