diff --git a/device_firmware/cart_firmware/src/sbus.cpp b/device_firmware/cart_firmware/src/sbus.cpp new file mode 100644 index 0000000..4d35e3b --- /dev/null +++ b/device_firmware/cart_firmware/src/sbus.cpp @@ -0,0 +1,55 @@ +#include "sbus.h" +#include +#include + +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); +} diff --git a/device_firmware/cart_firmware/src/sbus.h b/device_firmware/cart_firmware/src/sbus.h new file mode 100644 index 0000000..8f4d08f --- /dev/null +++ b/device_firmware/cart_firmware/src/sbus.h @@ -0,0 +1,25 @@ +// Общий SBUS-слой: упаковка кадра и вывод инвертированного SBUS на провод +// (USART1 TX = PA9, 100000 бод, 8E2, TXINV). Единственный владелец всего, что +// идёт на драйвер колёс — используется и TX (локальный провод), и RX. +// См. docs/protocol.md. +#pragma once +#include + +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]); diff --git a/device_firmware/cart_firmware/src/stick.cpp b/device_firmware/cart_firmware/src/stick.cpp new file mode 100644 index 0000000..65036fe --- /dev/null +++ b/device_firmware/cart_firmware/src/stick.cpp @@ -0,0 +1,80 @@ +#include "stick.h" +#include "sbus.h" +#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; // отсечка дребезга вокруг нейтрали + +// симметричные плечи: вперёд и назад дают одинаковый максимум +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; +} diff --git a/device_firmware/cart_firmware/src/stick.h b/device_firmware/cart_firmware/src/stick.h new file mode 100644 index 0000000..16be748 --- /dev/null +++ b/device_firmware/cart_firmware/src/stick.h @@ -0,0 +1,12 @@ +// Пульт: чтение стика (АЦП PA0/PA1), калибровка, профиль газа/поворота и +// формирование значений SBUS-каналов. Вся «логика пульта» живёт здесь. +// Компилируется только для роли TX (см. build_src_filter в platformio.ini). +#pragma once +#include + +// Инициализация АЦП-входов стика. +void stickInit(); + +// Прочитать стик и заполнить ch[16] значениями SBUS-каналов (нейтраль + ch2/ch4). +// Возвращает по ссылкам нормализованные оси для отладки. +void stickComputeChannels(uint16_t ch[16], float *fwdOut, float *rightOut);