From 3863535bdb0a086ac78fd8273ca047a39c06c064 Mon Sep 17 00:00:00 2001 From: Adam Borowski Date: Tue, 25 Feb 2025 11:51:11 +0100 Subject: [PATCH] AP_Proximity: LD19/LD06 data length sanity check fix Signed-off-by: Adam Borowski --- libraries/AP_Proximity/AP_Proximity_LD06.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Proximity/AP_Proximity_LD06.cpp b/libraries/AP_Proximity/AP_Proximity_LD06.cpp index 69781e21ee..428445c08b 100644 --- a/libraries/AP_Proximity/AP_Proximity_LD06.cpp +++ b/libraries/AP_Proximity/AP_Proximity_LD06.cpp @@ -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.