@@ -7725,25 +7725,33 @@ bool SFE_UBLOX_GNSS::getProtocolVersion(uint16_t maxWait)
77257725 // }
77267726
77277727 // We will step through the payload looking at each extension field of 30 bytes
7728- for (uint8_t extensionNumber = 0; extensionNumber < 10; extensionNumber++)
7728+ char *ptr;
7729+ for (uint8_t extensionNumber = 0; extensionNumber < ((packetCfg.len - 40) / 30); extensionNumber++)
77297730 {
7730- // Now we need to find "PROTVER=18.00" in the incoming byte stream
7731- if ((payloadCfg[(30 * extensionNumber) + 0] == 'P') && (payloadCfg[(30 * extensionNumber) + 6] == 'R') )
7731+ ptr = strstr((const char *)&payloadCfg[(30 * extensionNumber)], "PROTVER="); // Check for PROTVER (should be in extension 2)
7732+ if (ptr != nullptr )
77327733 {
7733- moduleSWVersion->versionHigh = (payloadCfg[(30 * extensionNumber) + 8] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 9] - '0'); // Convert '18' to 18
7734- moduleSWVersion->versionLow = (payloadCfg[(30 * extensionNumber) + 11] - '0') * 10 + (payloadCfg[(30 * extensionNumber) + 12] - '0'); // Convert '00' to 00
7735- moduleSWVersion->moduleQueried = true; // Mark this data as new
7734+ ptr += strlen("PROTVER="); // Point to the protocol version
7735+ int protHi = 0;
7736+ int protLo = 0;
7737+ int scanned = sscanf(ptr, "%d.%d", &protHi, &protLo);
7738+ if (scanned == 2) // Check we extracted the firmware version successfully
7739+ {
7740+ moduleSWVersion->versionHigh = protHi;
7741+ moduleSWVersion->versionLow = protLo;
7742+ moduleSWVersion->moduleQueried = true; // Mark this data as new
77367743
77377744#ifndef SFE_UBLOX_REDUCED_PROG_MEM
7738- if (_printDebug == true)
7739- {
7740- _debugSerial->print(F("Protocol version: "));
7741- _debugSerial->print(moduleSWVersion->versionHigh);
7742- _debugSerial->print(F("."));
7743- _debugSerial->println(moduleSWVersion->versionLow);
7744- }
7745+ if (_printDebug == true)
7746+ {
7747+ _debugSerial->print(F("Protocol version: "));
7748+ _debugSerial->print(moduleSWVersion->versionHigh);
7749+ _debugSerial->print(F("."));
7750+ _debugSerial->println(moduleSWVersion->versionLow);
7751+ }
77457752#endif
7746- return (true); // Success!
7753+ return (true); // Success!
7754+ }
77477755 }
77487756 }
77497757
0 commit comments