seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

Merge pull request 'feat: UART command protocol for Jetson-STM32 (Issue #629)' (#639) from sl-firmware/issue-629-uart-protocol into main

Browse Source
This commit is contained in:
sl-jetson 2026-03-15 16:30:09 -04:00
commit 45332f1a8b
4 changed files with 425 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
include/config.h
View File

@ -271,4 +271,11 @@
#define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter #define LVC_HYSTERESIS_MV 200u // recovery hysteresis to prevent threshold chatter
#define LVC_TLM_HZ 1u // JLINK_TLM_LVC transmit rate (Hz) #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 #endif // CONFIG_H

96
include/uart_protocol.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
/* ── 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 */

52
src/main.c
View File

@ -36,6 +36,7 @@
#include "servo_bus.h" #include "servo_bus.h"
#include "gimbal.h" #include "gimbal.h"
#include "lvc.h" #include "lvc.h"
#include "uart_protocol.h"
#include <math.h> #include <math.h>
#include <string.h> #include <string.h>
#include <stdio.h> #include <stdio.h>
@ -261,6 +262,9 @@ int main(void) {
/* Init LVC: low voltage cutoff state machine (Issue #613) */ /* Init LVC: low voltage cutoff state machine (Issue #613) */
lvc_init(); 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 */ /* Probe I2C1 for optional sensors — skip gracefully if not found */
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */ int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
mag_type_t mag_type = MAG_NONE; 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_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 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 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 */ uint8_t pm_pwm_phase = 0; /* Software PWM counter for sleep LED */
const float dt = 1.0f / PID_LOOP_HZ; /* 1ms at 1kHz */ 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 bus RX: drain FIFO0 and parse feedback frames (Issue #597) */
can_driver_process(); can_driver_process();
/* UART command protocol RX: parse Jetson frames (Issue #629) */
uart_protocol_process();
/* Handle JLink one-shot flags from Jetson binary protocol */ /* Handle JLink one-shot flags from Jetson binary protocol */
if (jlink_state.estop_req) { if (jlink_state.estop_req) {
jlink_state.estop_req = 0u; jlink_state.estop_req = 0u;
@ -430,6 +438,37 @@ int main(void) {
(double)bal.kp, (double)bal.ki, (double)bal.kd); (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) */ /* FAULT_LOG_GET: send fault log telemetry to Jetson (Issue #565) */
if (jlink_state.fault_log_req) { if (jlink_state.fault_log_req) {
jlink_state.fault_log_req = 0u; jlink_state.fault_log_req = 0u;
@ -768,6 +807,19 @@ int main(void) {
jlink_send_lvc_tlm(&ltlm); jlink_send_lvc_tlm(&ltlm);
} }
/* 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) */ /* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) { if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
send_tick = now; send_tick = now;

270
src/uart_protocol.c Normal file
View File

@ -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 <string.h>
/* ── 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));
}