Skip to content

fix reader buffer truncating multi-frame UDP datagrams (#245)#253

Open
evanofficial wants to merge 1 commit into
bluenviron:mainfrom
evanofficial:fix-udp-multi-frame-parse
Open

fix reader buffer truncating multi-frame UDP datagrams (#245)#253
evanofficial wants to merge 1 commit into
bluenviron:mainfrom
evanofficial:fix-udp-multi-frame-parse

Conversation

@evanofficial
Copy link
Copy Markdown

Fixes #245.

Root cause

The frame.Reader wraps its underlying io.Reader in a bufio.Reader sized to 512 bytes (the maximum length of a single MAVLink V2 frame: len(magic) + len(header) + 255 + len(check) + len(sig) ≈ 280, doubled by the original author as a safety margin).

That sizing is fine for stream transports (serial, TCP), but it is wrong for UDP. On UDP the underlying Read() consumes exactly one datagram per call, and any bytes that do not fit into the caller's buffer are silently discarded by the kernel (SOCK_DGRAM semantics). ArduPilot, mavp2p, and mavproxy commonly coalesce several MAVLink frames into a single UDP datagram; once the datagram exceeds 512 bytes the trailing bytes are dropped, so the next read picks up a fresh datagram mid-frame and the parser emits the symptoms reported in the issue:

wrong checksum, expected 690b, got e1cf, message id is 34
invalid magic byte: b3

The same stream works over TCP (no datagram boundaries to lose) and in QGroundControl / MAVProxy (they size their receive buffers for the full UDP MTU), which matches the report.

Fix

Raise the internal buffer to 65536 bytes so a single fill() can hold any plausible UDP datagram (max UDP payload is 65507). The buffer is allocated once per Reader, not per packet, so there is no hot-path allocation and no impact on the TCP / serial code paths.

Test

Added TestReaderUDPMultiFrameDatagram in pkg/frame/reader_test.go. It uses a small io.Reader that mimics UDP semantics (one datagram per Read, tail discarded if it does not fit) and feeds the Reader a single datagram containing three V2 frames totalling ~590 bytes. With the previous 512-byte buffer the third frame is corrupted; with the fix all three decode cleanly.

@aler9
Copy link
Copy Markdown
Member

aler9 commented May 16, 2026

Hello, i checked the source code of mavp2p (that i wrote myself), ardupilot and MAVproxy, and none of those coalesce multiple Mavlink frames into a single UDP packet.

In mavp2p / gomavlib, each Mavlink frame is written into a single UDP packet by calling the socket's Write() function once per frame:

_, err = w.ByteWriter.Write(w.bw[:n])

The same happens in Ardupilot:
https://github.com/ArduPilot/ardupilot/blob/7ff393232f224c811269cc17182603175ff61bba/libraries/AP_HAL/utility/packetise.cpp#L63

and MAVproxy:
https://github.com/ArduPilot/pymavlink/blob/3cedcf10bdd8e6d7664bacf494f6455e28c272b2/mavutil.py#L1107

where are your packets coming from?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

UDP client parse errors with valid ArduPilot MAVLink stream that QGroundControl can read

2 participants