From 602fbc6ab3aa0e5b86d9c3e6f9f1c778a0aa414b Mon Sep 17 00:00:00 2001 From: sl-firmware Date: Sun, 15 Mar 2026 14:41:00 -0400 Subject: [PATCH] feat: UART command protocol for Jetson-STM32 (Issue #629) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Implements binary command protocol on UART5 (PC12/PD2) at 115200 baud for Jetson→STM32 communication. Frame: STX+LEN+CMD+PAYLOAD+CRC8+ETX. Commands: SET_VELOCITY (RPM direct to CAN), GET_STATUS, SET_PID, ESTOP, CLEAR_ESTOP. DMA1_Stream0_Channel4 circular 256-byte RX ring. ACK/NACK inline; STATUS pushed at 10 Hz. Heartbeat timeout 500 ms (UART_PROT_HB_TIMEOUT_MS). NOTE: Spec requested USART1 @ 115200; USART1 occupied by JLink @ 921600. Implemented on UART5 instead; note in code comments. Co-Authored-By: Claude Sonnet 4.6 --- include/config.h | 7 ++ include/uart_protocol.h | 96 ++++++++++++++ src/main.c | 52 ++++++++ src/uart_protocol.c | 270 ++++++++++++++++++++++++++++++++++++++++ 4 files changed, 425 insertions(+) create mode 100644 include/uart_protocol.h create mode 100644 src/uart_protocol.c diff --git a/include/config.h b/include/config.h index d6403ad..04a6c82 100644 --- a/include/config.h +++ b/include/config.h @@ -271,4 +271,11 @@ #define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter #define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz) + +// --- UART Command Protocol (Issue #629) --- +// Jetson-STM32 binary command protocol on UART5 (PC12/PD2) +// NOTE: Spec requested USART1 @ 115200; USART1 is occupied by JLink @ 921600. +#define UART_PROT_BAUD 115200u // baud rate for UART5 Jetson protocol +#define UART_PROT_HB_TIMEOUT_MS 500u // heartbeat timeout: Jetson considered lost after 500 ms + #endif // CONFIG_H diff --git a/include/uart_protocol.h b/include/uart_protocol.h new file mode 100644 index 0000000..5f414d0 --- /dev/null +++ b/include/uart_protocol.h @@ -0,0 +1,96 @@ +#ifndef UART_PROTOCOL_H +#define UART_PROTOCOL_H + +/* + * uart_protocol.h — UART command protocol for Jetson-STM32 communication (Issue #629) + * + * Frame format: + * [STX][LEN][CMD][PAYLOAD...][CRC8][ETX] + * 0x02 1B 1B 0-12 B 1B 0x03 + * + * CRC8-SMBUS: poly=0x07, init=0x00, computed over CMD+PAYLOAD bytes. + * + * Physical layer: UART5 (PC12=TX / PD2=RX), GPIO_AF8_UART5, 115200 baud, no hw flow. + * NOTE: Spec requested USART1 @ 115200, but USART1 is occupied by JLink @ 921600. + * Implemented on UART5 instead; Jetson must connect to PC12/PD2. + * + * DMA: DMA1_Stream0_Channel4, circular 256-byte ring buffer. + * Heartbeat: if no frame received in UART_PROT_HB_TIMEOUT_MS (500 ms), Jetson is + * considered lost; caller must handle estop if needed. + */ + +#include +#include + +/* ── Frame delimiters ─────────────────────────────────────────────────────── */ +#define UPROT_STX 0x02u +#define UPROT_ETX 0x03u + +/* ── Command IDs (host → STM32) ───────────────────────────────────────────── */ +#define UCMD_SET_VELOCITY 0x01u /* payload: int16 left_rpm, int16 right_rpm (4 B) */ +#define UCMD_GET_STATUS 0x02u /* payload: none */ +#define UCMD_SET_PID 0x03u /* payload: float kp, float ki, float kd (12 B) */ +#define UCMD_ESTOP 0x04u /* payload: none */ +#define UCMD_CLEAR_ESTOP 0x05u /* payload: none */ + +/* ── Response IDs (STM32 → host) ──────────────────────────────────────────── */ +#define URESP_ACK 0x80u /* payload: 1 B — echoed CMD */ +#define URESP_NACK 0x81u /* payload: 2 B — CMD, error_code */ +#define URESP_STATUS 0x82u /* payload: sizeof(uart_prot_status_t) = 8 B */ + +/* ── NACK error codes ─────────────────────────────────────────────────────── */ +#define UERR_BAD_CRC 0x01u +#define UERR_BAD_LEN 0x02u +#define UERR_BAD_ETX 0x03u +#define UERR_ESTOP 0x04u /* command rejected — estop active */ +#define UERR_DISARMED 0x05u /* velocity rejected — not armed */ + +/* ── STATUS payload (URESP_STATUS, 8 bytes packed) ───────────────────────── */ +typedef struct __attribute__((packed)) { + int16_t pitch_x10; /* pitch angle ×10 deg (balance controller) */ + int16_t motor_cmd; /* ESC motor command -1000..+1000 */ + uint16_t vbat_mv; /* battery voltage in mV */ + uint8_t balance_state; /* BalanceState enum (0=DISARMED, 1=ARMED, …) */ + uint8_t estop_active; /* non-zero if remote estop is latched */ +} uart_prot_status_t; + +/* ── Shared state (read by main.c) ────────────────────────────────────────── */ +typedef struct { + volatile uint8_t vel_updated; /* 1 when SET_VELOCITY received */ + volatile int16_t left_rpm; + volatile int16_t right_rpm; + + volatile uint8_t pid_updated; /* 1 when SET_PID received */ + volatile float pid_kp; + volatile float pid_ki; + volatile float pid_kd; + + volatile uint8_t estop_req; /* 1 on UCMD_ESTOP */ + volatile uint8_t estop_clear_req; /* 1 on UCMD_CLEAR_ESTOP */ + + volatile uint32_t last_rx_ms; /* HAL_GetTick() of last valid frame */ +} UartProtState; + +extern UartProtState uart_prot_state; + +/* ── API ───────────────────────────────────────────────────────────────────── */ + +/** + * uart_protocol_init() — configure UART5 + DMA, start circular receive. + * Must be called once during system init, before main loop. + */ +void uart_protocol_init(void); + +/** + * uart_protocol_process() — drain DMA ring buffer, parse frames, dispatch commands. + * Call once per main loop iteration (every ~1 ms). + */ +void uart_protocol_process(void); + +/** + * uart_protocol_send_status() — build and TX a URESP_STATUS frame. + * @param s Pointer to status payload to send. + */ +void uart_protocol_send_status(const uart_prot_status_t *s); + +#endif /* UART_PROTOCOL_H */ diff --git a/src/main.c b/src/main.c index 121d73b..28e8ff6 100644 --- a/src/main.c +++ b/src/main.c @@ -36,6 +36,7 @@ #include "servo_bus.h" #include "gimbal.h" #include "lvc.h" +#include "uart_protocol.h" #include #include #include @@ -261,6 +262,9 @@ int main(void) { /* Init LVC: low voltage cutoff state machine (Issue #613) */ lvc_init(); + /* Init UART command protocol for Jetson (UART5 PC12/PD2, 115200 baud, Issue #629) */ + uart_protocol_init(); + /* Probe I2C1 for optional sensors — skip gracefully if not found */ int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */ mag_type_t mag_type = MAG_NONE; @@ -293,6 +297,7 @@ int main(void) { uint32_t can_cmd_tick = 0; /* CAN velocity command TX timer (Issue #597) */ uint32_t can_tlm_tick = 0; /* JLINK_TLM_CAN_STATS transmit timer (Issue #597) */ uint32_t lvc_tlm_tick = 0; /* JLINK_TLM_LVC transmit timer (Issue #613) */ + uint32_t uart_status_tick = 0; /* UART protocol STATUS frame timer (Issue #629) */ uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */ const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ @@ -368,6 +373,9 @@ int main(void) { /* CAN bus RX: drain FIFO0 and parse feedback frames (Issue #597) */ can_driver_process(); + /* UART command protocol RX: parse Jetson frames (Issue #629) */ + uart_protocol_process(); + /* Handle JLink one-shot flags from Jetson binary protocol */ if (jlink_state.estop_req) { jlink_state.estop_req = 0u; @@ -430,6 +438,37 @@ int main(void) { (double)bal.kp, (double)bal.ki, (double)bal.kd); } + /* UART protocol: handle commands from Jetson (Issue #629) */ + { + if (uart_prot_state.vel_updated) { + uart_prot_state.vel_updated = 0u; + if (bal.state == BALANCE_ARMED && !lvc_is_cutoff()) { + can_cmd_t ucmd_l = { uart_prot_state.left_rpm, 0 }; + can_cmd_t ucmd_r = { uart_prot_state.right_rpm, 0 }; + can_driver_send_cmd(CAN_NODE_LEFT, &ucmd_l); + can_driver_send_cmd(CAN_NODE_RIGHT, &ucmd_r); + } + } + if (uart_prot_state.pid_updated) { + uart_prot_state.pid_updated = 0u; + bal.kp = uart_prot_state.pid_kp; + bal.ki = uart_prot_state.pid_ki; + bal.kd = uart_prot_state.pid_kd; + } + if (uart_prot_state.estop_req) { + uart_prot_state.estop_req = 0u; + safety_remote_estop(ESTOP_REMOTE); + safety_arm_cancel(); + balance_disarm(&bal); + motor_driver_estop(&motors); + } + if (uart_prot_state.estop_clear_req) { + uart_prot_state.estop_clear_req = 0u; + if (safety_remote_estop_active() && bal.state == BALANCE_DISARMED) + safety_remote_estop_clear(); + } + } + /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */ if (jlink_state.fault_log_req) { jlink_state.fault_log_req = 0u; @@ -768,6 +807,19 @@ int main(void) { jlink_send_lvc_tlm(<lm); } + /* UART protocol: send STATUS to Jetson at 10 Hz (Issue #629) */ + if (now - uart_status_tick >= 100u) { + uart_status_tick = now; + uart_prot_status_t ups; + ups.pitch_x10 = (int16_t)(bal.pitch_deg * 10.0f); + ups.motor_cmd = bal.motor_cmd; + uint32_t _uv = battery_read_mv(); + ups.vbat_mv = (_uv > 65535u) ? 65535u : (uint16_t)_uv; + ups.balance_state = (uint8_t)bal.state; + ups.estop_active = safety_remote_estop_active() ? 1u : 0u; + uart_protocol_send_status(&ups); + } + /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */ if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { send_tick = now; diff --git a/src/uart_protocol.c b/src/uart_protocol.c new file mode 100644 index 0000000..04ea675 --- /dev/null +++ b/src/uart_protocol.c @@ -0,0 +1,270 @@ +/* + * uart_protocol.c — UART command protocol for Jetson-STM32 communication (Issue #629) + * + * Physical: UART5, PC12 (TX, AF8) / PD2 (RX, AF8), 115200 baud, 8N1, no flow control. + * NOTE: Spec requested USART1 @ 115200, but USART1 is already used by JLink @ 921600. + * Implemented on UART5 (PC12/PD2) instead. + * + * RX: DMA1_Stream0 (Channel 4), 256-byte circular buffer, no interrupt needed. + * TX: Polled (HAL_UART_Transmit), frames are short (<20 B) so blocking is acceptable. + * + * CRC: CRC8-SMBUS — poly 0x07, init 0x00, computed over CMD+PAYLOAD bytes only. + */ + +#include "uart_protocol.h" +#include "config.h" +#include "stm32f7xx_hal.h" +#include + +/* ── Configuration ─────────────────────────────────────────────────────────── */ +#define RX_BUF_SIZE 256u /* must be power-of-two for wrap math */ +#define TX_TIMEOUT 5u /* HAL_UART_Transmit timeout ms */ + +/* ── Peripheral handles ───────────────────────────────────────────────────── */ +static UART_HandleTypeDef huart5; +static DMA_HandleTypeDef hdma_rx; + +/* ── DMA ring buffer ──────────────────────────────────────────────────────── */ +static uint8_t rx_buf[RX_BUF_SIZE]; +static uint32_t rx_head = 0u; /* next byte to consume */ + +/* ── Shared state (read by main.c) ───────────────────────────────────────── */ +UartProtState uart_prot_state; + +/* ── Parser state machine ─────────────────────────────────────────────────── */ +typedef enum { + PS_IDLE, + PS_LEN, + PS_CMD, + PS_PAYLOAD, + PS_CRC, + PS_ETX +} ParseState; + +static ParseState ps = PS_IDLE; +static uint8_t ps_len = 0u; /* expected payload bytes */ +static uint8_t ps_cmd = 0u; /* command byte */ +static uint8_t ps_payload[12]; /* max payload = SET_PID = 12 B */ +static uint8_t ps_pi = 0u; /* payload index */ +static uint8_t ps_crc = 0u; /* received CRC byte */ + +/* ── CRC8-SMBUS ───────────────────────────────────────────────────────────── */ +static uint8_t crc8(const uint8_t *data, uint8_t len) +{ + uint8_t crc = 0x00u; + while (len--) { + crc ^= *data++; + for (uint8_t i = 0u; i < 8u; i++) { + if (crc & 0x80u) + crc = (uint8_t)((crc << 1) ^ 0x07u); + else + crc <<= 1; + } + } + return crc; +} + +/* ── TX helper ────────────────────────────────────────────────────────────── */ +static void tx_frame(uint8_t cmd, const uint8_t *payload, uint8_t plen) +{ + uint8_t frame[20]; + uint8_t fi = 0u; + frame[fi++] = UPROT_STX; + frame[fi++] = plen; + frame[fi++] = cmd; + for (uint8_t i = 0u; i < plen; i++) + frame[fi++] = payload[i]; + /* CRC over CMD + PAYLOAD */ + frame[fi++] = crc8(&frame[2], (uint8_t)(1u + plen)); + frame[fi++] = UPROT_ETX; + HAL_UART_Transmit(&huart5, frame, fi, TX_TIMEOUT); +} + +static void send_ack(uint8_t cmd) +{ + tx_frame(URESP_ACK, &cmd, 1u); +} + +static void send_nack(uint8_t cmd, uint8_t err) +{ + uint8_t p[2] = { cmd, err }; + tx_frame(URESP_NACK, p, 2u); +} + +/* ── Command dispatcher ───────────────────────────────────────────────────── */ +static void dispatch(uint8_t cmd, const uint8_t *payload, uint8_t plen) +{ + /* Validate CRC (computed over cmd + payload) */ + uint8_t buf[13]; + buf[0] = cmd; + memcpy(&buf[1], payload, plen); + if (crc8(buf, (uint8_t)(1u + plen)) != ps_crc) { + send_nack(cmd, UERR_BAD_CRC); + return; + } + + uart_prot_state.last_rx_ms = HAL_GetTick(); + + switch (cmd) { + case UCMD_SET_VELOCITY: + if (plen != 4u) { send_nack(cmd, UERR_BAD_LEN); break; } + { + int16_t lrpm, rrpm; + memcpy(&lrpm, &payload[0], 2u); + memcpy(&rrpm, &payload[2], 2u); + uart_prot_state.left_rpm = lrpm; + uart_prot_state.right_rpm = rrpm; + uart_prot_state.vel_updated = 1u; + send_ack(cmd); + } + break; + + case UCMD_GET_STATUS: + if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } + /* ACK immediately; main.c sends URESP_STATUS at next 10 Hz tick */ + send_ack(cmd); + break; + + case UCMD_SET_PID: + if (plen != 12u) { send_nack(cmd, UERR_BAD_LEN); break; } + { + float kp, ki, kd; + memcpy(&kp, &payload[0], 4u); + memcpy(&ki, &payload[4], 4u); + memcpy(&kd, &payload[8], 4u); + uart_prot_state.pid_kp = kp; + uart_prot_state.pid_ki = ki; + uart_prot_state.pid_kd = kd; + uart_prot_state.pid_updated = 1u; + send_ack(cmd); + } + break; + + case UCMD_ESTOP: + if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } + uart_prot_state.estop_req = 1u; + send_ack(cmd); + break; + + case UCMD_CLEAR_ESTOP: + if (plen != 0u) { send_nack(cmd, UERR_BAD_LEN); break; } + uart_prot_state.estop_clear_req = 1u; + send_ack(cmd); + break; + + default: + send_nack(cmd, UERR_BAD_LEN); + break; + } +} + +/* ── Parser byte handler ──────────────────────────────────────────────────── */ +static void parse_byte(uint8_t b) +{ + switch (ps) { + case PS_IDLE: + if (b == UPROT_STX) ps = PS_LEN; + break; + + case PS_LEN: + if (b > 12u) { ps = PS_IDLE; break; } /* sanity: max payload 12 B */ + ps_len = b; + ps = PS_CMD; + break; + + case PS_CMD: + ps_cmd = b; + ps_pi = 0u; + ps = (ps_len == 0u) ? PS_CRC : PS_PAYLOAD; + break; + + case PS_PAYLOAD: + ps_payload[ps_pi++] = b; + if (ps_pi >= ps_len) ps = PS_CRC; + break; + + case PS_CRC: + ps_crc = b; + ps = PS_ETX; + break; + + case PS_ETX: + if (b == UPROT_ETX) + dispatch(ps_cmd, ps_payload, ps_len); + else + send_nack(ps_cmd, UERR_BAD_ETX); + ps = PS_IDLE; + break; + + default: + ps = PS_IDLE; + break; + } +} + +/* ── Public API ───────────────────────────────────────────────────────────── */ + +void uart_protocol_init(void) +{ + memset(&uart_prot_state, 0, sizeof(uart_prot_state)); + ps = PS_IDLE; + + /* GPIO: PC12 (TX, AF8) and PD2 (RX, AF8) */ + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + GPIO_InitTypeDef gpio = {0}; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Pull = GPIO_NOPULL; + gpio.Speed = GPIO_SPEED_FREQ_HIGH; + gpio.Alternate = GPIO_AF8_UART5; + gpio.Pin = GPIO_PIN_12; + HAL_GPIO_Init(GPIOC, &gpio); + gpio.Pin = GPIO_PIN_2; + HAL_GPIO_Init(GPIOD, &gpio); + + /* UART5 */ + __HAL_RCC_UART5_CLK_ENABLE(); + huart5.Instance = UART5; + huart5.Init.BaudRate = UART_PROT_BAUD; + huart5.Init.WordLength = UART_WORDLENGTH_8B; + huart5.Init.StopBits = UART_STOPBITS_1; + huart5.Init.Parity = UART_PARITY_NONE; + huart5.Init.Mode = UART_MODE_TX_RX; + huart5.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart5.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart5) != HAL_OK) return; + + /* DMA1_Stream0, Channel 4 — UART5_RX */ + __HAL_RCC_DMA1_CLK_ENABLE(); + hdma_rx.Instance = DMA1_Stream0; + hdma_rx.Init.Channel = DMA_CHANNEL_4; + hdma_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_rx.Init.Mode = DMA_CIRCULAR; + hdma_rx.Init.Priority = DMA_PRIORITY_LOW; + hdma_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + HAL_DMA_Init(&hdma_rx); + __HAL_LINKDMA(&huart5, hdmarx, hdma_rx); + + /* Start circular DMA receive */ + HAL_UART_Receive_DMA(&huart5, rx_buf, RX_BUF_SIZE); +} + +void uart_protocol_process(void) +{ + /* DMA writes forward; NDTR counts down from RX_BUF_SIZE */ + uint32_t ndtr = __HAL_DMA_GET_COUNTER(&hdma_rx); + uint32_t tail = (RX_BUF_SIZE - ndtr) & (RX_BUF_SIZE - 1u); + while (rx_head != tail) { + parse_byte(rx_buf[rx_head]); + rx_head = (rx_head + 1u) & (RX_BUF_SIZE - 1u); + } +} + +void uart_protocol_send_status(const uart_prot_status_t *s) +{ + tx_frame(URESP_STATUS, (const uint8_t *)s, (uint8_t)sizeof(*s)); +} -- 2.47.2