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
parent 7329750c0c
commit 6f1eaf19e4
4 changed files with 172 additions and 0 deletions
@@ -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);