radar_system/device_firmware/cart_firmware/tools/link_proto.py

#!/usr/bin/env python3
"""Зеркало бинарного протокола из src/link_protocol.h (единственный источник
истины на стороне Python). Кадрирование поверх потока, CRC16-CCITT, упаковка и
разбор CONTROL/IDENT/HELLO/LOG, инкрементальный парсер с ресинхронизацией.

При правке src/link_protocol.h меняй ЭТОТ файл и сверяйся:
    python3 tools/link_proto.py --selftest
"""
import struct
import sys

SYNC0 = 0xA5
SYNC1 = 0x5A
VERSION = 1
MAX_PAYLOAD = 64

# типы кадров
HELLO = 0x03
IDENT = 0x02
CONTROL = 0x01
LOG = 0x04

# роли
ROLE_TX = ord("T")
ROLE_RX = ord("R")

# флаги CONTROL
CTRL_FLAG_FAILSAFE = 0x01

CONTROL_LEN = 4 + 4 + 1 + 2 * 16  # 41
IDENT_LEN = 1 + 1 + 2 + 12  # 16

SBUS_MID = 1024

# тайминги (мс), зеркало link_protocol.h
FRAME_PERIOD_MS = 50
RX_USB_TIMEOUT_MS = 400


def crc16(data: bytes) -> int:
    """CRC16-CCITT, poly 0x1021, init 0xFFFF — как linkCrc16 в прошивке."""
    crc = 0xFFFF
    for byte in data:
        crc ^= byte << 8
        for _ in range(8):
            crc = ((crc << 1) ^ 0x1021) & 0xFFFF if (crc & 0x8000) else (crc << 1) & 0xFFFF
    return crc


def build_frame(frame_type: int, payload: bytes) -> bytes:
    """Собрать кадр: SYNC + VER + TYPE + LEN + PAYLOAD + CRC16(LE)."""
    if len(payload) > MAX_PAYLOAD:
        raise ValueError("payload too long")
    body = bytes((VERSION, frame_type, len(payload))) + payload
    crc = crc16(body)
    return bytes((SYNC0, SYNC1)) + body + struct.pack("<H", crc)


def pack_control(seq: int, t_ms: int, flags: int, ch) -> bytes:
    """Полезная нагрузка CONTROL. ch — 16 значений 0..2047."""
    if len(ch) != 16:
        raise ValueError("need 16 channels")
    return struct.pack("<IIB16H", seq & 0xFFFFFFFF, t_ms & 0xFFFFFFFF, flags & 0xFF, *ch)


def unpack_control(payload: bytes):
    """-> dict(seq, t_ms, flags, ch[16])."""
    if len(payload) != CONTROL_LEN:
        raise ValueError("bad CONTROL length")
    seq, t_ms, flags = struct.unpack_from("<IIB", payload, 0)
    ch = list(struct.unpack_from("<16H", payload, 9))
    return {"seq": seq, "t_ms": t_ms, "flags": flags, "ch": ch}


def unpack_ident(payload: bytes):
    """-> dict(role, proto_ver, fw_ver, uid)."""
    if len(payload) != IDENT_LEN:
        raise ValueError("bad IDENT length")
    role, proto_ver, fw_ver = struct.unpack_from("<BBH", payload, 0)
    uid = payload[4:16]
    return {"role": role, "proto_ver": proto_ver, "fw_ver": fw_ver, "uid": uid}


def role_name(role: int) -> str:
    return {ROLE_TX: "tx", ROLE_RX: "rx"}.get(role, "?")


def neutral_control(seq: int = 0, t_ms: int = 0, failsafe: bool = True) -> bytes:
    """Нейтральный CONTROL (все каналы 1024) — для сетевого failsafe на RX."""
    flags = CTRL_FLAG_FAILSAFE if failsafe else 0
    return pack_control(seq, t_ms, flags, [SBUS_MID] * 16)


class FrameParser:
    """Инкрементальный разбор потока. Скорми feed() любые байты — вернёт список
    (type, payload) полных валидных кадров. Устойчив к мусору и потере байт:
    при сбое CRC/версии сдвигается на байт и ищет следующий SYNC."""

    def __init__(self):
        self._buf = bytearray()

    def feed(self, data: bytes):
        self._buf.extend(data)
        out = []
        while True:
            frame = self._try_one()
            if frame is None:
                break
            out.append(frame)
        return out

    def _try_one(self):
        buf = self._buf
        while True:
            # найти синхрослово
            while len(buf) >= 2 and not (buf[0] == SYNC0 and buf[1] == SYNC1):
                del buf[0]
            if len(buf) < 5:  # SYNC(2)+VER+TYPE+LEN
                return None
            ver, ftype, length = buf[2], buf[3], buf[4]
            if ver != VERSION or length > MAX_PAYLOAD:
                del buf[0]  # ложный синк — сдвиг и поиск заново в этом же вызове
                continue
            total = 5 + length + 2
            if len(buf) < total:
                return None
            body = bytes(buf[2:5 + length])
            want = struct.unpack_from("<H", buf, 5 + length)[0]
            if crc16(body) != want:
                del buf[0]  # битый кадр — ресинхронизация
                continue
            payload = bytes(buf[5:5 + length])
            del buf[:total]
            return (ftype, payload)


def _selftest():
    # тест-вектор CONTROL: совпадает с packControl в прошивке
    ch = [SBUS_MID] * 16
    ch[1] = 1500
    ch[3] = 600
    payload = pack_control(0x01020304, 0x0A0B0C0D, CTRL_FLAG_FAILSAFE, ch)
    assert len(payload) == CONTROL_LEN, len(payload)
    back = unpack_control(payload)
    assert back["seq"] == 0x01020304 and back["ch"][1] == 1500 and back["flags"] == 1, back

    # кадрирование туда-обратно через парсер, c мусором спереди
    frame = build_frame(CONTROL, payload)
    p = FrameParser()
    frames = p.feed(b"\x00\xffgarbage" + frame)
    assert len(frames) == 1 and frames[0][0] == CONTROL, frames
    assert frames[0][1] == payload

    # битый CRC отбрасывается, следующий кадр ловится
    bad = bytearray(frame)
    bad[-1] ^= 0xFF
    p2 = FrameParser()
    assert p2.feed(bytes(bad)) == []
    assert len(p2.feed(frame)) == 1

    # битый + валидный в ОДНОМ feed() -> валидный извлекается сразу (ресинхрон
    # не прерывает разбор остатка буфера)
    p3 = FrameParser()
    got = p3.feed(bytes(bad) + frame)
    assert len(got) == 1 and got[0][1] == payload, got

    # известный CRC-вектор для сверки с прошивкой
    assert crc16(b"123456789") == 0x29B1, hex(crc16(b"123456789"))
    print("link_proto selftest OK")


if __name__ == "__main__":
    if "--selftest" in sys.argv:
        _selftest()
    else:
        print(__doc__)