Compare commits
9
Commits
main
...
cart_net_bridge
| Author | SHA1 | Date | |
|---|---|---|---|
|
|
5826c4ff67 | ||
|
|
09a2aa7932 | ||
|
|
75185c7e36 | ||
|
|
5b70a9a4ce | ||
|
|
7334f89491 | ||
|
|
50f07caa64 | ||
|
|
8704db2de7 | ||
|
|
bd9395fc3d | ||
|
|
6f1eaf19e4 |
Vendored
+3
@@ -0,0 +1,3 @@
|
||||
{
|
||||
"cmake.sourceDirectory": "/home/awe/Documents/radar_system/data_acq_and_processing/gpr_cpp"
|
||||
}
|
||||
+2
-1
@@ -5,6 +5,7 @@
|
||||
"platformio.platformio-ide"
|
||||
],
|
||||
"unwantedRecommendations": [
|
||||
"ms-vscode.cpptools-extension-pack"
|
||||
"ms-vscode.cpptools-extension-pack",
|
||||
"pioarduino.pioarduino-ide"
|
||||
]
|
||||
}
|
||||
|
||||
@@ -0,0 +1,53 @@
|
||||
# systemd-службы сетевого моста
|
||||
|
||||
Автозапуск `tools/bridge.py` как служб: `cart-bridge-tx` (отправитель, рядом с
|
||||
пультом) и `cart-bridge-rx` (приёмник, рядом с тележкой). На каждой машине
|
||||
включается ровно одна роль.
|
||||
|
||||
## Установка
|
||||
|
||||
На машине-отправителе:
|
||||
|
||||
```sh
|
||||
sudo ./install-bridge.sh tx
|
||||
```
|
||||
|
||||
На машине-приёмнике:
|
||||
|
||||
```sh
|
||||
sudo ./install-bridge.sh rx
|
||||
```
|
||||
|
||||
Установщик подставляет в юнит реальные пути (python из `.venv` проекта, если
|
||||
есть, иначе системный `python3`; `bridge.py`; рабочий каталог) и пользователя
|
||||
(того, кто вызвал `sudo`). Юниты кладутся в `/etc/systemd/system/`, конфиги —
|
||||
в `/etc/cart-bridge/{tx,rx}.env` (создаются из примеров, существующие не
|
||||
перезаписываются). Без аргумента (`none`) — только установка, без включения.
|
||||
|
||||
Интерпретатор можно задать явно: `sudo PYTHON=/usr/bin/python3 ./install-bridge.sh rx`.
|
||||
В выбранном python должен быть `pyserial`.
|
||||
|
||||
## Настройка
|
||||
|
||||
Отредактировать конфиг под свою сеть и ключ:
|
||||
|
||||
```sh
|
||||
sudoedit /etc/cart-bridge/tx.env # peer = адрес приёмника, общий ключ
|
||||
sudoedit /etc/cart-bridge/rx.env # bind, тот же ключ
|
||||
sudo systemctl restart cart-bridge-tx # применить
|
||||
```
|
||||
|
||||
Ключ `CART_BRIDGE_KEY` на обеих сторонах должен совпадать (HMAC-аутентификация
|
||||
пакетов; обязателен для интернета).
|
||||
|
||||
## Эксплуатация
|
||||
|
||||
```sh
|
||||
systemctl status cart-bridge-tx
|
||||
journalctl -u cart-bridge-tx -f # логи; на отправителе видно «приёмник отвечает/НЕ отвечает»
|
||||
sudo systemctl restart cart-bridge-tx
|
||||
sudo systemctl disable --now cart-bridge-tx
|
||||
```
|
||||
|
||||
Служба перезапускается при падении (`Restart=always`). Доступ к USB-порту STM
|
||||
даётся группами `dialout`/`plugdev` (прописаны в юните).
|
||||
@@ -0,0 +1,19 @@
|
||||
# Конфигурация службы приёмника (cart-bridge-rx). Копируется в
|
||||
# /etc/cart-bridge/rx.env установщиком. Переменные читает tools/bridge.py.
|
||||
|
||||
# Локальный UDP-адрес для приёма HOST:PORT.
|
||||
CART_BRIDGE_BIND=0.0.0.0:5005
|
||||
|
||||
# Общий ключ HMAC (ОБЯЗАТЕЛЕН для интернета). Должен совпадать с отправителем.
|
||||
CART_BRIDGE_KEY=change-me-shared-secret
|
||||
|
||||
# Необязательно: принимать пакеты только с этого адреса отправителя (фильтр по
|
||||
# хосту). Для интернета за NAT обычно оставляют пустым и полагаются на HMAC.
|
||||
#CART_BRIDGE_PEER=10.0.0.1
|
||||
|
||||
# Необязательно: путь к порту или подстрока product/serial. По умолчанию порт
|
||||
# ищется по USB product string CART-RX.
|
||||
#CART_BRIDGE_DEVICE=CART-RX
|
||||
|
||||
# Необязательно: уровень логов (DEBUG/INFO/WARNING).
|
||||
#CART_BRIDGE_LOG_LEVEL=INFO
|
||||
@@ -0,0 +1,18 @@
|
||||
[Unit]
|
||||
Description=Cart bridge receiver (role rx): сеть -> USB тележки
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
StartLimitIntervalSec=0
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=@USER@
|
||||
SupplementaryGroups=dialout plugdev
|
||||
EnvironmentFile=/etc/cart-bridge/rx.env
|
||||
WorkingDirectory=@WORKDIR@
|
||||
ExecStart=@PYTHON@ @BRIDGE@ --role rx
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
@@ -0,0 +1,15 @@
|
||||
# Конфигурация службы отправителя (cart-bridge-tx). Копируется в
|
||||
# /etc/cart-bridge/tx.env установщиком. Переменные читает tools/bridge.py.
|
||||
|
||||
# Адрес приёмника HOST:PORT — куда слать управление.
|
||||
CART_BRIDGE_PEER=10.0.0.2:5005
|
||||
|
||||
# Общий ключ HMAC (ОБЯЗАТЕЛЕН для интернета). Должен совпадать с приёмником.
|
||||
CART_BRIDGE_KEY=change-me-shared-secret
|
||||
|
||||
# Необязательно: путь к порту или подстрока product/serial. По умолчанию порт
|
||||
# ищется по USB product string CART-TX.
|
||||
#CART_BRIDGE_DEVICE=CART-TX
|
||||
|
||||
# Необязательно: уровень логов (DEBUG/INFO/WARNING).
|
||||
#CART_BRIDGE_LOG_LEVEL=INFO
|
||||
@@ -0,0 +1,20 @@
|
||||
[Unit]
|
||||
Description=Cart bridge sender (role tx): пульт USB -> сеть
|
||||
After=network-online.target
|
||||
Wants=network-online.target
|
||||
# не упираться в лимит перезапусков при флапе сети/USB
|
||||
StartLimitIntervalSec=0
|
||||
|
||||
[Service]
|
||||
Type=simple
|
||||
User=@USER@
|
||||
# доступ к последовательному порту STM (dialout) и к USB-дескрипторам (plugdev)
|
||||
SupplementaryGroups=dialout plugdev
|
||||
EnvironmentFile=/etc/cart-bridge/tx.env
|
||||
WorkingDirectory=@WORKDIR@
|
||||
ExecStart=@PYTHON@ @BRIDGE@ --role tx
|
||||
Restart=always
|
||||
RestartSec=2
|
||||
|
||||
[Install]
|
||||
WantedBy=multi-user.target
|
||||
+79
@@ -0,0 +1,79 @@
|
||||
#!/usr/bin/env bash
|
||||
# Установка systemd-служб сетевого моста тележки.
|
||||
#
|
||||
# sudo ./install-bridge.sh [tx|rx|both|none]
|
||||
#
|
||||
# Аргумент выбирает, какую службу включить и запустить на ЭТОЙ машине
|
||||
# (tx — рядом с пультом, rx — рядом с тележкой). Без аргумента юниты только
|
||||
# ставятся, но не включаются. Файлы юнитов и шаблоны конфигов берутся из этого
|
||||
# каталога; пути к python и bridge.py подставляются автоматически.
|
||||
set -euo pipefail
|
||||
|
||||
SELF="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
TOOLS="$(cd "$SELF/../tools" && pwd)"
|
||||
REPO="$(cd "$SELF/../../.." && pwd)"
|
||||
BRIDGE="$TOOLS/bridge.py"
|
||||
WORKDIR="$TOOLS"
|
||||
ENABLE="${1:-none}"
|
||||
|
||||
case "$ENABLE" in tx|rx|both|none) ;; *) echo "аргумент: tx|rx|both|none" >&2; exit 2;; esac
|
||||
|
||||
if [ "$(id -u)" -ne 0 ]; then
|
||||
echo "нужны права root — перезапускаюсь через sudo"
|
||||
exec sudo -E PYTHON="${PYTHON:-}" bash "$0" "$ENABLE"
|
||||
fi
|
||||
|
||||
TARGET_USER="${SUDO_USER:-root}"
|
||||
|
||||
# интерпретатор: venv проекта, если есть, иначе системный python3
|
||||
PYTHON="${PYTHON:-}"
|
||||
if [ -z "$PYTHON" ]; then
|
||||
if [ -x "$REPO/.venv/bin/python" ]; then
|
||||
PYTHON="$REPO/.venv/bin/python"
|
||||
else
|
||||
PYTHON="$(command -v python3)"
|
||||
fi
|
||||
fi
|
||||
|
||||
echo "python : $PYTHON"
|
||||
echo "bridge : $BRIDGE"
|
||||
echo "user : $TARGET_USER"
|
||||
|
||||
if ! "$PYTHON" -c 'import serial' 2>/dev/null; then
|
||||
echo "ВНИМАНИЕ: в $PYTHON нет pyserial — поставь: $PYTHON -m pip install pyserial" >&2
|
||||
fi
|
||||
|
||||
gen_unit() { # <role>
|
||||
local role="$1"
|
||||
sed -e "s|@PYTHON@|$PYTHON|g" -e "s|@BRIDGE@|$BRIDGE|g" \
|
||||
-e "s|@WORKDIR@|$WORKDIR|g" -e "s|@USER@|$TARGET_USER|g" \
|
||||
"$SELF/cart-bridge-$role.service" > "/etc/systemd/system/cart-bridge-$role.service"
|
||||
echo "установлен юнит /etc/systemd/system/cart-bridge-$role.service"
|
||||
}
|
||||
|
||||
install -d -m 755 /etc/cart-bridge
|
||||
for role in tx rx; do
|
||||
gen_unit "$role"
|
||||
conf="/etc/cart-bridge/$role.env"
|
||||
if [ ! -e "$conf" ]; then
|
||||
install -m 600 "$SELF/cart-bridge-$role.env.example" "$conf"
|
||||
echo "создан конфиг $conf (ОТРЕДАКТИРУЙ: peer/bind и ключ)"
|
||||
else
|
||||
echo "конфиг $conf уже есть — не трогаю"
|
||||
fi
|
||||
done
|
||||
|
||||
systemctl daemon-reload
|
||||
|
||||
enable_one() { # <role>
|
||||
systemctl enable --now "cart-bridge-$1.service"
|
||||
echo "включена и запущена cart-bridge-$1.service"
|
||||
}
|
||||
case "$ENABLE" in
|
||||
tx) enable_one tx ;;
|
||||
rx) enable_one rx ;;
|
||||
both) enable_one tx; enable_one rx ;;
|
||||
none) echo "юниты установлены, не включены. Включить: systemctl enable --now cart-bridge-tx (или -rx)";;
|
||||
esac
|
||||
|
||||
echo "готово. Логи: journalctl -u cart-bridge-tx -f (или -rx)"
|
||||
@@ -55,3 +55,187 @@
|
||||
- Отправлять 25-байтовый кадр по таймеру каждые 50 мс.
|
||||
- Нейтраль: все каналы 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) различает экземпляры.
|
||||
|
||||
## Прошивка STM
|
||||
|
||||
Плата — WeAct STM32G431CB. Нужен PlatformIO Core (`pio`); если не установлен:
|
||||
`pip install platformio` (в репозитории он лежит в `~/.platformio/penv/bin/pio`).
|
||||
Все команды выполняются из каталога прошивки `device_firmware/cart_firmware`.
|
||||
|
||||
Две роли — две отдельные прошивки на две платы. По умолчанию загрузчик — ST-Link
|
||||
по SWD (`upload_protocol = stlink` в `platformio.ini`).
|
||||
|
||||
**1. Подключить ST-Link к плате по SWD** (4 провода):
|
||||
|
||||
| ST-Link | WeAct G431CB |
|
||||
|---|---|
|
||||
| SWDIO | PA13 (SWDIO) |
|
||||
| SWCLK | PA14 (SWCLK) |
|
||||
| GND | GND |
|
||||
| 3V3 | 3V3 |
|
||||
|
||||
**2. Собрать и прошить роль** (прошивать по одной плате за раз, один ST-Link):
|
||||
|
||||
```
|
||||
# плата-передатчик (к ней подключён пульт)
|
||||
pio run -e cart_tx -t upload
|
||||
# плата-приёмник (к ней подключён провод драйвера колёс)
|
||||
pio run -e cart_rx -t upload
|
||||
```
|
||||
|
||||
- Только собрать без заливки: `pio run -e cart_tx`. Артефакты —
|
||||
`.pio/build/<env>/firmware.{elf,bin}`.
|
||||
- Если подключено несколько ST-Link, указать конкретный:
|
||||
`pio run -e cart_tx -t upload --upload-port <serial-ST-Link>`.
|
||||
- Сменить роль платы — просто залить другой env, флеш перезаписывается целиком.
|
||||
|
||||
**3. Физические подключения по ролям** (распиновка общая, см. выше по документу):
|
||||
|
||||
| | CART-TX | CART-RX |
|
||||
|---|---|---|
|
||||
| Стик X / Y | PA0 / PA1 (АЦП) | — не используется |
|
||||
| Выход SBUS | PA9 → локальный драйвер (как раньше) | PA9 → провод драйвера колёс |
|
||||
| USB | → ПК (bridge --role tx) | → ПК (bridge --role rx) |
|
||||
|
||||
**4. Проверить роль после прошивки.** Подключить плату по USB к ПК и прочитать
|
||||
product string (роль зашита в неё):
|
||||
|
||||
```
|
||||
python3 -c "from serial.tools import list_ports; \
|
||||
[print(p.device, p.product, p.serial_number) for p in list_ports.comports()]"
|
||||
# ожидаемо: /dev/ttyACM0 CART-TX <UID> либо CART-RX
|
||||
```
|
||||
|
||||
Либо просто запустить `tools/bridge.py` соответствующей роли — на старте он
|
||||
делает handshake и логирует подтверждённые роль, версию прошивки и UID.
|
||||
|
||||
> USB CDC теперь несёт **бинарный** протокол, а не текстовый лог. `pio device
|
||||
> monitor` покажет «мусор» — это нормально. Отладочный текст идёт кадрами `LOG`,
|
||||
> которые печатает `bridge.py`.
|
||||
|
||||
## Кадр поверх 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 допустим только в доверенной локальной сети.
|
||||
- **Обратный heartbeat.** RX шлёт на адрес источника последнего CONTROL
|
||||
маленький служебный пакет (~5 раз/с): magic `CB 01`, флаг собственного
|
||||
failsafe, последний принятый seq; при включённом ключе — тоже под HMAC. TX
|
||||
принимает их на тот же сокет (сквозь NAT проходит, т.к. он сам инициировал
|
||||
поток) и показывает в логе, **отвечает ли приёмник** и не ушёл ли тот в
|
||||
failsafe. Это чисто служебный PC-трафик, STM в нём не участвует, поэтому он не
|
||||
в зеркале протокола, а в самом `bridge.py`. На выдачу SBUS и на основной поток
|
||||
управления heartbeat не влияет.
|
||||
- **Индикация для оператора.** Если запустить tx в терминале, он показывает одну
|
||||
живую строку, обновляемую на месте: зелёное `● СВЯЗЬ ЕСТЬ`, жёлтое
|
||||
`● СВЯЗЬ ЕСТЬ (приёмник в failsafe)` или красное `● СВЯЗИ НЕТ`. Под systemd
|
||||
(вывод не в терминал) вместо строки идут события в журнал по смене состояния.
|
||||
|
||||
## 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.
|
||||
|
||||
Параметры можно задавать через env (`CART_BRIDGE_ROLE/PEER/BIND/DEVICE/KEY/
|
||||
LOG_LEVEL`), CLI их переопределяет.
|
||||
|
||||
## Автозапуск: systemd-службы
|
||||
|
||||
Для постоянной работы мост ставится службами: `cart-bridge-tx` (отправитель) и
|
||||
`cart-bridge-rx` (приёмник), по одной роли на машину. Юниты, примеры конфигов и
|
||||
установщик — в каталоге `deploy/`:
|
||||
|
||||
```sh
|
||||
cd deploy
|
||||
sudo ./install-bridge.sh tx # на машине у пульта
|
||||
sudo ./install-bridge.sh rx # на машине у тележки
|
||||
```
|
||||
|
||||
Установщик подставляет реальные пути (python из `.venv`, `bridge.py`, рабочий
|
||||
каталог) и пользователя, кладёт конфиги в `/etc/cart-bridge/{tx,rx}.env`. Там
|
||||
задаются `CART_BRIDGE_PEER`/`BIND` и общий `CART_BRIDGE_KEY` (должен совпадать на
|
||||
обеих сторонах). Логи и индикация «отвечает ли приёмник» — `journalctl -u
|
||||
cart-bridge-tx -f`. Подробности — `deploy/README.md`.
|
||||
|
||||
@@ -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
|
||||
board = genericSTM32G431CB
|
||||
framework = arduino
|
||||
@@ -8,3 +15,18 @@ monitor_speed = 115200
|
||||
build_flags =
|
||||
-DUSBCON
|
||||
-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 = 450; // нет CONTROL дольше -> нейтраль на проводе
|
||||
@@ -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);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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,379 @@
|
||||
#!/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).
|
||||
|
||||
Обратный канал: rx шлёт tx heartbeat, поэтому на стороне отправителя видно,
|
||||
отвечает ли приёмник и не ушёл ли он сам в failsafe.
|
||||
|
||||
Устройство выбирается по USB product string (CART-TX / CART-RX) и подтверждается
|
||||
handshake-кадром IDENT (роль + версия протокола). Для интернета используй общий
|
||||
ключ (--key) — каждый UDP-пакет аутентифицируется HMAC-SHA256; без ключа трафик
|
||||
идёт в открытом виде (только для доверенной локальной сети).
|
||||
|
||||
Параметры можно задавать и через env (CART_BRIDGE_ROLE/PEER/BIND/DEVICE/KEY/
|
||||
LOG_LEVEL) — так настраиваются systemd-службы (см. deploy/).
|
||||
|
||||
Запуск (из каталога прошивки):
|
||||
.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 os
|
||||
import select
|
||||
import socket
|
||||
import struct
|
||||
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.50 # нет UDP дольше -> rx начинает слать failsafe
|
||||
HELLO_TIMEOUT_S = 1.0
|
||||
SEQ_RESET_AFTER = 20 # столько подряд «несвежих» пакетов = перезапуск источника
|
||||
|
||||
# Обратный канал «жив ли приёмник»: rx шлёт heartbeat в сторону tx (на адрес
|
||||
# источника последнего CONTROL), tx по ним показывает, отвечает ли приёмник и
|
||||
# не ушёл ли он сам в failsafe. Это PC-only служебный трафик (к STM отношения не
|
||||
# имеет), поэтому живёт здесь, а не в зеркале протокола link_proto.
|
||||
HEARTBEAT_MAGIC = b"\xCB\x01"
|
||||
HEARTBEAT_PERIOD_S = 0.2 # как часто rx шлёт heartbeat
|
||||
HEARTBEAT_TIMEOUT_S = 1.0 # нет heartbeat дольше -> tx считает приёмник недоступным
|
||||
HEARTBEAT_STATUS_S = 5.0 # период печати статуса приёмника на tx
|
||||
HB_FLAG_FAILSAFE = 0x01
|
||||
|
||||
log = logging.getLogger("bridge")
|
||||
|
||||
|
||||
def pack_hb(failsafe: bool, last_seq: int) -> bytes:
|
||||
flags = HB_FLAG_FAILSAFE if failsafe else 0
|
||||
return HEARTBEAT_MAGIC + struct.pack("<BI", flags, last_seq & 0xFFFFFFFF)
|
||||
|
||||
|
||||
def parse_hb(payload: bytes):
|
||||
"""-> dict(failsafe, last_seq) либо None, если это не heartbeat."""
|
||||
if len(payload) != len(HEARTBEAT_MAGIC) + 5 or payload[:2] != HEARTBEAT_MAGIC:
|
||||
return None
|
||||
flags, last_seq = struct.unpack_from("<BI", payload, 2)
|
||||
return {"failsafe": bool(flags & HB_FLAG_FAILSAFE), "last_seq": last_seq}
|
||||
|
||||
|
||||
_ANSI = {"green": "\033[1;32m", "red": "\033[1;31m", "yellow": "\033[1;33m", "reset": "\033[0m"}
|
||||
|
||||
|
||||
def render_link_status(alive: bool, failsafe: bool, color: bool):
|
||||
"""Живая одна строка для оператора: зелёная «СВЯЗЬ ЕСТЬ» / красная «СВЯЗИ
|
||||
НЕТ», обновляется на месте (возврат каретки)."""
|
||||
if not alive:
|
||||
text, c = "СВЯЗИ НЕТ", "red"
|
||||
elif failsafe:
|
||||
text, c = "СВЯЗЬ ЕСТЬ (приёмник в failsafe)", "yellow"
|
||||
else:
|
||||
text, c = "СВЯЗЬ ЕСТЬ", "green"
|
||||
body = "● " + text
|
||||
if color:
|
||||
body = _ANSI[c] + body + _ANSI["reset"]
|
||||
sys.stdout.write("\r\033[2K" + body) # \033[2K — очистить строку
|
||||
sys.stdout.flush()
|
||||
|
||||
|
||||
# ---------------- выбор и 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
|
||||
rx_alive = False # получаем ли heartbeat от приёмника
|
||||
rx_failsafe = False # сообщил ли приёмник, что сам в failsafe
|
||||
last_hb = 0.0
|
||||
last_status = 0.0
|
||||
# В терминале — живая цветная строка; под systemd (journald, не TTY) — обычные
|
||||
# события по изменению состояния.
|
||||
tty = sys.stdout.isatty()
|
||||
color = tty and os.environ.get("NO_COLOR") is None
|
||||
while True:
|
||||
# read(1) блокирует до первого байта, затем забираем всё доступное —
|
||||
# кадр уходит в сеть сразу, без ожидания таймаута чтения (иначе каждый
|
||||
# кадр задерживался бы до таймаута порта).
|
||||
data = ser.read(1)
|
||||
if ser.in_waiting:
|
||||
data += ser.read(ser.in_waiting)
|
||||
for ftype, payload in parser.feed(data):
|
||||
if ftype == lp.CONTROL:
|
||||
sock.sendto(seal(payload, key), peer)
|
||||
sent += 1
|
||||
elif ftype == lp.LOG:
|
||||
log.info("STM-TX: %s", payload.decode("ascii", "replace"))
|
||||
|
||||
now = time.monotonic()
|
||||
prev_alive, prev_fs = rx_alive, rx_failsafe
|
||||
# обратный канал: heartbeat от приёмника (приходит на тот же сокет)
|
||||
while select.select([sock], [], [], 0)[0]:
|
||||
try:
|
||||
datagram, _ = sock.recvfrom(2048)
|
||||
except BlockingIOError:
|
||||
break
|
||||
hb = parse_hb(unseal(datagram, key) or b"")
|
||||
if hb is None:
|
||||
continue
|
||||
last_hb = now
|
||||
rx_alive = True
|
||||
rx_failsafe = hb["failsafe"]
|
||||
if rx_alive and now - last_hb > HEARTBEAT_TIMEOUT_S:
|
||||
rx_alive, rx_failsafe = False, False
|
||||
|
||||
if tty:
|
||||
render_link_status(rx_alive, rx_failsafe, color) # живая строка
|
||||
else:
|
||||
if rx_alive != prev_alive:
|
||||
(log.info if rx_alive else log.warning)(
|
||||
"приёмник отвечает" if rx_alive else "приёмник НЕ отвечает")
|
||||
elif rx_alive and rx_failsafe != prev_fs:
|
||||
(log.warning if rx_failsafe else log.info)(
|
||||
"приёмник в failsafe: наш поток не доходит" if rx_failsafe
|
||||
else "приёмник принимает поток")
|
||||
if now - last_status >= HEARTBEAT_STATUS_S:
|
||||
last_status = now
|
||||
log.info("приёмник: %s (отправлено %d)",
|
||||
"отвечает" if rx_alive else "НЕ отвечает", sent)
|
||||
|
||||
|
||||
# ---------------- роль 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.setblocking(False)
|
||||
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
|
||||
last_hb_sent = 0.0
|
||||
fs_seq = 0
|
||||
in_failsafe = False
|
||||
rejects = 0 # подряд отброшенных по seq -> признак перезапуска источника
|
||||
tx_addr = None # куда слать heartbeat (источник последнего CONTROL)
|
||||
|
||||
def accept(datagram, addr):
|
||||
"""Проверить пакет и обновить состояние; вернуть payload принятого
|
||||
свежего кадра либо None."""
|
||||
nonlocal last_seq, last_net, in_failsafe, rejects, tx_addr
|
||||
if allowed_host and addr[0] != allowed_host:
|
||||
log.warning("пакет с неожиданного адреса %s отброшен", addr[0])
|
||||
return None
|
||||
payload = unseal(datagram, key)
|
||||
if not payload or len(payload) != lp.CONTROL_LEN:
|
||||
log.warning("битый/неаутентичный пакет отброшен")
|
||||
return None
|
||||
tx_addr = addr # аутентичный CONTROL -> знаем обратный адрес для heartbeat
|
||||
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 not fresh:
|
||||
return None # устаревший/повторный — anti-replay
|
||||
rejects = 0
|
||||
last_seq = c["seq"]
|
||||
last_net = time.monotonic()
|
||||
if in_failsafe:
|
||||
log.info("связь восстановлена")
|
||||
in_failsafe = False
|
||||
return payload
|
||||
|
||||
while True:
|
||||
# Ждём пакет до 20 мс (пауза цикла), затем БЕЗ блокировки выгребаем всю
|
||||
# накопившуюся очередь сокета и шлём на STM только самый свежий кадр
|
||||
# (latest-wins): так задержка не растёт, даже если пакеты пришли пачкой.
|
||||
newest = None
|
||||
ready = select.select([sock], [], [], lp.FRAME_PERIOD_MS / 2000.0)[0]
|
||||
while ready:
|
||||
try:
|
||||
datagram, addr = sock.recvfrom(2048)
|
||||
except BlockingIOError:
|
||||
break
|
||||
p = accept(datagram, addr)
|
||||
if p is not None:
|
||||
newest = p
|
||||
ready = select.select([sock], [], [], 0)[0]
|
||||
if newest is not None:
|
||||
ser.write(lp.build_frame(lp.CONTROL, newest))
|
||||
|
||||
# сетевой 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
|
||||
|
||||
# обратный heartbeat к tx: «приёмник жив», с признаком собственного failsafe
|
||||
if tx_addr is not None and now - last_hb_sent >= HEARTBEAT_PERIOD_S:
|
||||
sock.sendto(seal(pack_hb(in_failsafe, last_seq or 0), key), tx_addr)
|
||||
last_hb_sent = now
|
||||
|
||||
# неблокирующий дренаж входящих LOG от STM-RX
|
||||
if ser.in_waiting:
|
||||
for ftype, payload in parser.feed(ser.read(ser.in_waiting)):
|
||||
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():
|
||||
# Значения по умолчанию берутся из env (CART_BRIDGE_*), чтобы systemd-юнит
|
||||
# мог задавать всё через EnvironmentFile; аргументы CLI их переопределяют.
|
||||
env = os.environ.get
|
||||
ap = argparse.ArgumentParser(description="мост пульт<->тележка через сеть")
|
||||
ap.add_argument("--role", default=env("CART_BRIDGE_ROLE"), choices=("tx", "rx"))
|
||||
ap.add_argument("--peer", default=env("CART_BRIDGE_PEER"),
|
||||
help="tx: куда слать HOST:PORT; rx: разрешённый источник (опц.)")
|
||||
ap.add_argument("--bind", default=env("CART_BRIDGE_BIND", "0.0.0.0:5005"),
|
||||
help="rx: локальный UDP HOST:PORT")
|
||||
ap.add_argument("--device", default=env("CART_BRIDGE_DEVICE"),
|
||||
help="путь к порту или подстрока product/serial (иначе по роли)")
|
||||
ap.add_argument("--key", default=env("CART_BRIDGE_KEY"),
|
||||
help="общий ключ для HMAC-аутентификации пакетов (для интернета)")
|
||||
ap.add_argument("--log-level", default=env("CART_BRIDGE_LOG_LEVEL", "INFO"))
|
||||
args = ap.parse_args()
|
||||
|
||||
logging.basicConfig(level=args.log_level.upper(),
|
||||
format="%(asctime)s %(levelname)s %(message)s")
|
||||
if args.role not in ("tx", "rx"):
|
||||
raise SystemExit("нужна роль: --role tx|rx (или CART_BRIDGE_ROLE)")
|
||||
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("остановлено")
|
||||
finally:
|
||||
if sys.stdout.isatty():
|
||||
sys.stdout.write("\n") # увести живую строку от приглашения shell
|
||||
|
||||
|
||||
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 = 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__)
|
||||
Reference in New Issue
Block a user