AP_Proximity: LD19/LD06 data length sanity check fix

Signed-off-by: Adam Borowski <mr.adam.borowski@gmail.com>
This commit is contained in:
Adam Borowski 2025-02-25 11:51:11 +01:00 committed by Peter Barker
parent 3f5cde4bb1
commit 3863535bdb

View File

@ -107,8 +107,8 @@ void AP_Proximity_LD06::get_readings()
}
// total_packet_length = sizeof(header) + datalength + sizeof(footer):
const uint32_t total_packet_length = 6 + 3*_response[START_DATA_LENGTH] + 5;
if (_response[START_DATA_LENGTH] != PAYLOAD_COUNT ||
const uint32_t total_packet_length = 6 + 3*(_response[START_DATA_LENGTH] & 0x1F) + 5;
if ((_response[START_DATA_LENGTH] & 0x1F) != PAYLOAD_COUNT ||
total_packet_length > ARRAY_SIZE(_response)) {
// invalid packet received; throw away all data and
// start again.