cart_firmware_added

2026-08-26 15:40:10 +03:00
parent 6ada811c2f
commit cc6d189d52
12 changed files with 604 additions and 4 deletions
@@ -0,0 +1,144 @@
+// Пульт тележки: стик (АЦП 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 = 508, X_MID = 2014, X_BACK = 3625;   // PA0
+static const int Y_RIGHT = 479, Y_MID = 1981, Y_LEFT = 3586; // PA1
+static const int DEADZONE = 30; // отсечка дребезга вокруг нейтрали
+
+// ---- 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.4f;
+// ---- общий масштаб выхода: 1.0 = диапазон старого пульта ----
+static const float RANGE_SCALE = 0.5f;
+
+// симметричные плечи: вперёд и назад дают одинаковый максимум
+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 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));
+}
+
+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] = toSbus(expo(fwd) * RANGE_SCALE, CH2_SPAN);   // канал 2
+    ch[3] = toSbus(expo(right) * RANGE_SCALE, CH4_SPAN); // канал 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]);
+}