LD19/LD06 data length sanity check fix

Signed-off-by: Adam Borowski <[email protected]>
ArduPilot · Feb 25, 2025 · 8625163 · 8625163
1 parent aef8f9d
commit 8625163
Showing 1 changed file with 2 additions and 2 deletions.
diff --git 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.