cart: симметричный ПК-мост через сеть + зеркало протокола
Переносит управление с STM-передатчика на STM-приёмник через локальную сеть/интернет двумя экземплярами одной программы. - tools/link_proto.py: зеркало link_protocol.h на Python (кадрирование, CRC16-CCITT, (де)сериализация CONTROL/IDENT, инкрементальный парсер с ресинхронизацией) + --selftest с тест-векторами. - tools/bridge.py: роль tx (USB->UDP) и rx (UDP->USB) из одного флага; поиск порта по product string и handshake HELLO/IDENT; UDP с seq и anti-replay (распознавание новой сессии при рестарте источника); сетевой failsafe при тишине; опциональная HMAC-аутентификация пакетов и фильтр источника. Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
This commit is contained in:
Claude Opus 4.8
@@ -0,0 +1,246 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Сетевой мост «пульт -> тележка»: один симметричный сервер, роль задаётся
|
||||
флагом. Два экземпляра переносят управление с STM-передатчика на STM-приёмник
|
||||
через локальную сеть/интернет.
|
||||
|
||||
[пульт] ADC-> STM(CART-TX) --USB--> bridge --role tx --UDP--> bridge --role rx --USB--> STM(CART-RX) --SBUS-> драйвер
|
||||
|
||||
Роль tx: читает кадры CONTROL из USB от STM-TX и шлёт их по UDP на peer.
|
||||
Роль rx: принимает UDP, пишет CONTROL в USB на STM-RX; при тишине из сети шлёт
|
||||
явную нейтраль с флагом failsafe (главный предохранитель — на самом STM).
|
||||
|
||||
Устройство выбирается по USB product string (CART-TX / CART-RX) и подтверждается
|
||||
handshake-кадром IDENT (роль + версия протокола). Для интернета используй общий
|
||||
ключ (--key) — каждый UDP-пакет аутентифицируется HMAC-SHA256; без ключа трафик
|
||||
идёт в открытом виде (только для доверенной локальной сети).
|
||||
|
||||
Запуск (из каталога прошивки):
|
||||
.venv/bin/python tools/bridge.py --role tx --peer 10.0.0.2:5005
|
||||
.venv/bin/python tools/bridge.py --role rx --bind 0.0.0.0:5005
|
||||
"""
|
||||
import argparse
|
||||
import hmac
|
||||
import logging
|
||||
import socket
|
||||
import sys
|
||||
import time
|
||||
from hashlib import sha256
|
||||
|
||||
import serial
|
||||
from serial.tools import list_ports
|
||||
|
||||
import link_proto as lp
|
||||
|
||||
HMAC_LEN = 8 # усечённый HMAC-SHA256 на пакет
|
||||
NET_TIMEOUT_S = 0.10 # нет UDP дольше -> rx начинает слать failsafe
|
||||
HELLO_TIMEOUT_S = 1.0
|
||||
SEQ_RESET_AFTER = 20 # столько подряд «несвежих» пакетов = перезапуск источника
|
||||
|
||||
log = logging.getLogger("bridge")
|
||||
|
||||
|
||||
# ---------------- выбор и handshake USB-устройства ----------------
|
||||
|
||||
def find_port(role: str, selector: str | None) -> str:
|
||||
"""Найти последовательный порт по роли (product string) или явному
|
||||
селектору (путь /dev/..., подстрока product или серийник)."""
|
||||
if selector and ("/" in selector or selector.startswith("COM")):
|
||||
return selector # явный путь к устройству
|
||||
want_product = {"tx": "CART-TX", "rx": "CART-RX"}[role]
|
||||
matches = []
|
||||
for p in list_ports.comports():
|
||||
hay = " ".join(filter(None, (p.product, p.serial_number, p.description))).upper()
|
||||
if selector:
|
||||
if selector.upper() in hay:
|
||||
matches.append(p)
|
||||
elif want_product in hay:
|
||||
matches.append(p)
|
||||
if not matches:
|
||||
raise RuntimeError(f"USB-устройство роли {role} ({want_product}) не найдено")
|
||||
if len(matches) > 1:
|
||||
names = ", ".join(f"{m.device}({m.product})" for m in matches)
|
||||
raise RuntimeError(f"найдено несколько кандидатов роли {role}: {names}; уточни --device")
|
||||
return matches[0].device
|
||||
|
||||
|
||||
def handshake(ser: serial.Serial, role: str):
|
||||
"""Послать HELLO, дождаться IDENT, проверить роль и версию протокола."""
|
||||
want_role = {"tx": lp.ROLE_TX, "rx": lp.ROLE_RX}[role]
|
||||
parser = lp.FrameParser()
|
||||
ser.reset_input_buffer()
|
||||
ser.write(lp.build_frame(lp.HELLO, b""))
|
||||
deadline = time.monotonic() + HELLO_TIMEOUT_S
|
||||
while time.monotonic() < deadline:
|
||||
for ftype, payload in parser.feed(ser.read(256)):
|
||||
if ftype != lp.IDENT:
|
||||
continue
|
||||
ident = lp.unpack_ident(payload)
|
||||
if ident["proto_ver"] != lp.VERSION:
|
||||
raise RuntimeError(
|
||||
f"версия протокола устройства {ident['proto_ver']} != {lp.VERSION}")
|
||||
if ident["role"] != want_role:
|
||||
raise RuntimeError(
|
||||
f"устройство представилось ролью {lp.role_name(ident['role'])}, ожидалась {role}")
|
||||
log.info("handshake OK: роль %s, fw 0x%04x, uid %s",
|
||||
role, ident["fw_ver"], ident["uid"].hex())
|
||||
return
|
||||
raise RuntimeError("нет ответа IDENT (не та прошивка/порт?)")
|
||||
|
||||
|
||||
def open_device(role: str, selector: str | None) -> serial.Serial:
|
||||
port = find_port(role, selector)
|
||||
ser = serial.Serial(port, baudrate=115200, timeout=0.05)
|
||||
handshake(ser, role)
|
||||
return ser
|
||||
|
||||
|
||||
# ---------------- аутентификация пакетов ----------------
|
||||
|
||||
def seal(payload: bytes, key: bytes | None) -> bytes:
|
||||
if not key:
|
||||
return payload
|
||||
return payload + hmac.new(key, payload, sha256).digest()[:HMAC_LEN]
|
||||
|
||||
|
||||
def unseal(datagram: bytes, key: bytes | None) -> bytes | None:
|
||||
"""Проверить HMAC и вернуть payload, либо None если пакет невалиден."""
|
||||
if not key:
|
||||
return datagram
|
||||
if len(datagram) < HMAC_LEN:
|
||||
return None
|
||||
payload, tag = datagram[:-HMAC_LEN], datagram[-HMAC_LEN:]
|
||||
want = hmac.new(key, payload, sha256).digest()[:HMAC_LEN]
|
||||
return payload if hmac.compare_digest(tag, want) else None
|
||||
|
||||
|
||||
def seq_newer(a: int, b: int) -> bool:
|
||||
"""Арифметика последовательных номеров с переполнением uint32."""
|
||||
return 0 < ((a - b) & 0xFFFFFFFF) < 0x80000000
|
||||
|
||||
|
||||
# ---------------- роль TX: USB -> сеть ----------------
|
||||
|
||||
def run_tx(args, key):
|
||||
peer = parse_hostport(args.peer, "peer")
|
||||
ser = open_device("tx", args.device)
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
parser = lp.FrameParser()
|
||||
log.info("tx: %s -> UDP %s:%d", ser.port, *peer)
|
||||
sent = 0
|
||||
while True:
|
||||
for ftype, payload in parser.feed(ser.read(256)):
|
||||
if ftype == lp.CONTROL:
|
||||
sock.sendto(seal(payload, key), peer)
|
||||
sent += 1
|
||||
if sent % 100 == 0:
|
||||
c = lp.unpack_control(payload)
|
||||
log.debug("tx seq=%d ch2=%d ch4=%d", c["seq"], c["ch"][1], c["ch"][3])
|
||||
elif ftype == lp.LOG:
|
||||
log.info("STM-TX: %s", payload.decode("ascii", "replace"))
|
||||
|
||||
|
||||
# ---------------- роль RX: сеть -> USB ----------------
|
||||
|
||||
def run_rx(args, key):
|
||||
bind = parse_hostport(args.bind, "bind")
|
||||
# для rx --peer — фильтр источника по хосту (порт источника эфемерный)
|
||||
allowed_host = args.peer.split(":")[0] if args.peer else None
|
||||
ser = open_device("rx", args.device)
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
||||
sock.bind(bind)
|
||||
sock.settimeout(lp.FRAME_PERIOD_MS / 1000.0)
|
||||
parser = lp.FrameParser()
|
||||
log.info("rx: UDP %s:%d -> %s", bind[0], bind[1], ser.port)
|
||||
|
||||
last_seq = None
|
||||
last_net = time.monotonic()
|
||||
last_fs = 0.0
|
||||
fs_seq = 0
|
||||
in_failsafe = False
|
||||
rejects = 0 # подряд отброшенных по seq -> признак перезапуска источника
|
||||
|
||||
while True:
|
||||
try:
|
||||
datagram, addr = sock.recvfrom(2048)
|
||||
if allowed_host and addr[0] != allowed_host:
|
||||
log.warning("пакет с неожиданного адреса %s отброшен", addr[0])
|
||||
datagram = None
|
||||
payload = unseal(datagram, key) if datagram else None
|
||||
if payload and len(payload) == lp.CONTROL_LEN:
|
||||
c = lp.unpack_control(payload)
|
||||
# Принять свежий seq ИЛИ новую сессию: источник (STM-TX) после
|
||||
# ребута начинает seq заново с 0, и без сброса last_seq поток
|
||||
# завис бы в failsafe навсегда. Новую сессию распознаём по пачке
|
||||
# подряд «несвежих» пакетов (устойчиво к одиночным reorder/dup).
|
||||
fresh = last_seq is None or seq_newer(c["seq"], last_seq)
|
||||
if not fresh:
|
||||
rejects += 1
|
||||
if rejects >= SEQ_RESET_AFTER:
|
||||
log.warning("seq откатился (%d -> %d): новая сессия", last_seq, c["seq"])
|
||||
fresh = True
|
||||
if fresh:
|
||||
rejects = 0
|
||||
last_seq = c["seq"]
|
||||
last_net = time.monotonic()
|
||||
if in_failsafe:
|
||||
log.info("связь восстановлена")
|
||||
in_failsafe = False
|
||||
ser.write(lp.build_frame(lp.CONTROL, payload))
|
||||
elif datagram is not None:
|
||||
log.warning("битый/неаутентичный пакет отброшен")
|
||||
except socket.timeout:
|
||||
pass
|
||||
|
||||
# сетевой failsafe: тишина -> активная нейтраль на STM в темпе 20 Гц
|
||||
now = time.monotonic()
|
||||
if now - last_net >= NET_TIMEOUT_S and now - last_fs >= lp.FRAME_PERIOD_MS / 1000.0:
|
||||
if not in_failsafe:
|
||||
log.warning("потеря сети -> failsafe (нейтраль)")
|
||||
in_failsafe = True
|
||||
last_seq = None # рестарт источника при простое примем сразу
|
||||
ser.write(lp.build_frame(lp.CONTROL, lp.neutral_control(fs_seq, failsafe=True)))
|
||||
fs_seq = (fs_seq + 1) & 0xFFFFFFFF
|
||||
last_fs = now
|
||||
|
||||
# дренируем входящие LOG/IDENT от STM-RX
|
||||
for ftype, payload in parser.feed(ser.read(256)):
|
||||
if ftype == lp.LOG:
|
||||
log.info("STM-RX: %s", payload.decode("ascii", "replace"))
|
||||
|
||||
|
||||
# ---------------- общее ----------------
|
||||
|
||||
def parse_hostport(s: str, what: str):
|
||||
if ":" not in s:
|
||||
raise SystemExit(f"--{what} ожидает HOST:PORT, получено {s!r}")
|
||||
host, port = s.rsplit(":", 1)
|
||||
return (host, int(port))
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser(description="мост пульт<->тележка через сеть")
|
||||
ap.add_argument("--role", required=True, choices=("tx", "rx"))
|
||||
ap.add_argument("--peer", help="tx: куда слать HOST:PORT; rx: разрешённый источник (опц.)")
|
||||
ap.add_argument("--bind", default="0.0.0.0:5005", help="rx: локальный UDP HOST:PORT")
|
||||
ap.add_argument("--device", help="путь к порту или подстрока product/serial (иначе по роли)")
|
||||
ap.add_argument("--key", help="общий ключ для HMAC-аутентификации пакетов (для интернета)")
|
||||
ap.add_argument("--log-level", default="INFO")
|
||||
args = ap.parse_args()
|
||||
|
||||
logging.basicConfig(level=args.log_level.upper(),
|
||||
format="%(asctime)s %(levelname)s %(message)s")
|
||||
key = args.key.encode() if args.key else None
|
||||
if args.role == "tx" and not args.peer:
|
||||
raise SystemExit("--peer обязателен для роли tx")
|
||||
if not key:
|
||||
log.warning("без --key: трафик не аутентифицирован, только для доверенной сети")
|
||||
|
||||
try:
|
||||
(run_tx if args.role == "tx" else run_rx)(args, key)
|
||||
except KeyboardInterrupt:
|
||||
log.info("остановлено")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,176 @@
|
||||
#!/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 = 150
|
||||
|
||||
|
||||
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__)
|
||||
Reference in New Issue
Block a user