USD1 legacy Version 0 serial parser waits for six bytes and discards measurements

Bug Report

Issue details

The USD1 serial rangefinder backend incorrectly waits for six bytes before processing the legacy three-byte Version 0 protocol. This can cause legacy USD1 measurements to be discarded or prevent the backend from producing readings.

The legacy Version 0 format consists of three bytes:

Byte 0: 0x48 header
Byte 1: Lower seven distance bits
Byte 2: Upper seven distance bits

The modern USD1 protocol uses a six-byte frame beginning with 0xFE.

In [AP_RangeFinder_USD1_Serial.cpp](https://github.com/ArduPilot/ardupilot/blob/4fe7ad4fab8c1bf4ade7cbc7ae85a73c81d73e05/libraries/AP_RangeFinder/AP_RangeFinder_USD1_Serial.cpp#L124-L160), the parser uses the following condition:

if ((_linebuf_len < (sizeof(_linebuf)/sizeof(_linebuf[0]))) ||
    (_version == 0 && _linebuf_len < 3)) {
    /* don't process _linebuf until we've collected six bytes of data
     * (or 3 bytes for Version 0 firmware)
     */
    continue;
}

Because _linebuf contains six bytes, the first operand remains true until _linebuf_len reaches six. For a complete three-byte Version 0 frame, the condition evaluates as:

(3 < 6) || (_version == 0 && 3 < 3)
true     || false

The frame is therefore not processed.

For a continuous Version 0 stream:

H A B | H C D | H E F | ...

where H is the 0x48 header, the parser stores two complete frames before processing:

Index:    0  1  2  3  4  5
Contents: H  A  B  H  C  D

It then calculates the distance using only the first frame:

sum += (_linebuf[2] & 0x7F) * 128 +
       (_linebuf[1] & 0x7F);

Afterward, it resets _linebuf_len, discarding the second complete frame.

The resulting behavior depends on how many bytes are available during each get_reading() call:

• If at least two Version 0 frames are available, the first frame is decoded and the second is discarded. This reduces the effective measurement rate.
• If only one complete Version 0 frame is available, the function returns without producing a reading. On the next call, the following header resets _linebuf_len, causing the previous unprocessed frame to be overwritten.
• Modern six-byte frames beginning with 0xFE are unaffected because waiting for six bytes is correct for that protocol.

The condition should select the required frame length based on the detected framing format:

const uint8_t required_length =
    (_version == 0 && _header == USD1_HDR_V0) ?
    3 : sizeof(_linebuf);

if (_linebuf_len < required_length) {
    continue;
}

Affected code:

• AP_RangeFinder_USD1_Serial.cpp, get_reading()
• AP_RangeFinder_USD1_Serial.cpp, protocol headers and version detection

Version

ArduPilot revision 4fe7ad4fab8c1bf4ade7cbc7ae85a73c81d73e05.

The same condition is also present on the current master branch.

Platform

• All
• AntennaTracker
• Copter
• Plane
• Rover
• Submarine

The affected backend is shared and is not specific to a particular vehicle platform. The issue applies only when AP_RANGEFINDER_USD1_SERIAL_ENABLED is compiled and a sensor using the legacy three-byte Version 0 serial format is connected.

Airframe type

Not airframe-specific.

Hardware type

Any autopilot hardware using a UART-connected USD1/uLanding radar with the legacy beta Version 0 three-byte 0x48 protocol. Devices using the modern six-byte 0xFE protocol are unaffected.