diff --git a/device_firmware/cart_firmware/platformio.ini b/device_firmware/cart_firmware/platformio.ini index 86ee5ec..a1c0055 100644 --- a/device_firmware/cart_firmware/platformio.ini +++ b/device_firmware/cart_firmware/platformio.ini @@ -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 = -<*> + + + +[env:cart_tx] +build_flags = + ${env.build_flags} + -DCART_ROLE_TX + -DUSB_PRODUCT_STRING="\"CART-TX\"" +build_src_filter = ${env.build_src_filter} + + + +[env:cart_rx] +build_flags = + ${env.build_flags} + -DCART_ROLE_RX + -DUSB_PRODUCT_STRING="\"CART-RX\"" +build_src_filter = ${env.build_src_filter} + diff --git a/device_firmware/cart_firmware/src/app_rx.cpp b/device_firmware/cart_firmware/src/app_rx.cpp new file mode 100644 index 0000000..189c877 --- /dev/null +++ b/device_firmware/cart_firmware/src/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 +#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 diff --git a/device_firmware/cart_firmware/src/app_tx.cpp b/device_firmware/cart_firmware/src/app_tx.cpp new file mode 100644 index 0000000..e00f538 --- /dev/null +++ b/device_firmware/cart_firmware/src/app_tx.cpp @@ -0,0 +1,59 @@ +// Роль TX: к STM подключён пульт. Как и раньше, формирует SBUS на локальный +// провод (старый функционал не изменён), и ДОПОЛНИТЕЛЬНО шлёт те же значения +// каналов по USB на ПК кадром CONTROL. Отвечает на HELLO кадром IDENT. +#ifdef CART_ROLE_TX +#include +#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 diff --git a/device_firmware/cart_firmware/src/main.cpp b/device_firmware/cart_firmware/src/main.cpp deleted file mode 100644 index 6af2c78..0000000 --- a/device_firmware/cart_firmware/src/main.cpp +++ /dev/null @@ -1,166 +0,0 @@ -// Пульт тележки: стик (АЦП PA0/PA1) -> SBUS на USART1 TX (PA9). -// Протокол: инвертированный SBUS, 100000 бод 8E2, 25 байт каждые 50 мс -// (см. docs/protocol.md). USB CDC (Serial) — отладочный вывод. -#include - -// ---- калибровка стика (АЦП 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]); -}