From 4ef61f0f1599cde71d7f85c3d29274fa21522810 Mon Sep 17 00:00:00 2001 From: Dmitry Galenko Date: Sun, 2 Jul 2023 02:20:40 +0200 Subject: [PATCH] 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 --- src/gps/GPS.cpp | 167 ++++++++++++++++++++++++++++++++++++++++++++++-- src/gps/GPS.h | 5 +- 2 files changed, 164 insertions(+), 8 deletions(-) diff --git a/src/gps/GPS.cpp b/src/gps/GPS.cpp index 1b7c8511f..13c46d62e 100644 --- a/src/gps/GPS.cpp +++ b/src/gps/GPS.cpp @@ -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 -} +} \ No newline at end of file diff --git a/src/gps/GPS.h b/src/gps/GPS.h index a5f5f2ff4..136632741 100644 --- a/src/gps/GPS.h +++ b/src/gps/GPS.h @@ -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; \ No newline at end of file