Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_GPS: UBlox: Fix the assumption that all keys are 1 byte keys

Browse Source
This commit is contained in:
Michael du Breuil 2019-11-13 16:45:52 -07:00 committed by Andrew Tridgell
parent 150a4edcf2
commit 63a6a1084c
1 changed files with 42 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -886,8 +886,6 @@ AP_GPS_UBLOX::_parse_gps(void)
switch (id) {
case ConfigKey::TMODE_MODE: {
uint8_t mode = cfg_data[0];
cfg_len -= 1;
cfg_data += 1;
if (mode != 0) {
// ask for mode 0, to disable TIME mode
mode = 0;
@ -899,9 +897,38 @@ AP_GPS_UBLOX::_parse_gps(void)
break;
}
default:
// we don't know the length so we stop parsing
break;
}
// step over the value
const uint8_t key_size = ((uint32_t)id >> 28) & 0x07; // mask off the storage size
uint8_t step_size = 0;
switch (key_size) {
case 0x1: // bit
step_size = 1;
break;
case 0x2: // byte
step_size = 1;
break;
case 0x3: // 2 bytes
step_size = 2;
break;
case 0x4: // 4 bytes
step_size = 3;
break;
case 0x5: // 8 bytes
step_size = 4;
break;
default:
// unknown/bad key size
return false;
}
if (cfg_len <= step_size) {
cfg_len = 0;
} else {
cfg_len -= step_size;
cfg_data += step_size;
}
}
}
}