Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 10:28:29 -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 62f69cf92f
commit ea337209e6
1 changed files with 42 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -884,23 +884,50 @@ AP_GPS_UBLOX::_parse_gps(void)
cfg_len -= 4; cfg_len -= 4;
cfg_data += 4; cfg_data += 4;
switch (id) { switch (id) {
case ConfigKey::TMODE_MODE: { case ConfigKey::TMODE_MODE: {
uint8_t mode = cfg_data[0]; uint8_t mode = cfg_data[0];
cfg_len -= 1; if (mode != 0) {
cfg_data += 1; // ask for mode 0, to disable TIME mode
if (mode != 0) { mode = 0;
// ask for mode 0, to disable TIME mode _configure_valset(ConfigKey::TMODE_MODE, 1, &mode);
mode = 0; _unconfigured_messages |= CONFIG_TMODE_MODE;
_configure_valset(ConfigKey::TMODE_MODE, 1, &mode); } else {
_unconfigured_messages |= CONFIG_TMODE_MODE; _unconfigured_messages &= ~CONFIG_TMODE_MODE;
} else { }
_unconfigured_messages &= ~CONFIG_TMODE_MODE; break;
} }
break; default:
break;
} }
default:
// we don't know the length so we stop parsing // step over the value
return false; 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;
} }
} }
} }