diff --git a/docs/Settings.md b/docs/Settings.md index 9ad22c177f5..bb7e91d4a48 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1975,7 +1975,7 @@ Which SBAS mode to be used ### gps_ublox_nav_hz -Navigation update rate for UBLOX receivers. M9 modules limit satellite tracking to 16 satellites at 10Hz or higher, but use 32 satellites below 10Hz for better accuracy. M10 modules work well at 8Hz with 3 constellations. Some M10 devices with high-performance clock can do up to 25Hz with 4 constellations. 8Hz is a safe, accurate default for M8/M9/M10. +Navigation update rate for UBLOX receivers. M9 modules limit satellite tracking to 16 satellites at 10Hz or higher, but use 32 satellites below 10Hz for better accuracy. M10 modules work well at 8Hz with 3 constellations. Some M10 devices with high-performance clock can do up to 25Hz with 4 constellations. ZED-F9P modules reach 7Hz with 4 constellations on firmware HPG 1.51 (9Hz on HPG 1.32), ZED-X20P modules up to 25Hz. 8Hz is a safe, accurate default for M8/M9/M10. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b8d0d28e9e9..3787cd129b7 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1847,7 +1847,7 @@ groups: min: 5 max: 10 - name: gps_ublox_nav_hz - description: "Navigation update rate for UBLOX receivers. M9 modules limit satellite tracking to 16 satellites at 10Hz or higher, but use 32 satellites below 10Hz for better accuracy. M10 modules work well at 8Hz with 3 constellations. Some M10 devices with high-performance clock can do up to 25Hz with 4 constellations. 8Hz is a safe, accurate default for M8/M9/M10." + description: "Navigation update rate for UBLOX receivers. M9 modules limit satellite tracking to 16 satellites at 10Hz or higher, but use 32 satellites below 10Hz for better accuracy. M10 modules work well at 8Hz with 3 constellations. Some M10 devices with high-performance clock can do up to 25Hz with 4 constellations. ZED-F9P modules reach 7Hz with 4 constellations on firmware HPG 1.51 (9Hz on HPG 1.32), ZED-X20P modules up to 25Hz. 8Hz is a safe, accurate default for M8/M9/M10." default_value: 8 field: ubloxNavHz type: uint8_t diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 3d2afe754b4..91fed3508ca 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -193,8 +193,12 @@ const char *getGpsHwVersion(void) return "UBLOX8"; case UBX_HW_VERSION_UBLOX9: return "UBLOX9"; + case UBX_HW_VERSION_UBLOX_F9: + return "UBLOXF9"; case UBX_HW_VERSION_UBLOX10: return "UBLOX10"; + case UBX_HW_VERSION_UBLOX20: + return "UBLOX20"; default: return "Unknown"; } diff --git a/src/main/io/gps_ublox.c b/src/main/io/gps_ublox.c index 703242d10dd..4fb2a7b6026 100755 --- a/src/main/io/gps_ublox.c +++ b/src/main/io/gps_ublox.c @@ -32,7 +32,6 @@ #include "common/axis.h" -#include "common/typeconversion.h" #include "common/gps_conversion.h" #include "common/maths.h" #include "common/utils.h" @@ -120,6 +119,7 @@ static struct { int capMaxGnss; uint8_t defaultGnss; uint8_t enabledGnss; + bool monGnssUnsupported; // MON-GNSS came back in a version the parser does not read } ubx_capabilities = { }; // Example packet sizes from UBlox u-center from a Glonass capable GPS receiver. @@ -475,6 +475,16 @@ static void configureGNSS10(void) ubloxSendSetCfgBytes(gnssConfigValues, 12); } +static void configureGNSSEnables(void) +{ + ubx_config_data8_payload_t gnssConfigValues[UBLOX_GNSS_ENABLE_KEYS_MAX]; + const uint8_t count = ubloxGnssEnableKeys(gnssConfigValues, gpsState.gpsConfig->sbasMode != SBAS_NONE, + gpsState.gpsConfig->ubloxUseGalileo, gpsState.gpsConfig->ubloxUseBeidou, gpsState.gpsConfig->ubloxUseGlonass, + ubx_capabilities.supported); + + ubloxSendSetCfgBytes(gnssConfigValues, count); +} + static void configureGNSS(void) { int blocksUsed = 0; @@ -530,12 +540,6 @@ static void configureMSG(uint8_t msg_class, uint8_t id, uint8_t rate) */ static void configureRATE(uint16_t measRate) { - if(ubloxVersionLT(24, 0)) { - measRate = MAX(50, measRate); - } else { - measRate = MAX(25, measRate); - } - if (ubloxVersionLTE(23, 1)) { send_buffer.message.header.msg_class = CLASS_CFG; send_buffer.message.header.msg_id = MSG_CFG_RATE; @@ -564,6 +568,11 @@ static void configureRATE(uint16_t measRate) } } +static uint8_t ubloxEffectiveNavHz(void) +{ + return ubloxNavHzFor(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor, gpsState.gpsConfig->ubloxNavHz); +} + /* */ static void configureSBAS(void) @@ -579,53 +588,6 @@ static void configureSBAS(void) sendConfigMessageUBLOX(); } -static void gpsDecodeProtocolVersion(const char *proto, size_t bufferLength) -{ - if (bufferLength > 13 && (!strncmp(proto, "PROTVER=", 8) || !strncmp(proto, "PROTVER ", 8))) { - proto+=8; - - float ver = fastA2F(proto); - - gpsState.swVersionMajor = (uint8_t)ver; - gpsState.swVersionMinor = (uint8_t)((ver - gpsState.swVersionMajor) * 100.0f); - } -} - -static uint8_t gpsDecodeHardwareVersion(const char * szBuf, unsigned nBufSize) -{ - // ublox_5 hwVersion 00040005 - if (strncmp(szBuf, "00040005", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX5; - } - - // ublox_6 hwVersion 00040007 - if (strncmp(szBuf, "00040007", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX6; - } - - // ublox_7 hwVersion 00070000 - if (strncmp(szBuf, "00070000", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX7; - } - - // ublox_M8 hwVersion 00080000 - if (strncmp(szBuf, "00080000", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX8; - } - - // ublox_M9 hwVersion 00190000 - if (strncmp(szBuf, "00190000", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX9; - } - - // ublox_M10 hwVersion 000A0000 - if (strncmp(szBuf, "000A0000", nBufSize) == 0) { - return UBX_HW_VERSION_UBLOX10; - } - - return UBX_HW_VERSION_UNKNOWN; -} - static bool gpsParseFrameUBLOX(void) { switch (_msg_id) { @@ -732,9 +694,23 @@ static bool gpsParseFrameUBLOX(void) _new_speed = true; break; case MSG_VER: - if (_class == CLASS_MON) { - gpsState.hwVersion = gpsDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { + if (_class == CLASS_MON && _payload_length >= sizeof(ubx_mon_ver)) { + gpsState.hwVersion = ubloxDecodeHardwareVersion(_buffer.ver.hwVersion, sizeof(_buffer.ver.hwVersion)); + // Parsed before the gates below so receivers missing from the hardware ID table are not treated as legacy + const char *module = NULL; + for (unsigned j = sizeof(ubx_mon_ver); j + 30 <= _payload_length; j += 30) { + const char *extension = (const char *)(_buffer.bytes + j); + uint8_t major, minor; + if (ubloxParseProtocolVersion(extension, 30, &major, &minor)) { + gpsState.swVersionMajor = major; + gpsState.swVersionMinor = minor; + } + if (strncmp(extension, "MOD=", 4) == 0) { + module = extension; + } + } + gpsState.hwVersion = ubloxRefineHardwareVersion(gpsState.hwVersion, _buffer.ver.swVersion, sizeof(_buffer.ver.swVersion), module, 30); + if (ubloxCanConfigureGnss(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { if (_buffer.ver.swVersion[9] > '2' || true) { // check extensions; // after hw + sw vers; each is 30 bytes @@ -759,12 +735,6 @@ static bool gpsParseFrameUBLOX(void) } } } - for(int j = 40; j < _payload_length; j += 30) { - if (strnstr((const char *)(_buffer.bytes + j), "PROTVER", 30)) { - gpsDecodeProtocolVersion((const char *)(_buffer.bytes + j), 30); - break; - } - } } } break; @@ -776,6 +746,8 @@ static bool gpsParseFrameUBLOX(void) ubx_capabilities.enabledGnss = _buffer.gnss.enabled; ubx_capabilities.capMaxGnss = _buffer.gnss.maxConcurrent; gpsState.lastCapaUpdMs = millis(); + } else { + ubx_capabilities.monGnssUnsupported = true; } } break; @@ -1125,10 +1097,8 @@ STATIC_PROTOTHREAD(gpsConfigure) }// end message config ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_SHORT_TIMEOUT); - if ((gpsState.hwVersion >= UBX_HW_VERSION_UBLOX7)) { - configureRATE(hz2rate(gpsState.gpsConfig->ubloxNavHz)); // default 10Hz - } else { - configureRATE(hz2rate(5)); // 5Hz + configureRATE(hz2rate(ubloxEffectiveNavHz())); + if (!ubloxCanConfigureNavRate(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { gpsConfigMutable()->ubloxNavHz = SETTING_GPS_UBLOX_NAV_HZ_DEFAULT; } ptWait(_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK); @@ -1151,14 +1121,12 @@ STATIC_PROTOTHREAD(gpsConfigure) ptWaitTimeout((_ack_state == UBX_ACK_GOT_ACK || _ack_state == UBX_ACK_GOT_NAK), GPS_CFG_CMD_TIMEOUT_MS); // Configure GNSS for M8N and later - if (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX8) { // TODO: This check can be remove in INAV 9.0.0 + if (ubloxCanConfigureGnss(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); - bool use_VALSET = 0; - if (ubloxVersionGT(23,1)) { - use_VALSET = 1; - } - if ( use_VALSET && (gpsState.hwVersion >= UBX_HW_VERSION_UBLOX10) ) { + if (ubloxUseGnssEnableKeys(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { + configureGNSSEnables(); + } else if (ubloxUseM10GnssKeys(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { configureGNSS10(); } else { configureGNSS(); @@ -1250,26 +1218,41 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) // Attempt to detect GPS hw version gpsState.hwVersion = UBX_HW_VERSION_UNKNOWN; + gpsState.swVersionMajor = 0; + gpsState.swVersionMinor = 0; gpsState.autoConfigStep = 0; // Configure GPS module if enabled if (gpsState.gpsConfig->autoConfig) { + // Before MON-VER, whose extensions set the constellation bits + ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; + ubx_capabilities.monGnssUnsupported = false; do { + // gps.c counts its timeout from this call, not from received frames, so the retries would be cut short + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); pollVersion(); gpsState.autoConfigStep++; - ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN), GPS_CFG_CMD_TIMEOUT_MS); - } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN); + ptWaitTimeout((gpsState.hwVersion != UBX_HW_VERSION_UNKNOWN || gpsState.swVersionMajor != 0), GPS_CFG_CMD_TIMEOUT_MS); + } while(gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && + gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN && gpsState.swVersionMajor == 0); + + // Without a version the receiver would be set up on the legacy path, so restart like a communication loss + if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN && gpsState.swVersionMajor == 0) { + ptStop(0); + } gpsState.autoConfigStep = 0; - ubx_capabilities.supported = ubx_capabilities.enabledGnss = ubx_capabilities.defaultGnss = 0; // M7 and earlier will never get pass this step, so skip it (#9440). // UBLOX documents that this is M8N and later - if (gpsState.hwVersion > UBX_HW_VERSION_UBLOX7) { + if (ubloxCanConfigureGnss(gpsState.hwVersion, gpsState.swVersionMajor, gpsState.swVersionMinor)) { do { + // MON-VER was answered, so stay alive even when MON-GNSS (version 1 on the X20) is not parsed + gpsSetProtocolTimeout(GPS_SHORT_TIMEOUT); pollGnssCapabilities(); gpsState.autoConfigStep++; - ptWaitTimeout((ubx_capabilities.capMaxGnss != 0), GPS_CFG_CMD_TIMEOUT_MS); - } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0); + ptWaitTimeout((ubx_capabilities.capMaxGnss != 0 || ubx_capabilities.monGnssUnsupported), GPS_CFG_CMD_TIMEOUT_MS); + } while (gpsState.autoConfigStep < GPS_VERSION_RETRY_TIMES && ubx_capabilities.capMaxGnss == 0 && + !ubx_capabilities.monGnssUnsupported); } // Configure GPS @@ -1290,17 +1273,19 @@ STATIC_PROTOTHREAD(gpsProtocolStateThread) if ((millis() - gpsState.lastCapaPoolMs) > GPS_CAPA_INTERVAL) { gpsState.lastCapaPoolMs = millis(); gnssPolled=true; - if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN) + if (gpsState.hwVersion == UBX_HW_VERSION_UNKNOWN && gpsState.swVersionMajor == 0) { pollVersion(); } - pollGnssCapabilities(); + if (!ubx_capabilities.monGnssUnsupported) { + pollGnssCapabilities(); + } } } /* Periodically poll MON-RF (~1s) for HW > UBLOX8 if OSD widget requested. Do not change ACK state. */ - if ((!gnssPolled) && gpsState.hwVersion > UBX_HW_VERSION_UBLOX8 && osdMonRfWidgetEnabled) { + if ((!gnssPolled) && UBX_HW_GENERATION(gpsState.hwVersion) > UBX_HW_GENERATION(UBX_HW_VERSION_UBLOX8) && osdMonRfWidgetEnabled) { if ((millis() - lastMonRfMs) > 1000) { lastMonRfMs = millis(); pollMonRf(); diff --git a/src/main/io/gps_ublox.h b/src/main/io/gps_ublox.h index 75f10901035..b7237005206 100644 --- a/src/main/io/gps_ublox.h +++ b/src/main/io/gps_ublox.h @@ -64,15 +64,19 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); /* * hwVersion encoding (fits in uint8_t): - * bits [7:6] series: 0b00=unknown, 0b01=u-blox Neo/M series - * bits [5:0] generation within series (e.g. 8=M8, 9=M9, 10=M10) + * bits [7:6] series: 0b00=unknown, 0b01=u-blox Neo/M series, 0b10=u-blox F9 + * bits [5:0] generation within series (e.g. 8=M8, 9=M9/F9, 10=M10, 20=X20) * - * This leaves 0b10 and 0b11 available for future series (e.g. u-blox F9, - * other manufacturers). + * The ZED-X20P reports 000B0000 (after the M10's 000A0000); the ZED-F9P reports the M9's 00190000, + * so it is told apart by the MON-VER module name. + * + * This leaves 0b11 available for future series (e.g. other manufacturers). */ #define UBX_HW_SERIES_MASK 0xC0 #define UBX_HW_GEN_MASK 0x3F #define UBX_HW_SERIES_UBLOX_NM 0x40 // 0b01 << 6: u-blox Neo/M series +#define UBX_HW_SERIES_UBLOX_F9 0x80 // 0b10 << 6: u-blox F9, which reports the M9's hardware ID +#define UBX_HW_GENERATION(hw) ((hw) & UBX_HW_GEN_MASK) #define UBX_HW_VERSION_UNKNOWN 0 #define UBX_HW_VERSION_UBLOX5 (UBX_HW_SERIES_UBLOX_NM | 5) // 0x45 @@ -81,6 +85,8 @@ STATIC_ASSERT(MAX_UBLOX_PAYLOAD_SIZE >= 256, ubx_size_too_small); #define UBX_HW_VERSION_UBLOX8 (UBX_HW_SERIES_UBLOX_NM | 8) // 0x48 #define UBX_HW_VERSION_UBLOX9 (UBX_HW_SERIES_UBLOX_NM | 9) // 0x49 #define UBX_HW_VERSION_UBLOX10 (UBX_HW_SERIES_UBLOX_NM | 10) // 0x4A +#define UBX_HW_VERSION_UBLOX20 (UBX_HW_SERIES_UBLOX_NM | 20) // 0x54, X20 +#define UBX_HW_VERSION_UBLOX_F9 (UBX_HW_SERIES_UBLOX_F9 | 9) // 0x89 #define UBLOX_CFG_MSGOUT_NAV_POSLLH_UART1 0x2091002a // U1 #define UBLOX_CFG_MSGOUT_NAV_SAT_UART1 0x20910016 // U1 diff --git a/src/main/io/gps_ublox_utils.c b/src/main/io/gps_ublox_utils.c index c9c1682a5f7..d1e1ded14ce 100644 --- a/src/main/io/gps_ublox_utils.c +++ b/src/main/io/gps_ublox_utils.c @@ -16,11 +16,22 @@ */ +#include +#include #include #include #include "gps_ublox_utils.h" +#define UBLOX_PROTVER(major, minor) ((uint16_t)((major) * 100 + (minor))) + +#define UBLOX_PROTVER_NAV_RATE UBLOX_PROTVER(15, 0) +#define UBLOX_PROTVER_GNSS_CONFIG UBLOX_PROTVER(15, 0) +#define UBLOX_PROTVER_LAST_BEFORE_VALSET UBLOX_PROTVER(23, 1) +#define UBLOX_PROTVER_FAST_MEAS_RATE UBLOX_PROTVER(24, 0) +// Below this the documented firmware lacks the BDS_B1C signal key, and M9/F9P still take CFG-GNSS +#define UBLOX_PROTVER_VALSET_ONLY UBLOX_PROTVER(34, 0) + void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *ck_b) { *ck_a = *ck_b = 0; @@ -113,4 +124,148 @@ void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info * // bit7: carrier correction used // bit8: doper corrections used //uint8_t reserved[4]; -} \ No newline at end of file +} + +uint8_t ubloxDecodeHardwareVersion(const char *field, size_t len) +{ + // ublox_5 hwVersion 00040005 + if (strncmp(field, "00040005", len) == 0) { + return UBX_HW_VERSION_UBLOX5; + } + + // ublox_6 hwVersion 00040007 + if (strncmp(field, "00040007", len) == 0) { + return UBX_HW_VERSION_UBLOX6; + } + + // ublox_7 hwVersion 00070000 + if (strncmp(field, "00070000", len) == 0) { + return UBX_HW_VERSION_UBLOX7; + } + + // ublox_M8 hwVersion 00080000 + if (strncmp(field, "00080000", len) == 0) { + return UBX_HW_VERSION_UBLOX8; + } + + // ublox_M9 hwVersion 00190000 + if (strncmp(field, "00190000", len) == 0) { + return UBX_HW_VERSION_UBLOX9; + } + + // ublox_M10 hwVersion 000A0000 + if (strncmp(field, "000A0000", len) == 0) { + return UBX_HW_VERSION_UBLOX10; + } + + // ublox_X20 hwVersion 000B0000 + if (strncmp(field, "000B0000", len) == 0) { + return UBX_HW_VERSION_UBLOX20; + } + + return UBX_HW_VERSION_UNKNOWN; +} + +bool ubloxParseProtocolVersion(const char *field, size_t len, uint8_t *major, uint8_t *minor) +{ + // Parsed digit by digit because a float round-trip turns 34.10 into 34.09 + if (len < 14 || (strncmp(field, "PROTVER=", 8) && strncmp(field, "PROTVER ", 8))) { + return false; + } + if (!isdigit((unsigned char)field[8]) || !isdigit((unsigned char)field[9]) || field[10] != '.' || + !isdigit((unsigned char)field[11]) || !isdigit((unsigned char)field[12]) || field[13] != '\0') { + return false; + } + + *major = (field[8] - '0') * 10 + field[9] - '0'; + *minor = (field[11] - '0') * 10 + field[12] - '0'; + return true; +} + +// "MOD=" names the module and wins over the F9's "EXT CORE 1." base +static bool ubloxIsF9(const char *swVersion, size_t swLen, const char *module, size_t moduleLen) +{ + if (module && moduleLen >= 4 && strncmp(module, "MOD=", 4) == 0) { + for (size_t i = 4; i + 1 < moduleLen && module[i] != '\0'; i++) { + if (module[i] == 'F' && module[i + 1] == '9') { + return true; + } + } + return false; + } + + return swVersion && swLen >= 11 && strncmp(swVersion, "EXT CORE 1.", 11) == 0; +} + +// The F9 reports the M9's hardware ID +uint8_t ubloxRefineHardwareVersion(uint8_t hwVersion, const char *swVersion, size_t swLen, const char *module, size_t moduleLen) +{ + if (hwVersion == UBX_HW_VERSION_UBLOX9 && ubloxIsF9(swVersion, swLen, module, moduleLen)) { + return UBX_HW_VERSION_UBLOX_F9; + } + return hwVersion; +} + +bool ubloxCanConfigureNavRate(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor) +{ + return UBX_HW_GENERATION(hwVersion) >= UBX_HW_GENERATION(UBX_HW_VERSION_UBLOX7) || + UBLOX_PROTVER(protMajor, protMinor) >= UBLOX_PROTVER_NAV_RATE; +} + +bool ubloxCanConfigureGnss(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor) +{ + return UBX_HW_GENERATION(hwVersion) >= UBX_HW_GENERATION(UBX_HW_VERSION_UBLOX8) || + UBLOX_PROTVER(protMajor, protMinor) >= UBLOX_PROTVER_GNSS_CONFIG; +} + +bool ubloxUseM10GnssKeys(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor) +{ + const uint16_t protocol = UBLOX_PROTVER(protMajor, protMinor); + // Unknown hardware stays on CFG-GNSS wherever its firmware still accepts it + return protocol > UBLOX_PROTVER_LAST_BEFORE_VALSET && + (UBX_HW_GENERATION(hwVersion) >= UBX_HW_GENERATION(UBX_HW_VERSION_UBLOX10) || + (hwVersion == UBX_HW_VERSION_UNKNOWN && protocol >= UBLOX_PROTVER_VALSET_ONLY)); +} + +// F9 NAKs single-band signal masks and the X20 plan lacks BDS B1I, so these only switch whole constellations +bool ubloxUseGnssEnableKeys(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor) +{ + return UBLOX_PROTVER(protMajor, protMinor) > UBLOX_PROTVER_LAST_BEFORE_VALSET && + (hwVersion == UBX_HW_VERSION_UBLOX_F9 || UBX_HW_GENERATION(hwVersion) >= UBX_HW_GENERATION(UBX_HW_VERSION_UBLOX20)); +} + +static uint8_t ubloxAddKey(ubx_config_data8_payload_t *out, uint8_t count, uint32_t key, bool value) +{ + out[count].key = key; + out[count].value = value ? 1 : 0; + return count + 1; +} + +uint8_t ubloxGnssEnableKeys(ubx_config_data8_payload_t *out, bool sbas, bool galileo, bool beidou, bool glonass, uint8_t supportedMask) +{ + uint8_t count = ubloxAddKey(out, 0, UBLOX_CFG_SIGNAL_SBAS_ENA, sbas); + + if (supportedMask & UBX_MON_GNSS_GALILEO_MASK) { + count = ubloxAddKey(out, count, UBLOX_CFG_SIGNAL_GAL_ENA, galileo); + } + if (supportedMask & UBX_MON_GNSS_BEIDOU_MASK) { + count = ubloxAddKey(out, count, UBLOX_CFG_SIGNAL_BDS_ENA, beidou); + } + // Should be enabled with GPS + count = ubloxAddKey(out, count, UBLOX_CFG_QZSS_ENA, true); + if (supportedMask & UBX_MON_GNSS_GLONASS_MASK) { + count = ubloxAddKey(out, count, UBLOX_CFG_GLO_ENA, glonass); + } + + return count; +} + +uint8_t ubloxNavHzFor(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor, uint8_t configuredHz) +{ + if (!ubloxCanConfigureNavRate(hwVersion, protMajor, protMinor)) { + return 5; + } + + const uint16_t minMeasPeriodMs = UBLOX_PROTVER(protMajor, protMinor) >= UBLOX_PROTVER_FAST_MEAS_RATE ? 25 : 50; + return MIN(configuredHz, 1000 / minMeasPeriodMs); +} diff --git a/src/main/io/gps_ublox_utils.h b/src/main/io/gps_ublox_utils.h index 996cbe2a68c..47ac46e12ea 100644 --- a/src/main/io/gps_ublox_utils.h +++ b/src/main/io/gps_ublox_utils.h @@ -17,6 +17,8 @@ #pragma once +#include +#include #include #include "gps_ublox.h" @@ -32,6 +34,19 @@ void ublox_update_checksum(uint8_t *data, uint8_t len, uint8_t *ck_a, uint8_t *c void ubloxNavSat2NavSig(const ubx_nav_svinfo_channel *navSat, ubx_nav_sig_info *navSig); +#define UBLOX_GNSS_ENABLE_KEYS_MAX 5 + +uint8_t ubloxDecodeHardwareVersion(const char *field, size_t len); +bool ubloxParseProtocolVersion(const char *field, size_t len, uint8_t *major, uint8_t *minor); +uint8_t ubloxRefineHardwareVersion(uint8_t hwVersion, const char *swVersion, size_t swLen, const char *module, size_t moduleLen); + +bool ubloxCanConfigureNavRate(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor); +bool ubloxCanConfigureGnss(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor); +bool ubloxUseM10GnssKeys(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor); +bool ubloxUseGnssEnableKeys(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor); +uint8_t ubloxGnssEnableKeys(ubx_config_data8_payload_t *out, bool sbas, bool galileo, bool beidou, bool glonass, uint8_t supportedMask); +uint8_t ubloxNavHzFor(uint8_t hwVersion, uint8_t protMajor, uint8_t protMinor, uint8_t configuredHz); + #ifdef __cplusplus } #endif diff --git a/src/test/unit/gps_ublox_unittest.cc b/src/test/unit/gps_ublox_unittest.cc index ea5164b32a0..9e2a6d6f8c7 100644 --- a/src/test/unit/gps_ublox_unittest.cc +++ b/src/test/unit/gps_ublox_unittest.cc @@ -19,6 +19,8 @@ #include "gtest/gtest.h" #include "unittest_macros.h" +#include +#include #include #include #include @@ -97,4 +99,279 @@ TEST(GPSUbloxTest, navSigStructureSizes) { EXPECT_TRUE(sizeof(ubx_nav_svinfo_channel) == 12); EXPECT_TRUE(sizeof(ubx_nav_svinfo) == (8 + (12 * UBLOX_MAX_SIGNALS))); -} \ No newline at end of file +} + +TEST(GPSUbloxTest, DecodeHardwareVersion) +{ + const struct { + const char *hwVersion; + uint8_t expected; + } cases[] = { + { "00040005", UBX_HW_VERSION_UBLOX5 }, + { "00040007", UBX_HW_VERSION_UBLOX6 }, + { "00070000", UBX_HW_VERSION_UBLOX7 }, + { "00080000", UBX_HW_VERSION_UBLOX8 }, + { "00190000", UBX_HW_VERSION_UBLOX9 }, // also reported by the ZED-F9P + { "000A0000", UBX_HW_VERSION_UBLOX10 }, + { "000B0000", UBX_HW_VERSION_UBLOX20 }, + { "000C0000", UBX_HW_VERSION_UNKNOWN }, + { "000a0000", UBX_HW_VERSION_UNKNOWN }, + { "0008000", UBX_HW_VERSION_UNKNOWN }, + { "00080000XY", UBX_HW_VERSION_UNKNOWN }, + { "", UBX_HW_VERSION_UNKNOWN }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(c.hwVersion); + ubx_mon_ver ver = {}; + memcpy(ver.hwVersion, c.hwVersion, std::min(strlen(c.hwVersion), sizeof(ver.hwVersion))); + EXPECT_EQ(c.expected, ubloxDecodeHardwareVersion(ver.hwVersion, sizeof(ver.hwVersion))); + } +} + +TEST(GPSUbloxTest, ParseProtocolVersion) +{ + const size_t extensionLength = 30; + const struct { + const char *extension; + size_t length; + bool valid; + uint8_t major; + uint8_t minor; + } cases[] = { + { "PROTVER=18.00", extensionLength, true, 18, 0 }, + { "PROTVER 14.00", extensionLength, true, 14, 0 }, + { "PROTVER=34.10", extensionLength, true, 34, 10 }, + { "PROTVER=27.31", extensionLength, true, 27, 31 }, + { "PROTVER=27.50", extensionLength, true, 27, 50 }, + { "PROTVER=50.11", extensionLength, true, 50, 11 }, + { "PROTVER=18.00", 14, true, 18, 0 }, + { "PROTVER=18.00", 13, false, 0, 0 }, + { "PROTVER=18.00ABCDEFGHIJKLMNOPQ", extensionLength, false, 0, 0 }, + { "PROTVER=18.000", extensionLength, false, 0, 0 }, + { "PROTVER=18.", extensionLength, false, 0, 0 }, + { "PROTVER=18", extensionLength, false, 0, 0 }, + { "PROTVER=18.0", extensionLength, false, 0, 0 }, + { "PROTVER=8.00", extensionLength, false, 0, 0 }, + { "PROTVER=50.xx", extensionLength, false, 0, 0 }, + { "PROTVER:18.00", extensionLength, false, 0, 0 }, + { "protver=18.00", extensionLength, false, 0, 0 }, + { "FWVER=HPG 2.10", extensionLength, false, 0, 0 }, + { "", extensionLength, false, 0, 0 }, + { "", 0, false, 0, 0 }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(testing::Message() << "'" << c.extension << "' length " << c.length); + char extension[extensionLength] = {}; + memcpy(extension, c.extension, std::min(strlen(c.extension), sizeof(extension))); + uint8_t major = 0; + uint8_t minor = 0; + EXPECT_EQ(c.valid, ubloxParseProtocolVersion(extension, c.length, &major, &minor)); + if (c.valid) { + EXPECT_EQ(c.major, major); + EXPECT_EQ(c.minor, minor); + } + } +} + +TEST(GPSUbloxTest, RefineHardwareVersion) +{ + const size_t fieldLength = 30; + const uint8_t M9 = UBX_HW_VERSION_UBLOX9; + const uint8_t F9 = UBX_HW_VERSION_UBLOX_F9; + const struct { + const char *receiver; + uint8_t hwVersion; + const char *swVersion; + size_t swLength; + const char *module; // NULL: MON-VER without a "MOD=" extension + size_t moduleLength; + uint8_t expected; + } cases[] = { + { "ZED-F9P HPG 1.51", M9, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", fieldLength, F9 }, + { "ZED-F9P HPG 1.32", M9, "EXT CORE 1.00 (0fa0ae)", fieldLength, "MOD=ZED-F9P", fieldLength, F9 }, + { "ZED-F9P without MOD", M9, "EXT CORE 1.00 (9e1716)", fieldLength, NULL, 0, F9 }, + { "ZED-F9R", M9, "", fieldLength, "MOD=ZED-F9R", fieldLength, F9 }, + { "NEO-F9P", M9, "", fieldLength, "MOD=NEO-F9P", fieldLength, F9 }, + { "NEO-M9N", M9, "EXT CORE 4.04 (7f89f7)", fieldLength, "MOD=NEO-M9N", fieldLength, M9 }, + { "M9 without MOD", M9, "EXT CORE 4.04 (7f89f7)", fieldLength, NULL, 0, M9 }, + { "MOD overrides F9 core", M9, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=NEO-M9N", fieldLength, M9 }, + { "MOD overrides M9 core", M9, "EXT CORE 4.04 (7f89f7)", fieldLength, "MOD=ZED-F9P", fieldLength, F9 }, + { "empty MOD", M9, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=", fieldLength, M9 }, + { "F without 9", M9, "", fieldLength, "MOD=F", fieldLength, M9 }, + { "MOD cut after F9", M9, "", fieldLength, "MOD=ZED-F9P", 10, F9 }, + { "MOD cut inside F9", M9, "", fieldLength, "MOD=ZED-F9P", 9, M9 }, + { "MOD cut before its prefix", M9, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", 3, F9 }, + { "not a MOD field", M9, "EXT CORE 4.04 (7f89f7)", fieldLength, "FWVER=F9", fieldLength, M9 }, + { "core cut before the minor", M9, "EXT CORE 1.00 (9e1716)", 10, NULL, 0, M9 }, + { "core cut after the dot", M9, "EXT CORE 1.00 (9e1716)", 11, NULL, 0, F9 }, + { "core 10.x", M9, "EXT CORE 10.00", fieldLength, NULL, 0, M9 }, + { "M9 without strings", M9, "", fieldLength, NULL, 0, M9 }, + { "M8 with F9 strings", UBX_HW_VERSION_UBLOX8, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", fieldLength, UBX_HW_VERSION_UBLOX8 }, + { "M10 with F9 strings", UBX_HW_VERSION_UBLOX10, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", fieldLength, UBX_HW_VERSION_UBLOX10 }, + { "ZED-X20P", UBX_HW_VERSION_UBLOX20, "EXT HPG 2.10 (b0eda3)", fieldLength, "MOD=ZED-X20P", fieldLength, UBX_HW_VERSION_UBLOX20 }, + { "X20 with F9 strings", UBX_HW_VERSION_UBLOX20, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", fieldLength, UBX_HW_VERSION_UBLOX20 }, + { "unknown with F9 strings", UBX_HW_VERSION_UNKNOWN, "EXT CORE 1.00 (9e1716)", fieldLength, "MOD=ZED-F9P", fieldLength, UBX_HW_VERSION_UNKNOWN }, + { "F9 stays F9", F9, "EXT CORE 4.04 (7f89f7)", fieldLength, "MOD=NEO-M9N", fieldLength, F9 }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(c.receiver); + char swVersion[fieldLength] = {}; + char module[fieldLength] = {}; + memcpy(swVersion, c.swVersion, std::min(strlen(c.swVersion), sizeof(swVersion))); + if (c.module) { + memcpy(module, c.module, std::min(strlen(c.module), sizeof(module))); + } + EXPECT_EQ(c.expected, ubloxRefineHardwareVersion(c.hwVersion, swVersion, c.swLength, c.module ? module : NULL, c.moduleLength)); + } + + // Text after a NUL does not belong to the field + const char afterNul[fieldLength] = { 'M', 'O', 'D', '=', 'Z', '\0', 'F', '9' }; + EXPECT_EQ(M9, ubloxRefineHardwareVersion(M9, "EXT CORE 1.00 (9e1716)", fieldLength, afterNul, fieldLength)); + + // An unterminated field is read up to its length and not beyond + char unterminated[fieldLength]; + memset(unterminated, 'X', sizeof(unterminated)); + memcpy(unterminated, "MOD=", 4); + unterminated[fieldLength - 2] = 'F'; + unterminated[fieldLength - 1] = '9'; + EXPECT_EQ(F9, ubloxRefineHardwareVersion(M9, NULL, 0, unterminated, fieldLength)); + EXPECT_EQ(M9, ubloxRefineHardwareVersion(M9, NULL, 0, unterminated, fieldLength - 1)); + EXPECT_EQ(M9, ubloxRefineHardwareVersion(M9, NULL, 0, NULL, 0)); +} + +TEST(GPSUbloxTest, CapabilitiesFollowHardwareOrProtocol) +{ + const struct { + const char *receiver; + uint8_t hwVersion; + uint8_t protMajor; + uint8_t protMinor; + bool canConfigureGnss; + bool useM10GnssKeys; + bool useEnableKeys; + uint8_t navHz; + } cases[] = { + { "M5", UBX_HW_VERSION_UBLOX5, 0, 0, false, false, false, 5 }, + { "M6", UBX_HW_VERSION_UBLOX6, 0, 0, false, false, false, 5 }, + { "M7", UBX_HW_VERSION_UBLOX7, 0, 0, false, false, false, 10 }, + { "M7 14.00", UBX_HW_VERSION_UBLOX7, 14, 0, false, false, false, 10 }, + { "M8", UBX_HW_VERSION_UBLOX8, 0, 0, true, false, false, 10 }, + { "M8 18.00", UBX_HW_VERSION_UBLOX8, 18, 0, true, false, false, 10 }, + { "M9 32.01", UBX_HW_VERSION_UBLOX9, 32, 1, true, false, false, 10 }, + { "F9P not refined 27.50", UBX_HW_VERSION_UBLOX9, 27, 50, true, false, false, 10 }, + { "F9 27.31", UBX_HW_VERSION_UBLOX_F9, 27, 31, true, false, true, 10 }, + { "F9 27.50", UBX_HW_VERSION_UBLOX_F9, 27, 50, true, false, true, 10 }, + { "F9 0.00", UBX_HW_VERSION_UBLOX_F9, 0, 0, true, false, false, 10 }, + { "M10", UBX_HW_VERSION_UBLOX10, 0, 0, true, false, false, 10 }, + { "M10 34.10", UBX_HW_VERSION_UBLOX10, 34, 10, true, true, false, 10 }, + { "X20 50.11", UBX_HW_VERSION_UBLOX20, 50, 11, true, true, true, 10 }, + { "unknown 50.11", UBX_HW_VERSION_UNKNOWN, 50, 11, true, true, false, 10 }, + { "unknown 50.10", UBX_HW_VERSION_UNKNOWN, 50, 10, true, true, false, 10 }, + { "unknown 34.00", UBX_HW_VERSION_UNKNOWN, 34, 0, true, true, false, 10 }, + { "unknown 33.99", UBX_HW_VERSION_UNKNOWN, 33, 99, true, false, false, 10 }, + { "unknown 24.00", UBX_HW_VERSION_UNKNOWN, 24, 0, true, false, false, 10 }, + { "unknown 23.01", UBX_HW_VERSION_UNKNOWN, 23, 1, true, false, false, 10 }, + { "unknown 15.00", UBX_HW_VERSION_UNKNOWN, 15, 0, true, false, false, 10 }, + { "unknown 14.99", UBX_HW_VERSION_UNKNOWN, 14, 99, false, false, false, 5 }, + { "unknown 0.00", UBX_HW_VERSION_UNKNOWN, 0, 0, false, false, false, 5 }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(c.receiver); + EXPECT_EQ(c.canConfigureGnss, ubloxCanConfigureGnss(c.hwVersion, c.protMajor, c.protMinor)); + EXPECT_EQ(c.useM10GnssKeys, ubloxUseM10GnssKeys(c.hwVersion, c.protMajor, c.protMinor)); + EXPECT_EQ(c.useEnableKeys, ubloxUseGnssEnableKeys(c.hwVersion, c.protMajor, c.protMinor)); + EXPECT_EQ(c.navHz, ubloxNavHzFor(c.hwVersion, c.protMajor, c.protMinor, 10)); + EXPECT_EQ(c.navHz == 10, ubloxCanConfigureNavRate(c.hwVersion, c.protMajor, c.protMinor)); + } +} + +TEST(GPSUbloxTest, NavHzFollowsMinimumMeasurementPeriod) +{ + const struct { + const char *receiver; + uint8_t hwVersion; + uint8_t protMajor; + uint8_t protMinor; + uint8_t configuredHz; + uint8_t navHz; + } cases[] = { + { "M5 50 Hz", UBX_HW_VERSION_UBLOX5, 0, 0, 50, 5 }, + { "M8 18.00 10 Hz", UBX_HW_VERSION_UBLOX8, 18, 0, 10, 10 }, + { "M8 18.00 20 Hz", UBX_HW_VERSION_UBLOX8, 18, 0, 20, 20 }, + { "M8 18.00 21 Hz", UBX_HW_VERSION_UBLOX8, 18, 0, 21, 20 }, + { "M8 18.00 50 Hz", UBX_HW_VERSION_UBLOX8, 18, 0, 50, 20 }, + { "unknown 23.01 25 Hz", UBX_HW_VERSION_UNKNOWN, 23, 1, 25, 20 }, + { "unknown 24.00 25 Hz", UBX_HW_VERSION_UNKNOWN, 24, 0, 25, 25 }, + { "M10 34.10 10 Hz", UBX_HW_VERSION_UBLOX10, 34, 10, 10, 10 }, + { "M10 34.10 40 Hz", UBX_HW_VERSION_UBLOX10, 34, 10, 40, 40 }, + { "M10 34.10 50 Hz", UBX_HW_VERSION_UBLOX10, 34, 10, 50, 40 }, + { "X20 50.11 200 Hz", UBX_HW_VERSION_UBLOX20, 50, 11, 200, 40 }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(c.receiver); + EXPECT_EQ(c.navHz, ubloxNavHzFor(c.hwVersion, c.protMajor, c.protMinor, c.configuredHz)); + } +} + +TEST(GPSUbloxTest, GnssEnableKeysSwitchOnlyWholeConstellations) +{ + const uint8_t all = UBX_MON_GNSS_GPS_MASK | UBX_MON_GNSS_GLONASS_MASK | UBX_MON_GNSS_BEIDOU_MASK | UBX_MON_GNSS_GALILEO_MASK; + const uint8_t withoutGlonass = all & ~UBX_MON_GNSS_GLONASS_MASK; + const struct { + const char *name; + bool sbas; + bool galileo; + bool beidou; + bool glonass; + uint8_t supported; + uint8_t count; + ubx_config_data8_payload_t keys[UBLOX_GNSS_ENABLE_KEYS_MAX]; + } cases[] = { + { "GPS, Galileo and BeiDou", true, true, true, false, all, 5, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 1 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 1 }, { UBLOX_CFG_SIGNAL_BDS_ENA, 1 }, + { UBLOX_CFG_QZSS_ENA, 1 }, { UBLOX_CFG_GLO_ENA, 0 } } }, + { "all constellations", true, true, true, true, all, 5, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 1 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 1 }, { UBLOX_CFG_SIGNAL_BDS_ENA, 1 }, + { UBLOX_CFG_QZSS_ENA, 1 }, { UBLOX_CFG_GLO_ENA, 1 } } }, + { "GPS only without SBAS", false, false, false, false, all, 5, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 0 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 0 }, { UBLOX_CFG_SIGNAL_BDS_ENA, 0 }, + { UBLOX_CFG_QZSS_ENA, 1 }, { UBLOX_CFG_GLO_ENA, 0 } } }, + { "GLONASS without SBAS", false, false, false, true, all, 5, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 0 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 0 }, { UBLOX_CFG_SIGNAL_BDS_ENA, 0 }, + { UBLOX_CFG_QZSS_ENA, 1 }, { UBLOX_CFG_GLO_ENA, 1 } } }, + { "X20 HPG 2.00 without GLONASS", true, true, true, true, withoutGlonass, 4, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 1 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 1 }, { UBLOX_CFG_SIGNAL_BDS_ENA, 1 }, + { UBLOX_CFG_QZSS_ENA, 1 } } }, + { "Galileo only supported", true, true, true, true, UBX_MON_GNSS_GPS_MASK | UBX_MON_GNSS_GALILEO_MASK, 3, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 1 }, { UBLOX_CFG_SIGNAL_GAL_ENA, 1 }, { UBLOX_CFG_QZSS_ENA, 1 } } }, + { "nothing supported", true, true, true, true, 0, 2, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 1 }, { UBLOX_CFG_QZSS_ENA, 1 } } }, + { "nothing supported without SBAS", false, true, true, true, 0, 2, + { { UBLOX_CFG_SIGNAL_SBAS_ENA, 0 }, { UBLOX_CFG_QZSS_ENA, 1 } } }, + }; + + for (const auto &c : cases) { + SCOPED_TRACE(c.name); + ubx_config_data8_payload_t keys[UBLOX_GNSS_ENABLE_KEYS_MAX + 1]; + memset(keys, 0xAA, sizeof(keys)); + + const uint8_t count = ubloxGnssEnableKeys(keys, c.sbas, c.galileo, c.beidou, c.glonass, c.supported); + + ASSERT_EQ(c.count, count); + for (unsigned i = 0; i < count; i++) { + SCOPED_TRACE(testing::Message() << "key " << i); + EXPECT_EQ(c.keys[i].key, keys[i].key); + EXPECT_EQ(c.keys[i].value, keys[i].value); + } + // Per-signal keys are never written, and nothing past the count + for (unsigned i = count; i < UBLOX_GNSS_ENABLE_KEYS_MAX + 1; i++) { + EXPECT_EQ(0xAAAAAAAAu, keys[i].key); + EXPECT_EQ(0xAA, keys[i].value); + } + } +}