d5246fe3a8
- esc_hoverboard.c: huart2 static in production; non-static only under #ifdef DEBUG_MOTOR_TEST (needed by R command in jetson_uart.c) - esc_hoverboard.c: UART5 diagnostic in hoverboard_backend_init() and per-packet printf in hoverboard_backend_send() guarded by same flag - esc_hoverboard.c: #include <stdio.h> also guarded (not needed in production) - jetson_uart.c: R (baud sweep) and X (GPIO test) commands guarded by #ifdef DEBUG_MOTOR_TEST — not compiled into production firmware Production build: no debug output, static huart2, no R/X commands. Debug build: define DEBUG_MOTOR_TEST to re-enable all diagnostics. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
288 lines
10 KiB
C
288 lines
10 KiB
C
/*
|
|
* jetson_uart.c — USART6 command interface for Jetson Orin
|
|
*
|
|
* Mirrors the USB CDC command protocol over UART so the Jetson can
|
|
* arm, disarm, heartbeat, drive, and e-stop via hardware UART.
|
|
* This fixes the CDC TX bug (Issue USB_CDC_BUG.md) by providing
|
|
* a reliable non-USB path.
|
|
*
|
|
* USART6: PC6=TX, PC7=RX @ 921600 baud (matches Orin ttyTHS1)
|
|
*
|
|
* Command protocol (same as CDC):
|
|
* A — arm request
|
|
* D — disarm request
|
|
* E — emergency stop
|
|
* Z — clear e-stop
|
|
* H — heartbeat (refresh timeout)
|
|
* C<s>,<t> — drive command: speed,steer (also refreshes heartbeat)
|
|
* G — gyro recalibration
|
|
*/
|
|
|
|
#include "jetson_uart.h"
|
|
#include "jetson_cmd.h"
|
|
#include "config.h"
|
|
#include "safety.h"
|
|
#include "mpu6000.h"
|
|
#include "stm32f7xx_hal.h"
|
|
#include <stdio.h>
|
|
|
|
/* Shared flags — same ones CDC sets, main loop consumes */
|
|
extern volatile uint8_t cdc_arm_request;
|
|
extern volatile uint8_t cdc_disarm_request;
|
|
extern volatile uint8_t cdc_recal_request;
|
|
extern volatile uint8_t cdc_estop_request;
|
|
extern volatile uint8_t cdc_estop_clear_request;
|
|
extern volatile uint8_t cdc_streaming;
|
|
extern volatile char cdc_cmd_buf[32];
|
|
extern volatile uint8_t cdc_cmd_ready;
|
|
|
|
/* From jetson_cmd.h */
|
|
extern volatile uint8_t jetson_cmd_ready;
|
|
extern volatile char jetson_cmd_buf[32];
|
|
extern volatile uint32_t jetson_hb_tick;
|
|
|
|
static UART_HandleTypeDef huart6;
|
|
static uint8_t rx_byte;
|
|
static char line_buf[64];
|
|
static uint8_t line_idx = 0;
|
|
|
|
static void process_line(const char *buf, uint8_t len);
|
|
|
|
/* Keep FC awake on Jetson activity */
|
|
extern void power_mgmt_activity(void);
|
|
|
|
void jetson_uart_init(void) {
|
|
__HAL_RCC_USART6_CLK_ENABLE();
|
|
__HAL_RCC_GPIOC_CLK_ENABLE();
|
|
|
|
/* PC6=TX, PC7=RX, AF8 for USART6 */
|
|
GPIO_InitTypeDef gpio = {0};
|
|
gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7;
|
|
gpio.Mode = GPIO_MODE_AF_PP;
|
|
gpio.Pull = GPIO_PULLUP;
|
|
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
|
|
gpio.Alternate = GPIO_AF8_USART6;
|
|
HAL_GPIO_Init(GPIOC, &gpio);
|
|
|
|
huart6.Instance = USART6;
|
|
huart6.Init.BaudRate = 921600;
|
|
huart6.Init.WordLength = UART_WORDLENGTH_8B;
|
|
huart6.Init.StopBits = UART_STOPBITS_1;
|
|
huart6.Init.Parity = UART_PARITY_NONE;
|
|
huart6.Init.Mode = UART_MODE_TX_RX;
|
|
huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
|
|
huart6.Init.OverSampling = UART_OVERSAMPLING_16;
|
|
HAL_UART_Init(&huart6);
|
|
|
|
HAL_NVIC_SetPriority(USART6_IRQn, 4, 0);
|
|
HAL_NVIC_EnableIRQ(USART6_IRQn);
|
|
|
|
/* Start interrupt-driven receive */
|
|
HAL_UART_Receive_IT(&huart6, &rx_byte, 1);
|
|
|
|
/* Boot banner — confirms USART6 is alive */
|
|
const char *banner = "SALTYLAB USART6 OK\n";
|
|
HAL_UART_Transmit(&huart6, (const uint8_t *)banner, 19, 50);
|
|
}
|
|
|
|
/* Send telemetry/status back to Jetson */
|
|
void jetson_uart_send(const uint8_t *data, uint16_t len) {
|
|
HAL_UART_Transmit(&huart6, (uint8_t *)data, len, 10);
|
|
}
|
|
|
|
/* ISR callback — accumulate bytes into line buffer, process on \n */
|
|
void jetson_uart_rx_callback(UART_HandleTypeDef *huart) {
|
|
if (huart != &huart6) return;
|
|
|
|
/* Any UART activity keeps the FC awake (prevents STOP mode) */
|
|
power_mgmt_activity();
|
|
|
|
if (rx_byte == '\n' || rx_byte == '\r') {
|
|
if (line_idx > 0) {
|
|
line_buf[line_idx] = '\0';
|
|
process_line(line_buf, line_idx);
|
|
line_idx = 0;
|
|
}
|
|
} else if (line_idx < sizeof(line_buf) - 1) {
|
|
line_buf[line_idx++] = (char)rx_byte;
|
|
} else {
|
|
line_idx = 0; /* overflow — reset */
|
|
}
|
|
|
|
/* Re-arm receive */
|
|
HAL_UART_Receive_IT(&huart6, &rx_byte, 1);
|
|
}
|
|
|
|
/*
|
|
* Process a complete line. Same command set as CDC_Receive.
|
|
* Single-char commands work without newline too (for compatibility).
|
|
*/
|
|
static void process_line(const char *buf, uint8_t len) {
|
|
if (len < 1) return;
|
|
|
|
switch (buf[0]) {
|
|
case 'A': cdc_arm_request = 1;
|
|
/* Send arm attempt status for debugging */
|
|
{
|
|
extern bool mpu6000_is_calibrated(void);
|
|
extern float bal_pitch_deg_get(void); /* we'll add this */
|
|
char dbg[80];
|
|
int n = snprintf(dbg, sizeof(dbg), "ARM_REQ cal=%d estop=%d\n",
|
|
mpu6000_is_calibrated() ? 1 : 0,
|
|
safety_remote_estop_active() ? 1 : 0);
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)dbg, n, 10);
|
|
}
|
|
break;
|
|
case 'D': cdc_disarm_request = 1; break;
|
|
case 'G': cdc_recal_request = 1; break;
|
|
case 'E': cdc_estop_request = 1; break;
|
|
case 'F': cdc_estop_request = 2; break;
|
|
case 'Z': cdc_estop_clear_request = 1; break;
|
|
case 'S': cdc_streaming = !cdc_streaming; break;
|
|
|
|
case 'H':
|
|
jetson_hb_tick = HAL_GetTick();
|
|
break;
|
|
|
|
case 'C': {
|
|
uint8_t copy_len = len < 31 ? len : 31;
|
|
for (uint8_t i = 0; i < copy_len; i++)
|
|
jetson_cmd_buf[i] = buf[i];
|
|
jetson_cmd_buf[copy_len] = '\0';
|
|
jetson_hb_tick = HAL_GetTick();
|
|
jetson_cmd_ready = 1;
|
|
break;
|
|
}
|
|
|
|
/* Direct motor test: W<speed>,<steer> — bypasses balance PID,
|
|
* sends directly to ESC. For bench testing and diagnostics only.
|
|
* Does NOT require arming. ESC watchdog stops motors if commands stop. */
|
|
case 'W': {
|
|
int spd = 0, str = 0;
|
|
if (len > 1 && sscanf(buf + 1, "%d,%d", &spd, &str) >= 1) {
|
|
/* Clamp to safe bench test range */
|
|
if (spd > 100) spd = 100;
|
|
if (spd < -100) spd = -100;
|
|
if (str > 100) str = 100;
|
|
if (str < -100) str = -100;
|
|
/* Set persistent direct test globals — main loop sends at 50Hz */
|
|
extern volatile int16_t direct_test_speed;
|
|
extern volatile int16_t direct_test_steer;
|
|
direct_test_speed = (int16_t)spd;
|
|
direct_test_steer = (int16_t)str;
|
|
char ack[32];
|
|
int n = snprintf(ack, sizeof(ack), "W:%d,%d\n", spd, str);
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)ack, n, 10);
|
|
}
|
|
break;
|
|
}
|
|
|
|
#ifdef DEBUG_MOTOR_TEST
|
|
/* GPIO test: reconfigure PC12 as plain GPIO output, set HIGH for 10s, then restore UART */
|
|
case 'X': {
|
|
GPIO_InitTypeDef gpio = {0};
|
|
gpio.Pin = GPIO_PIN_12;
|
|
gpio.Mode = GPIO_MODE_OUTPUT_PP;
|
|
gpio.Pull = GPIO_NOPULL;
|
|
gpio.Speed = GPIO_SPEED_FREQ_HIGH;
|
|
HAL_GPIO_Init(GPIOC, &gpio);
|
|
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_12, GPIO_PIN_SET);
|
|
char msg[] = "PC12=HIGH 10s, measure T5 pad\n";
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)msg, sizeof(msg)-1, 10);
|
|
HAL_Delay(10000);
|
|
/* Restore to UART AF */
|
|
gpio.Mode = GPIO_MODE_AF_PP;
|
|
gpio.Pull = GPIO_PULLUP;
|
|
gpio.Alternate = GPIO_AF8_UART5;
|
|
HAL_GPIO_Init(GPIOC, &gpio);
|
|
char done[] = "PC12 restored to UART5\n";
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)done, sizeof(done)-1, 10);
|
|
break;
|
|
}
|
|
|
|
/* Baud rate sweep test: try multiple baud rates on UART5, 3s each */
|
|
case 'R': {
|
|
extern UART_HandleTypeDef huart2;
|
|
uint16_t start = 0xABCD;
|
|
int16_t steer = 0, speed = 50;
|
|
uint16_t checksum = start ^ (uint16_t)steer ^ (uint16_t)speed;
|
|
uint8_t pkt[8];
|
|
pkt[0] = start & 0xFF; pkt[1] = (start >> 8) & 0xFF;
|
|
pkt[2] = steer & 0xFF; pkt[3] = (steer >> 8) & 0xFF;
|
|
pkt[4] = speed & 0xFF; pkt[5] = (speed >> 8) & 0xFF;
|
|
pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF;
|
|
|
|
/* Also report current BRR and APB1 freq */
|
|
uint32_t brr = UART5->BRR;
|
|
uint32_t apb1 = HAL_RCC_GetPCLK1Freq();
|
|
char info[80];
|
|
int n = snprintf(info, sizeof(info), "BRR=%lu APB1=%lu\n",
|
|
(unsigned long)brr, (unsigned long)apb1);
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)info, n, 20);
|
|
|
|
const uint32_t bauds[] = {115200, 38400, 9600, 19200, 57600, 230400};
|
|
for (int b = 0; b < 6; b++) {
|
|
/* Reconfigure baud */
|
|
huart2.Init.BaudRate = bauds[b];
|
|
HAL_UART_Init(&huart2);
|
|
|
|
char msg[40];
|
|
int mn = snprintf(msg, sizeof(msg), "BAUD %lu...\n", (unsigned long)bauds[b]);
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)msg, mn, 20);
|
|
|
|
/* Send for 3 sec */
|
|
for (int i = 0; i < 150; i++) {
|
|
HAL_UART_Transmit(&huart2, pkt, 8, 5);
|
|
HAL_Delay(20);
|
|
}
|
|
/* Brief stop between bauds */
|
|
HAL_Delay(500);
|
|
}
|
|
|
|
/* Stop motors, restore 115200 */
|
|
speed = 0; checksum = start;
|
|
pkt[4] = 0; pkt[5] = 0;
|
|
pkt[6] = checksum & 0xFF; pkt[7] = (checksum >> 8) & 0xFF;
|
|
huart2.Init.BaudRate = 115200;
|
|
HAL_UART_Init(&huart2);
|
|
for (int i = 0; i < 50; i++) {
|
|
HAL_UART_Transmit(&huart2, pkt, 8, 5);
|
|
HAL_Delay(20);
|
|
}
|
|
char ack[] = "R:done\n";
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)ack, 7, 10);
|
|
break;
|
|
}
|
|
#endif /* DEBUG_MOTOR_TEST */
|
|
|
|
case '?': {
|
|
/* Status dump for debugging */
|
|
char st[128];
|
|
int n = snprintf(st, sizeof(st),
|
|
"ST cal=%d estop=%d pitch=? hb=%lu\n",
|
|
mpu6000_is_calibrated() ? 1 : 0,
|
|
safety_remote_estop_active() ? 1 : 0,
|
|
(unsigned long)jetson_hb_tick);
|
|
HAL_UART_Transmit(&huart6, (uint8_t*)st, n, 20);
|
|
break;
|
|
}
|
|
|
|
/* PID tuning commands */
|
|
case 'P': case 'I': case 'K': case 'T': case 'M': {
|
|
uint8_t copy_len = len < 31 ? len : 31;
|
|
for (uint8_t i = 0; i < copy_len; i++)
|
|
cdc_cmd_buf[i] = buf[i];
|
|
cdc_cmd_buf[copy_len] = '\0';
|
|
cdc_cmd_ready = 1;
|
|
break;
|
|
}
|
|
|
|
default: break;
|
|
}
|
|
}
|
|
|
|
/* IRQ handler — call from stm32 interrupt vector */
|
|
void USART6_IRQHandler(void) {
|
|
HAL_UART_IRQHandler(&huart6);
|
|
}
|