@@ -7725,25 +7725,33 @@ bool SFE_UBLOX_GNSS::getProtocolVersion(uint16_t maxWait)
7725
7725
// }
7726
7726
7727
7727
// 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++)
7729
7730
{
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 )
7732
7733
{
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
7736
7743
7737
7744
#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
+ }
7745
7752
#endif
7746
- return (true); // Success!
7753
+ return (true); // Success!
7754
+ }
7747
7755
}
7748
7756
}
7749
7757
0 commit comments