Compare commits

5 Commits
Author SHA1 Message Date
aweandClaude Opus 4.8 7334f89491 cart: документация сетевого моста в protocol.md
Разделы про роли и идентификацию устройства, кадр USB CDC, сетевой слой
(UDP, seq/новая сессия, HMAC, WireGuard), многоуровневый failsafe и
запуск моста.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-10-19 13:13:24 +03:00
aweandClaude Opus 4.8 50f07caa64 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>
2026-10-19 13:13:24 +03:00
aweandClaude Opus 4.8 8704db2de7 cart: роли прошивки TX и RX, выбор на этапе сборки
Прошивка получает две роли, выбираемые compile-time env; общий код
собирается в оба env, специфичный — через build_src_filter, без мёртвого
кода неактивной роли. Роль публикуется в USB product string
(CART-TX/CART-RX), по которой ПК находит нужный порт.

- app_tx.cpp: как раньше формирует SBUS на локальный провод и
  дополнительно шлёт те же ch[16] по USB кадром CONTROL; отвечает на
  HELLO кадром IDENT. Старый проводной функционал не изменён.
- app_rx.cpp: принимает CONTROL по USB и гонит SBUS на провод строго
  каждые 50 мс из последнего состояния. FAILSAFE: нет свежего CONTROL
  дольше 150 мс -> нейтраль с флагом failsafe (срабатывает и при обрыве
  USB/зависании ПК); сетевой флаг failsafe транслируется в бит SBUS.
- platformio.ini: env cart_tx/cart_rx, макросы роли и USB_PRODUCT_STRING.
- удалён монолитный main.cpp (логика разнесена по модулям).

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-10-19 13:13:01 +03:00
aweandClaude Opus 4.8 bd9395fc3d cart: общий бинарный протокол связи поверх USB CDC
Единый слой связи пульт<->ПК<->тележка, одинаковый на обеих ролях и
зеркалимый на Python.

- link_protocol.h: единственный источник истины — кадрирование
  (SYNC/VER/TYPE/LEN/PAYLOAD/CRC16-CCITT), типы CONTROL/IDENT/HELLO/LOG,
  layout полезных нагрузок, роли, тайминги failsafe.
- usb_link.{h,cpp}: CRC16, сериализация CONTROL/IDENT, отправка кадров и
  стейт-машина приёма с ресинхронизацией по синхрослову и проверкой CRC.
- device_id.h: чтение 96-битного UID чипа для IDENT.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-10-19 13:12:48 +03:00
aweandClaude Opus 4.8 6f1eaf19e4 cart: вынести SBUS и логику пульта в отдельные модули
Подготовка к ролям TX/RX: выделяю из монолитного main.cpp
переиспользуемые части без изменения поведения.

- sbus.{h,cpp}: упаковка 25-байтового SBUS-кадра, инициализация и
  вывод инвертированного SBUS на USART1 (PA9) — единственный владелец
  провода к драйверу, общий для будущих ролей.
- stick.{h,cpp}: калибровка стика, нормализация осей и профиль
  газа/поворота, формирование ch[16]. Неиспользованные в прежнем loop
  expo()/toSbus() и мёртвые константы не переносятся.

Co-Authored-By: Claude Opus 4.8 (1M context) <noreply@anthropic.com>
2026-10-19 13:12:07 +03:00
15 changed files with 1079 additions and 167 deletions
@@ -55,3 +55,97 @@
- Отправлять 25-байтовый кадр по таймеру каждые 50 мс. - Отправлять 25-байтовый кадр по таймеру каждые 50 мс.
- Нейтраль: все каналы 1024; управление — каналы 2 и 4 в измеренных - Нейтраль: все каналы 1024; управление — каналы 2 и 4 в измеренных
диапазонах. диапазонах.
# Сетевой мост «пульт → тележка»
Та же команда управления может идти не по проводу рядом, а через локальную
сеть/интернет, не ломая прежний проводной режим. Цепочка:
```
[пульт] ADC→ STM(CART-TX) ─USB─► bridge --role tx ─UDP─► bridge --role rx ─USB─► STM(CART-RX) ─SBUS─► драйвер
```
STM-TX продолжает гнать SBUS на свой локальный провод как раньше и
дополнительно отдаёт те же значения каналов по USB. STM-RX принимает их по USB
и выдаёт на свой провод так, будто сигнал пришёл с пульта.
## Роли и идентификация устройства
Роль выбирается на сборке (`platformio.ini`): `pio run -e cart_tx` или
`-e cart_rx`. Общий код (`sbus`, `usb_link`) собирается в оба env, специфичный —
через `build_src_filter`, мёртвого кода неактивной роли нет.
Каждая прошивка публикует USB **product string**: `CART-TX` / `CART-RX`. По ней
`tools/bridge.py` находит нужный порт (`serial.tools.list_ports`). После открытия
порта ПК шлёт `HELLO` и ждёт `IDENT` — подтверждение роли, версии протокола и
96-битного UID чипа (на случай неверно прошитого устройства, рассинхрона версий
или чужого CDC с тем же VID/PID). Serial-дескриптор (UID) различает экземпляры.
## Кадр поверх USB CDC
Единственный источник истины — `src/link_protocol.h`, зеркало на Python —
`tools/link_proto.py` (сверка: `python3 tools/link_proto.py --selftest`).
```
[0xA5][0x5A][VER=1][TYPE][LEN][PAYLOAD..LEN][CRC16_LO][CRC16_HI]
```
- CRC16-CCITT (poly `0x1021`, init `0xFFFF`) по байтам `[VER..конец PAYLOAD]`, LE.
Контрольный вектор: `crc16("123456789") = 0x29B1`.
- Приёмник (STM и Python) — стейт-машина: ищет синхрослово, читает по `LEN`,
проверяет CRC; при сбое сдвиг на байт и ресинхронизация (устойчивость к мусору
и потере байта).
- Типы: `0x01 CONTROL`, `0x02 IDENT`, `0x03 HELLO`, `0x04 LOG`. Отладочный текст
идёт только кадром `LOG` — «голых» `Serial.print`, ломающих бинарный поток, нет.
Payload `CONTROL` (LE): `seq:u32`, `t_ms:u32`, `flags:u8`, `ch[16]:u16` = 41 байт.
`ch[]` — значения SBUS-каналов 0..2047, ровно те, что TX кладёт на свой провод.
Это общая «валюта» всей цепочки: ПК перекладывает payload в UDP и обратно, RX
распаковывает и пакует в SBUS. `flags` bit0 = failsafe.
## Сетевой слой (между экземплярами bridge)
- Транспорт — **UDP**: для управления 20 Гц свежий кадр ценнее
доставленного; TCP при потере встал бы в head-of-line blocking и копил лаг.
- Полезная нагрузка UDP — тот же `CONTROL` payload. `seq` даёт отбрасывание
устаревших/повторных пакетов (защита от переупорядочивания и дубликатов UDP)
с корректной арифметикой переполнения uint32. Перезапуск источника (seq снова
с 0) распознаётся по пачке подряд «несвежих» пакетов и трактуется как новая
сессия — иначе поток завис бы в failsafe навсегда. Это best-effort защита от
повтора старых команд, не криптографическая: аутентичность даёт HMAC, против
целевого replay на недоверенной сети используй WireGuard.
- Приём из сети и выдача SBUS на провод — **разные таймеры**: джиттер сети не
попадает на провод, STM-RX гонит SBUS строго каждые 50 мс из последнего
состояния.
- **Безопасность.** Для интернета обязателен общий ключ (`--key`): каждый UDP-
пакет аутентифицируется усечённым HMAC-SHA256 (8 байт), RX отбрасывает
неаутентичные и принимает только с ожидаемого адреса (`--peer`). Рекомендуемый
способ наружу — WireGuard-туннель, внутри которого идёт простой UDP. «Голый»
неаутентифицированный UDP допустим только в доверенной локальной сети.
## FAILSAFE
Потеря связи никогда не оставляет тележку на последней команде — выход всегда
нейтраль (все каналы 1024), а не удержание.
- **Главный предохранитель — на самом STM-RX.** Если валидный `CONTROL` по USB
не приходил дольше `RX_USB_TIMEOUT_MS = 150` мс (3 кадра), STM-RX сам выдаёт на
провод нейтраль с флагом failsafe (SBUS bit3). Срабатывает при обрыве USB,
зависании или падении ПК-процесса. SBUS на провод идёт непрерывно 20 Гц в любом
случае — драйвер не видит пропажи сигнала.
- **Сетевой failsafe на ПК-RX.** При тишине из сети дольше 100 мс bridge активно
шлёт на STM-RX нейтральные `CONTROL` с флагом `CTRL_FLAG_FAILSAFE` в темпе
20 Гц, чтобы причина была явной. STM-RX транслирует этот флаг в бит failsafe
SBUS-кадра (byte 23 bit3). Восстановление — по первому свежему валидному пакету.
## Запуск
```
# сторона пульта
.venv/bin/python tools/bridge.py --role tx --peer 10.0.0.2:5005 --key SECRET
# сторона тележки
.venv/bin/python tools/bridge.py --role rx --bind 0.0.0.0:5005 --peer 10.0.0.1 --key SECRET
```
Локальная проверка без сети: оба экземпляра на одной машине с `--peer 127.0.0.1`
и двумя STM по USB; SBUS с RX снять `tools/decode.py` и сверить с осями стика TX.
+23 -1
View File
@@ -1,4 +1,11 @@
[env:weact_g431cb] ; Две роли одной прошивки тележки, выбираются на этапе сборки:
; pio run -e cart_tx — пульт -> локальный SBUS + отправка по USB на ПК
; pio run -e cart_rx — приём по USB с ПК -> SBUS на драйвер колёс
; Роль публикуется в USB product string (CART-TX / CART-RX), по которой ПК-сервер
; (tools/bridge.py) находит нужный порт. Общий код (sbus, usb_link) собирается в
; оба env; специфичный — через build_src_filter, без мёртвого кода неактивной роли.
[env]
platform = ststm32 platform = ststm32
board = genericSTM32G431CB board = genericSTM32G431CB
framework = arduino framework = arduino
@@ -8,3 +15,18 @@ monitor_speed = 115200
build_flags = build_flags =
-DUSBCON -DUSBCON
-DUSBD_USE_CDC -DUSBD_USE_CDC
build_src_filter = -<*> +<sbus.cpp> +<usb_link.cpp>
[env:cart_tx]
build_flags =
${env.build_flags}
-DCART_ROLE_TX
-DUSB_PRODUCT_STRING="\"CART-TX\""
build_src_filter = ${env.build_src_filter} +<stick.cpp> +<app_tx.cpp>
[env:cart_rx]
build_flags =
${env.build_flags}
-DCART_ROLE_RX
-DUSB_PRODUCT_STRING="\"CART-RX\""
build_src_filter = ${env.build_src_filter} +<app_rx.cpp>
@@ -0,0 +1,70 @@
// Роль RX: принимает CONTROL по USB от ПК и гонит SBUS на провод драйвера так
// же, как если бы сигнал шёл с пульта. SBUS отправляется строго каждые 50 мс
// из последнего известного состояния — джиттер сети/USB не попадает на провод.
// FAILSAFE: если валидный CONTROL не приходил дольше RX_USB_TIMEOUT_MS, провод
// получает нейтраль (все каналы 1024) с флагом failsafe. Главный предохранитель
// живёт на самом STM и срабатывает даже при зависшем ПК или оборванном USB.
#ifdef CART_ROLE_RX
#include <Arduino.h>
#include "sbus.h"
#include "usb_link.h"
#include "device_id.h"
static uint16_t s_ch[16]; // последнее принятое состояние каналов
static uint8_t s_ctrlFlags = 0; // флаги последнего CONTROL (CTRL_FLAG_*)
static bool s_haveData = false;
static uint32_t s_lastGoodMs = 0;
static void replyIdent() {
LinkIdent id;
id.role = ROLE_RX;
id.proto_ver = LINK_VERSION;
id.fw_ver = CART_FW_VERSION;
cartReadUid(id.uid);
usbSendIdent(&id);
}
void setup() {
usbLinkInit();
sbusUartInit();
sbusNeutral(s_ch);
}
void loop() {
// приём кадров из USB
LinkRxEvent ev;
do {
usbLinkPoll(&ev);
if (ev.type == LINK_HELLO) {
replyIdent();
} else if (ev.type == LINK_CONTROL) {
memcpy(s_ch, ev.control.ch, sizeof(s_ch));
s_ctrlFlags = ev.control.flags;
s_lastGoodMs = millis();
s_haveData = true;
}
} while (ev.type != 0);
// выдача SBUS на провод строго по таймеру
static uint32_t next = 0;
uint32_t now = millis();
if (now < next)
return;
next = now + FRAME_PERIOD_MS;
bool stale = !s_haveData || (now - s_lastGoodMs > RX_USB_TIMEOUT_MS);
uint8_t frame[25];
if (stale) {
// нет свежих данных по USB -> нейтраль + явный failsafe на провод
uint16_t neutral[16];
sbusNeutral(neutral);
sbusPack(frame, neutral, SBUS_FLAG_FAILSAFE | SBUS_FLAG_FRAME_LOST);
} else {
// транслируем сетевой failsafe (CTRL_FLAG_FAILSAFE от ПК) в бит SBUS
uint8_t flags = (s_ctrlFlags & CTRL_FLAG_FAILSAFE) ? SBUS_FLAG_FAILSAFE : 0;
sbusPack(frame, s_ch, flags);
}
sbusSendFrame(frame);
}
#endif // CART_ROLE_RX
@@ -0,0 +1,59 @@
// Роль TX: к STM подключён пульт. Как и раньше, формирует SBUS на локальный
// провод (старый функционал не изменён), и ДОПОЛНИТЕЛЬНО шлёт те же значения
// каналов по USB на ПК кадром CONTROL. Отвечает на HELLO кадром IDENT.
#ifdef CART_ROLE_TX
#include <Arduino.h>
#include "sbus.h"
#include "stick.h"
#include "usb_link.h"
#include "device_id.h"
static void replyIdent() {
LinkIdent id;
id.role = ROLE_TX;
id.proto_ver = LINK_VERSION;
id.fw_ver = CART_FW_VERSION;
cartReadUid(id.uid);
usbSendIdent(&id);
}
void setup() {
usbLinkInit();
stickInit();
sbusUartInit();
}
void loop() {
// обслуживаем входящие кадры (ждём только HELLO от ПК)
LinkRxEvent ev;
do {
usbLinkPoll(&ev);
if (ev.type == LINK_HELLO)
replyIdent();
} while (ev.type != 0);
static uint32_t next = 0;
static uint32_t seq = 0;
uint32_t now = millis();
if (now < next)
return;
next = now + FRAME_PERIOD_MS;
uint16_t ch[16];
float fwd, right;
stickComputeChannels(ch, &fwd, &right);
// 1) локальный провод к драйверу — как раньше
uint8_t frame[25];
sbusPack(frame, ch);
sbusSendFrame(frame);
// 2) те же каналы по USB на ПК
LinkControl c;
c.seq = seq++;
c.t_ms = now;
c.flags = 0;
memcpy(c.ch, ch, sizeof(c.ch));
usbSendControl(&c);
}
#endif // CART_ROLE_TX
@@ -0,0 +1,9 @@
// 96-битный уникальный идентификатор чипа STM32G431 (UID base 0x1FFF7590).
// Используется в IDENT, чтобы ПК отличал конкретные экземпляры устройств.
#pragma once
#include <stdint.h>
#include <string.h>
static inline void cartReadUid(uint8_t uid[12]) {
memcpy(uid, (const void *)0x1FFF7590UL, 12);
}
@@ -0,0 +1,59 @@
// Единый источник истины для бинарного протокола связи пульт<->ПК<->тележка.
// Эти же числа зеркалятся в tools/link_proto.py — при любой правке меняй ОБА
// файла и сверяйся тест-векторами (tools/link_proto.py --selftest).
//
// Кадр поверх потока (USB CDC и полезная нагрузка UDP используют общий payload):
// [SYNC0][SYNC1][VER][TYPE][LEN][PAYLOAD..LEN][CRC16_LO][CRC16_HI]
// CRC16-CCITT (poly 0x1021, init 0xFFFF) по байтам [VER..конец PAYLOAD], LE.
#pragma once
#include <stdint.h>
// --- кадрирование ---
static const uint8_t LINK_SYNC0 = 0xA5;
static const uint8_t LINK_SYNC1 = 0x5A;
static const uint8_t LINK_VERSION = 1; // версия протокола, проверяется в handshake
static const uint8_t LINK_MAX_PAYLOAD = 64;
// --- типы кадров ---
enum LinkType : uint8_t {
LINK_HELLO = 0x03, // ПК -> STM: запрос идентификации (payload пуст)
LINK_IDENT = 0x02, // STM -> ПК: ответ на HELLO (LinkIdent)
LINK_CONTROL = 0x01, // поток управления в обе стороны (LinkControl)
LINK_LOG = 0x04, // STM -> ПК: отладочный ASCII-текст (вместо голого Serial)
};
// --- роли устройства (совпадают с продуктовой строкой USB) ---
enum LinkRole : uint8_t {
ROLE_TX = 'T', // к STM подключён пульт
ROLE_RX = 'R', // STM гонит SBUS на драйвер колёс
};
// --- флаги в LinkControl.flags ---
static const uint8_t CTRL_FLAG_FAILSAFE = 0x01; // источник объявил безопасный стоп
// Полезная нагрузка CONTROL (LE на проводе). Это общая «валюта» всей цепочки:
// TX кладёт сюда ровно те ch[], что уходят на его локальный провод, ПК
// перекладывает payload в UDP и обратно, RX распаковывает и пакует в SBUS.
// Пакуется/распаковывается вручную (без struct-паддинга) в link_pack.*.
struct LinkControl {
uint32_t seq; // монотонный счётчик кадров источника
uint32_t t_ms; // millis() на источнике (оценка задержки/джиттера)
uint8_t flags; // CTRL_FLAG_*
uint16_t ch[16]; // значения SBUS-каналов 0..2047
};
static const uint8_t LINK_CONTROL_LEN = 4 + 4 + 1 + 2 * 16; // 41 байт
// Полезная нагрузка IDENT.
struct LinkIdent {
uint8_t role; // LinkRole
uint8_t proto_ver; // LINK_VERSION на устройстве
uint16_t fw_ver; // версия прошивки
uint8_t uid[12]; // 96-битный UID чипа
};
static const uint8_t LINK_IDENT_LEN = 1 + 1 + 2 + 12; // 16 байт
static const uint16_t CART_FW_VERSION = 0x0100; // 1.00
// --- тайминги (мс) ---
static const uint32_t FRAME_PERIOD_MS = 50; // 20 Гц, темп старого пульта
static const uint32_t RX_USB_TIMEOUT_MS = 150; // нет CONTROL дольше -> нейтраль на проводе
-166
View File
@@ -1,166 +0,0 @@
// Пульт тележки: стик (АЦП PA0/PA1) -> SBUS на USART1 TX (PA9).
// Протокол: инвертированный SBUS, 100000 бод 8E2, 25 байт каждые 50 мс
// (см. docs/protocol.md). USB CDC (Serial) — отладочный вывод.
#include <Arduino.h>
// ---- калибровка стика (АЦП 12 бит, замерено 2026-08-20) ----
static const int X_FWD = 650, X_MID = 2014, X_BACK = 3378; // PA0 (плечи равны: 2014-650 = 3378-2014 = 1364)
static const int Y_RIGHT = 479, Y_MID = 1981, Y_LEFT = 3586; // PA1
static const int DEADZONE = 15; // отсечка дребезга вокруг нейтрали
// ---- SBUS-значения старого пульта ----
static const uint16_t SBUS_MID = 1024;
static const uint16_t CH2_MIN = 433, CH2_MAX = 1643; // назад..вперёд
static const uint16_t CH4_MIN = 446, CH4_MAX = 1654; // влево..вправо
// ---- expo: 0 = линейно, 1 = максимально мягкая нейтраль ----
static const float EXPO_K = 0.0f;
// ---- общий масштаб выхода: 1.0 = диапазон старого пульта ----
static const float RANGE_SCALE = 0.75f;
// симметричные плечи: вперёд и назад дают одинаковый максимум
static const uint16_t CH2_SPAN = 591; // min(1643-1024, 1024-433)
static const uint16_t CH4_SPAN = 578; // min(1654-1024, 1024-446)
// ---- профиль газа/поворота ----
static const float MOVE_START = 0.15f; // старт движения, доля хода стика
static const float POWER_FWD = 0.60f; // потолок «вперёд»
static const float POWER_BACK = 0.50f; // потолок «назад»
static const float POWER_TURN = 0.70f; // потолок поворота
static const uint16_t CH2_DB = 221; // мёртвая зона приёмника тележки (ЗАМЕРИТЬ по монитору)
static const uint16_t CH4_DB = 221;
static const uint32_t FRAME_PERIOD_MS = 50;
static const uint32_t PIN_X = PA0;
static const uint32_t PIN_Y = PA1;
static UART_HandleTypeDef s_sbusUart;
// USART1 TX = PA9 (AF7), 100000 бод, 8E2, TX инвертирован
static void sbusUartInit() {
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_USART1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {};
gpio.Pin = GPIO_PIN_9;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_LOW;
gpio.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &gpio);
s_sbusUart.Instance = USART1;
s_sbusUart.Init.BaudRate = 100000;
s_sbusUart.Init.WordLength = UART_WORDLENGTH_9B; // 8 данных + чётность
s_sbusUart.Init.StopBits = UART_STOPBITS_2;
s_sbusUart.Init.Parity = UART_PARITY_EVEN;
s_sbusUart.Init.Mode = UART_MODE_TX;
s_sbusUart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
s_sbusUart.Init.OverSampling = UART_OVERSAMPLING_16;
s_sbusUart.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_TXINVERT_INIT;
s_sbusUart.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE;
HAL_UART_Init(&s_sbusUart);
}
// нормализация одной оси в [-1..1] с мёртвой зоной и асимметричными плечами
static float axisNorm(int adc, int lowEnd, int mid, int highEnd) {
float x;
if (adc < mid - DEADZONE) {
x = (float)(mid - adc) / (float)(mid - lowEnd); // к lowEnd -> +1
} else if (adc > mid + DEADZONE) {
x = -(float)(adc - mid) / (float)(highEnd - mid); // к highEnd -> -1
} else {
return 0.0f;
}
return constrain(x, -1.0f, 1.0f);
}
// expo-кривая: гасит чувствительность у нейтрали, сохраняет края
static float expo(float x) {
return EXPO_K * x * x * x + (1.0f - EXPO_K) * x;
}
static uint16_t toSbus(float x, uint16_t span) {
return (uint16_t)(SBUS_MID + lroundf(x * span));
}
// стик [-1..1] -> SBUS-значение канала: ниже MOVE_START — нейтраль, выше —
// линейно от порога срабатывания приёмника (db) до POWER_FRAC*span на полном стике
static uint16_t driveToSbus(float x, uint16_t span, uint16_t db,
float powerPos, float powerNeg) {
float mag = fabsf(x);
if (mag <= MOVE_START)
return SBUS_MID;
float outMax = (x > 0.0f ? powerPos : powerNeg) * span;
float t = (mag - MOVE_START) / (1.0f - MOVE_START);
long delta = lroundf(db + t * (outMax - db));
return (x > 0.0f) ? (uint16_t)(SBUS_MID + delta)
: (uint16_t)(SBUS_MID - delta);
}
static void sbusPack(uint8_t out[25], const uint16_t ch[16]) {
out[0] = 0x0F;
memset(out + 1, 0, 22);
uint32_t bitpos = 0;
for (int n = 0; n < 16; n++) {
for (int b = 0; b < 11; b++) {
if (ch[n] & (1u << b))
out[1 + (bitpos >> 3)] |= 1u << (bitpos & 7);
bitpos++;
}
}
out[23] = 0x00; // флаги
out[24] = 0x00; // футер
}
static int readAvg(uint32_t pin) {
uint32_t acc = 0;
for (int i = 0; i < 16; i++)
acc += analogRead(pin);
return acc / 16;
}
void setup() {
Serial.begin(115200);
analogReadResolution(12);
pinMode(PIN_X, INPUT_ANALOG);
pinMode(PIN_Y, INPUT_ANALOG);
sbusUartInit();
}
void loop() {
static uint32_t next = 0;
uint32_t now = millis();
if (now < next)
return;
next = now + FRAME_PERIOD_MS;
int adcX = readAvg(PIN_X);
int adcY = readAvg(PIN_Y);
// вперёд = adcX к X_FWD (вниз) -> +1; вправо = adcY к Y_RIGHT -> +1
float fwd = axisNorm(adcX, X_FWD, X_MID, X_BACK);
float right = axisNorm(adcY, Y_RIGHT, Y_MID, Y_LEFT);
uint16_t ch[16];
for (int i = 0; i < 16; i++)
ch[i] = SBUS_MID;
ch[1] = driveToSbus(fwd, CH2_SPAN, CH2_DB, POWER_FWD, POWER_BACK); // канал 2 — газ
ch[3] = driveToSbus(right, CH4_SPAN, CH4_DB, POWER_TURN, POWER_TURN); // канал 4 — поворот
uint8_t frame[25];
sbusPack(frame, ch);
HAL_UART_Transmit(&s_sbusUart, frame, sizeof(frame), 20);
Serial.print("adc=");
Serial.print(adcX);
Serial.print(",");
Serial.print(adcY);
Serial.print(" fwd=");
Serial.print(fwd, 3);
Serial.print(" right=");
Serial.print(right, 3);
Serial.print(" ch2=");
Serial.print(ch[1]);
Serial.print(" ch4=");
Serial.println(ch[3]);
}
@@ -0,0 +1,55 @@
#include "sbus.h"
#include <Arduino.h>
#include <string.h>
static UART_HandleTypeDef s_sbusUart;
void sbusNeutral(uint16_t ch[16]) {
for (int i = 0; i < 16; i++)
ch[i] = SBUS_MID;
}
void sbusPack(uint8_t out[25], const uint16_t ch[16], uint8_t flags) {
out[0] = 0x0F;
memset(out + 1, 0, 22);
uint32_t bitpos = 0;
for (int n = 0; n < 16; n++) {
for (int b = 0; b < 11; b++) {
if (ch[n] & (1u << b))
out[1 + (bitpos >> 3)] |= 1u << (bitpos & 7);
bitpos++;
}
}
out[23] = flags; // флаги: bit2=frame_lost, bit3=failsafe
out[24] = 0x00; // футер
}
// USART1 TX = PA9 (AF7), 100000 бод, 8E2, TX инвертирован
void sbusUartInit() {
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_USART1_CLK_ENABLE();
GPIO_InitTypeDef gpio = {};
gpio.Pin = GPIO_PIN_9;
gpio.Mode = GPIO_MODE_AF_PP;
gpio.Pull = GPIO_NOPULL;
gpio.Speed = GPIO_SPEED_FREQ_LOW;
gpio.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &gpio);
s_sbusUart.Instance = USART1;
s_sbusUart.Init.BaudRate = 100000;
s_sbusUart.Init.WordLength = UART_WORDLENGTH_9B; // 8 данных + чётность
s_sbusUart.Init.StopBits = UART_STOPBITS_2;
s_sbusUart.Init.Parity = UART_PARITY_EVEN;
s_sbusUart.Init.Mode = UART_MODE_TX;
s_sbusUart.Init.HwFlowCtl = UART_HWCONTROL_NONE;
s_sbusUart.Init.OverSampling = UART_OVERSAMPLING_16;
s_sbusUart.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_TXINVERT_INIT;
s_sbusUart.AdvancedInit.TxPinLevelInvert = UART_ADVFEATURE_TXINV_ENABLE;
HAL_UART_Init(&s_sbusUart);
}
void sbusSendFrame(const uint8_t frame[25]) {
HAL_UART_Transmit(&s_sbusUart, (uint8_t *)frame, 25, 20);
}
+25
View File
@@ -0,0 +1,25 @@
// Общий SBUS-слой: упаковка кадра и вывод инвертированного SBUS на провод
// (USART1 TX = PA9, 100000 бод, 8E2, TXINV). Единственный владелец всего, что
// идёт на драйвер колёс — используется и TX (локальный провод), и RX.
// См. docs/protocol.md.
#pragma once
#include <stdint.h>
static const uint16_t SBUS_MID = 1024; // нейтраль всех каналов
// Флаги в байте 23 SBUS-кадра (см. docs/protocol.md).
static const uint8_t SBUS_FLAG_FRAME_LOST = 0x04; // bit2
static const uint8_t SBUS_FLAG_FAILSAFE = 0x08; // bit3
// Заполнить ch[16] нейтралью (все каналы 1024).
void sbusNeutral(uint16_t ch[16]);
// Упаковать 16 каналов (0..2047) в стандартный 25-байтовый SBUS-кадр.
// flags идёт в байт 23 (bit2=frame_lost, bit3=failsafe).
void sbusPack(uint8_t out[25], const uint16_t ch[16], uint8_t flags = 0);
// Инициализация USART1 для вывода инвертированного SBUS.
void sbusUartInit();
// Передать готовый 25-байтовый кадр на провод (блокирующе, таймаут 20 мс).
void sbusSendFrame(const uint8_t frame[25]);
@@ -0,0 +1,80 @@
#include "stick.h"
#include "sbus.h"
#include <Arduino.h>
// ---- калибровка стика (АЦП 12 бит, замерено 2026-08-20) ----
static const int X_FWD = 650, X_MID = 2014, X_BACK = 3378; // PA0 (плечи равны: 2014-650 = 3378-2014 = 1364)
static const int Y_RIGHT = 479, Y_MID = 1981, Y_LEFT = 3586; // PA1
static const int DEADZONE = 15; // отсечка дребезга вокруг нейтрали
// симметричные плечи: вперёд и назад дают одинаковый максимум
static const uint16_t CH2_SPAN = 591; // min(1643-1024, 1024-433)
static const uint16_t CH4_SPAN = 578; // min(1654-1024, 1024-446)
// ---- профиль газа/поворота ----
static const float MOVE_START = 0.15f; // старт движения, доля хода стика
static const float POWER_FWD = 0.60f; // потолок «вперёд»
static const float POWER_BACK = 0.50f; // потолок «назад»
static const float POWER_TURN = 0.70f; // потолок поворота
static const uint16_t CH2_DB = 221; // мёртвая зона приёмника тележки
static const uint16_t CH4_DB = 221;
static const uint32_t PIN_X = PA0;
static const uint32_t PIN_Y = PA1;
// нормализация одной оси в [-1..1] с мёртвой зоной и асимметричными плечами
static float axisNorm(int adc, int lowEnd, int mid, int highEnd) {
float x;
if (adc < mid - DEADZONE) {
x = (float)(mid - adc) / (float)(mid - lowEnd); // к lowEnd -> +1
} else if (adc > mid + DEADZONE) {
x = -(float)(adc - mid) / (float)(highEnd - mid); // к highEnd -> -1
} else {
return 0.0f;
}
return constrain(x, -1.0f, 1.0f);
}
// стик [-1..1] -> SBUS-значение канала: ниже MOVE_START — нейтраль, выше —
// линейно от порога срабатывания приёмника (db) до POWER_FRAC*span на полном стике
static uint16_t driveToSbus(float x, uint16_t span, uint16_t db,
float powerPos, float powerNeg) {
float mag = fabsf(x);
if (mag <= MOVE_START)
return SBUS_MID;
float outMax = (x > 0.0f ? powerPos : powerNeg) * span;
float t = (mag - MOVE_START) / (1.0f - MOVE_START);
long delta = lroundf(db + t * (outMax - db));
return (x > 0.0f) ? (uint16_t)(SBUS_MID + delta)
: (uint16_t)(SBUS_MID - delta);
}
static int readAvg(uint32_t pin) {
uint32_t acc = 0;
for (int i = 0; i < 16; i++)
acc += analogRead(pin);
return acc / 16;
}
void stickInit() {
analogReadResolution(12);
pinMode(PIN_X, INPUT_ANALOG);
pinMode(PIN_Y, INPUT_ANALOG);
}
void stickComputeChannels(uint16_t ch[16], float *fwdOut, float *rightOut) {
int adcX = readAvg(PIN_X);
int adcY = readAvg(PIN_Y);
// вперёд = adcX к X_FWD (вниз) -> +1; вправо = adcY к Y_RIGHT -> +1
float fwd = axisNorm(adcX, X_FWD, X_MID, X_BACK);
float right = axisNorm(adcY, Y_RIGHT, Y_MID, Y_LEFT);
sbusNeutral(ch);
ch[1] = driveToSbus(fwd, CH2_SPAN, CH2_DB, POWER_FWD, POWER_BACK); // канал 2 — газ
ch[3] = driveToSbus(right, CH4_SPAN, CH4_DB, POWER_TURN, POWER_TURN); // канал 4 — поворот
if (fwdOut)
*fwdOut = fwd;
if (rightOut)
*rightOut = right;
}
+12
View File
@@ -0,0 +1,12 @@
// Пульт: чтение стика (АЦП PA0/PA1), калибровка, профиль газа/поворота и
// формирование значений SBUS-каналов. Вся «логика пульта» живёт здесь.
// Компилируется только для роли TX (см. build_src_filter в platformio.ini).
#pragma once
#include <stdint.h>
// Инициализация АЦП-входов стика.
void stickInit();
// Прочитать стик и заполнить ch[16] значениями SBUS-каналов (нейтраль + ch2/ch4).
// Возвращает по ссылкам нормализованные оси для отладки.
void stickComputeChannels(uint16_t ch[16], float *fwdOut, float *rightOut);
@@ -0,0 +1,148 @@
#include "usb_link.h"
#include <Arduino.h>
#include <string.h>
uint16_t linkCrc16(const uint8_t *data, uint16_t len) {
uint16_t crc = 0xFFFF;
for (uint16_t i = 0; i < len; i++) {
crc ^= (uint16_t)data[i] << 8;
for (int b = 0; b < 8; b++)
crc = (crc & 0x8000) ? (uint16_t)((crc << 1) ^ 0x1021) : (uint16_t)(crc << 1);
}
return crc;
}
// ---- сериализация payload (little-endian, без struct-паддинга) ----
static void put_u16(uint8_t *p, uint16_t v) { p[0] = v & 0xFF; p[1] = v >> 8; }
static void put_u32(uint8_t *p, uint32_t v) {
p[0] = v & 0xFF; p[1] = (v >> 8) & 0xFF; p[2] = (v >> 16) & 0xFF; p[3] = (v >> 24) & 0xFF;
}
static uint16_t get_u16(const uint8_t *p) { return (uint16_t)p[0] | ((uint16_t)p[1] << 8); }
static uint32_t get_u32(const uint8_t *p) {
return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}
static uint8_t packControl(const LinkControl *c, uint8_t *out) {
put_u32(out + 0, c->seq);
put_u32(out + 4, c->t_ms);
out[8] = c->flags;
for (int i = 0; i < 16; i++)
put_u16(out + 9 + 2 * i, c->ch[i]);
return LINK_CONTROL_LEN;
}
static void unpackControl(const uint8_t *in, LinkControl *c) {
c->seq = get_u32(in + 0);
c->t_ms = get_u32(in + 4);
c->flags = in[8];
for (int i = 0; i < 16; i++)
c->ch[i] = get_u16(in + 9 + 2 * i);
}
// ---- отправка кадра ----
static void sendFrame(uint8_t type, const uint8_t *payload, uint8_t len) {
uint8_t buf[3 + LINK_MAX_PAYLOAD + 2];
buf[0] = LINK_VERSION;
buf[1] = type;
buf[2] = len;
if (len)
memcpy(buf + 3, payload, len);
uint16_t crc = linkCrc16(buf, 3 + len);
uint8_t hdr[2] = {LINK_SYNC0, LINK_SYNC1};
Serial.write(hdr, 2);
Serial.write(buf, 3 + len);
uint8_t crcb[2] = {(uint8_t)(crc & 0xFF), (uint8_t)(crc >> 8)};
Serial.write(crcb, 2);
}
void usbSendControl(const LinkControl *c) {
uint8_t pl[LINK_CONTROL_LEN];
uint8_t n = packControl(c, pl);
sendFrame(LINK_CONTROL, pl, n);
}
void usbSendIdent(const LinkIdent *id) {
uint8_t pl[LINK_IDENT_LEN];
pl[0] = id->role;
pl[1] = id->proto_ver;
put_u16(pl + 2, id->fw_ver);
memcpy(pl + 4, id->uid, 12);
sendFrame(LINK_IDENT, pl, LINK_IDENT_LEN);
}
void usbSendLog(const char *msg) {
uint8_t len = (uint8_t)strnlen(msg, LINK_MAX_PAYLOAD);
sendFrame(LINK_LOG, (const uint8_t *)msg, len);
}
void usbLinkInit() {
Serial.begin(115200);
}
// ---- стейт-машина приёма ----
enum RxState { WAIT_S0, WAIT_S1, READ_HDR, READ_PAYLOAD, READ_CRC };
static RxState s_state = WAIT_S0;
static uint8_t s_hdr[3]; // VER, TYPE, LEN
static uint8_t s_hdrGot;
static uint8_t s_payload[LINK_MAX_PAYLOAD];
static uint8_t s_payGot;
static uint8_t s_crc[2];
static uint8_t s_crcGot;
static void resync() { s_state = WAIT_S0; s_hdrGot = s_payGot = s_crcGot = 0; }
void usbLinkPoll(LinkRxEvent *ev) {
ev->type = 0;
while (Serial.available() > 0) {
uint8_t b = (uint8_t)Serial.read();
switch (s_state) {
case WAIT_S0:
if (b == LINK_SYNC0) s_state = WAIT_S1;
break;
case WAIT_S1:
s_state = (b == LINK_SYNC1) ? READ_HDR : WAIT_S0;
s_hdrGot = 0;
break;
case READ_HDR:
s_hdr[s_hdrGot++] = b;
if (s_hdrGot == 3) {
// s_hdr: VER, TYPE, LEN
if (s_hdr[0] != LINK_VERSION || s_hdr[2] > LINK_MAX_PAYLOAD) {
resync(); // неверная версия/длина — сдвиг и поиск заново
break;
}
s_payGot = 0;
s_state = (s_hdr[2] == 0) ? READ_CRC : READ_PAYLOAD;
}
break;
case READ_PAYLOAD:
s_payload[s_payGot++] = b;
if (s_payGot == s_hdr[2])
s_state = READ_CRC;
break;
case READ_CRC:
s_crc[s_crcGot++] = b;
if (s_crcGot == 2) {
uint8_t chk[3 + LINK_MAX_PAYLOAD];
chk[0] = s_hdr[0]; chk[1] = s_hdr[1]; chk[2] = s_hdr[2];
memcpy(chk + 3, s_payload, s_hdr[2]);
uint16_t want = (uint16_t)s_crc[0] | ((uint16_t)s_crc[1] << 8);
uint16_t got = linkCrc16(chk, 3 + s_hdr[2]);
uint8_t type = s_hdr[1];
uint8_t len = s_hdr[2];
resync();
if (want == got) {
if (type == LINK_CONTROL && len == LINK_CONTROL_LEN) {
unpackControl(s_payload, &ev->control);
ev->type = LINK_CONTROL;
return;
} else if (type == LINK_HELLO || type == LINK_IDENT || type == LINK_LOG) {
ev->type = type;
return;
}
}
// битый CRC или неизвестный тип — молча ищем следующий кадр
}
break;
}
}
}
@@ -0,0 +1,23 @@
// Бинарный протокол поверх USB CDC (Serial): кадрирование, CRC16, сериализация
// CONTROL/IDENT/LOG и стейт-машина приёма с ресинхронизацией. Общий для TX и RX.
#pragma once
#include "link_protocol.h"
// Результат опроса входящего потока.
struct LinkRxEvent {
uint8_t type; // 0 = в этот вызов целого кадра нет; иначе LinkType
LinkControl control; // валидно при type == LINK_CONTROL
};
void usbLinkInit();
// Разобрать доступные байты. Возвращает максимум один полный валидный кадр за
// вызов (type != 0). Вызывать в цикле, пока event.type != 0, затем дальше.
void usbLinkPoll(LinkRxEvent *ev);
void usbSendControl(const LinkControl *c);
void usbSendIdent(const LinkIdent *id);
void usbSendLog(const char *msg);
// Общий CRC16-CCITT (poly 0x1021, init 0xFFFF). Экспортирован для тестов.
uint16_t linkCrc16(const uint8_t *data, uint16_t len);
@@ -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__)