MAVLink and packet analysis

Example 1 Packet loss from sequence numbers

Sequence numbers the station received from one sender (system 1, component 1) over a period.

seqs = [250, 251, 252, 253, 254, 255, 0, 1, 3, 4, 5, 9, 10, 11, 12, 13]

lost = 0
for prev, cur in zip(seqs, seqs[1:]):
    gap = (cur - prev - 1) % 256
    if gap:
        print(f"after {prev:>3} came {cur:>3}: {gap} packet(s) lost")
    lost += gap
naive = sum(max(cur - prev - 1, 0) + (255 if cur < prev else 0) for prev, cur in zip(seqs, seqs[1:]))
print(f"received {len(seqs)}, lost {lost}, loss rate {lost / (lost + len(seqs)):.1%}")
print(f"(a naive count that ignores the wrap would report {naive} lost)")

after   1 came   3: 1 packet(s) lost
after   5 came   9: 3 packet(s) lost
received 16, lost 4, loss rate 20.0%
(a naive count that ignores the wrap would report 259 lost)

Count separately for each sender (system ID and component ID), since each has its own sequence. If a tool such as MAVProxy merges several senders, the computed rate will be wrong.

Example 2 Splitting frames and catching a bad one

A stream is built from four frames with junk bytes between them, and one byte of the third frame is corrupted. The instructor checked that pymavlink 2.4.50 reads these hand-built frames correctly.

import struct

def crc_x25(data, crc=0xFFFF):
    for b in data:
        tmp = b ^ (crc & 0xFF)
        tmp = (tmp ^ (tmp << 4)) & 0xFF
        crc = ((crc >> 8) ^ (tmp << 8) ^ (tmp << 3) ^ (tmp >> 4)) & 0xFFFF
    return crc

CRC_EXTRA = {0: 50, 30: 39}                    # HEARTBEAT, ATTITUDE
NAMES = {0: "HEARTBEAT", 30: "ATTITUDE"}

def frame(seq, msgid, payload):
    hdr = bytes([0xFD, len(payload), 0, 0, seq, 1, 1]) + msgid.to_bytes(3, "little")
    crc = crc_x25(hdr[1:] + payload + bytes([CRC_EXTRA[msgid]]))
    return hdr + payload + struct.pack("<H", crc)

hb = struct.pack("<IBBBBB", 0, 2, 3, 0x81, 4, 3)
att = struct.pack("<Iffffff", 12000, 0.05, -0.02, 1.57, 0.01, 0.0, 0.02)
stream = bytearray(b"\x00\x13" + frame(10, 0, hb) + frame(11, 30, att) + b"\x55" + frame(12, 30, att) + frame(13, 0, hb))
bad_at = stream.index(bytes([0xFD, 28, 0, 0, 12])) + 15
stream[bad_at] ^= 0xFF                          # corrupt the payload of frame seq 12

i, good, bad = 0, {}, 0
while i < len(stream):
    if stream[i] != 0xFD or i + 12 > len(stream):
        i += 1
        continue
    n = stream[i + 1]
    fr = stream[i:i + 12 + n]
    msgid = int.from_bytes(fr[7:10], "little")
    crc = struct.unpack("<H", fr[10 + n:12 + n])[0]
    if msgid in CRC_EXTRA and crc_x25(fr[1:10 + n] + bytes([CRC_EXTRA[msgid]])) == crc:
        good[NAMES[msgid]] = good.get(NAMES[msgid], 0) + 1
        print(f"seq {fr[4]:>3}: {NAMES[msgid]:<9} ok")
        i += 12 + n
    else:
        bad += 1
        print(f"seq {fr[4]:>3}: CRC failed, resync")
        i += 1
print("good frames:", good, "| bad frames:", bad)

seq  10: HEARTBEAT ok
seq  11: ATTITUDE  ok
seq  12: CRC failed, resync
seq  13: HEARTBEAT ok
good frames: {'HEARTBEAT': 2, 'ATTITUDE': 1} | bad frames: 1

When the CRC fails, the receiver advances one byte at a time to find a new start byte, so junk bytes do not spoil the next frame, and the bad frame is discarded instead of passing wrong values to the application.