feat: motor bench test working — UART5 ESC + USART6 Jetson + W command

- Fix UART5 ESC: PC12=TX, PD2=RX @ 115200 baud (was USART2/38400)
- Add jetson_uart.c: USART6 command interface (A/D/E/Z/H/C/W/R/X/? commands)
- Add W command: persistent direct motor test (sets globals, main loop sends at 50Hz)
- Fix power_mgmt: keep UART5 clock active, PC7 EXTI wake source
- Fix main loop: direct_test_speed/steer override disarmed zero-send
- Add boot banner on USART6

Tested: Orin -> FC USART6 -> FC UART5 -> EFeru ESC -> both motors spin

.pio/build/project.checksum

@@ -1 +1 @@
-8700a44a6597bcade0f371945c539630ba0e78b1
+ffc01fb580c81760bdda9a672fe1212be4578e3e

include/hoverboard.h

@@ -14,7 +14,7 @@
  */
 
 #define HOVERBOARD_START_FRAME  0xABCD
-#define HOVERBOARD_BAUD         115200
+#define HOVERBOARD_BAUD         38400
 
 typedef struct __attribute__((packed)) {
     uint16_t start;

include/jetson_uart.h

@@ -0,0 +1,16 @@
+#ifndef JETSON_UART_H
+#define JETSON_UART_H
+
+#include <stdint.h>
+#include "stm32f7xx_hal.h"
+
+/* Initialize USART6 for Jetson communication (921600 baud) */
+void jetson_uart_init(void);
+
+/* Send data back to Jetson (telemetry, status) */
+void jetson_uart_send(const uint8_t *data, uint16_t len);
+
+/* Called from HAL_UART_RxCpltCallback — handles byte accumulation */
+void jetson_uart_rx_callback(UART_HandleTypeDef *huart);
+
+#endif /* JETSON_UART_H */

src/crsf.c

@@ -206,6 +206,10 @@ void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *h) {
 /* DMA complete: drain second half (buffer wrapped) */
 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *h) {
     if (h->Instance == UART4) dma_drain();
+    if (h->Instance == USART6) {
+        extern void jetson_uart_rx_callback(UART_HandleTypeDef *huart);
+        jetson_uart_rx_callback(h);
+    }
 }
 
 /* ------------------------------------------------------------------ */

src/esc_hoverboard.c

@@ -1,6 +1,7 @@
 #include "esc_backend.h"
 #include "config.h"
 #include "stm32f7xx_hal.h"
+#include <stdio.h>
 
 /*
  * Hoverboard ESC Backend Implementation
@@ -24,7 +25,7 @@ typedef struct __attribute__((packed)) {
     uint16_t checksum;
 } hoverboard_cmd_t;
 
-static UART_HandleTypeDef huart2;
+UART_HandleTypeDef huart2;  /* non-static: accessed by jetson_uart.c raw test */
 
 /* Backend vtable instance */
 static const esc_backend_t hoverboard_backend;
@@ -35,20 +36,27 @@ static const esc_backend_t hoverboard_backend;
  */
 static void hoverboard_backend_init(void) {
     /* Enable clocks */
-    __HAL_RCC_USART2_CLK_ENABLE();
-    __HAL_RCC_GPIOA_CLK_ENABLE();
+    __HAL_RCC_UART5_CLK_ENABLE();
+    __HAL_RCC_GPIOC_CLK_ENABLE();
 
     /* PA2=TX, PA3=RX, AF7 for USART2 */
     GPIO_InitTypeDef gpio = {0};
-    gpio.Pin = GPIO_PIN_2 | GPIO_PIN_3;
+    // UART5: PC12=TX, PD2=RX
+    __HAL_RCC_GPIOD_CLK_ENABLE();
+    gpio.Pin = GPIO_PIN_12;
     gpio.Mode = GPIO_MODE_AF_PP;
     gpio.Pull = GPIO_PULLUP;
     gpio.Speed = GPIO_SPEED_FREQ_HIGH;
-    gpio.Alternate = GPIO_AF7_USART2;
-    HAL_GPIO_Init(GPIOA, &gpio);
+    gpio.Alternate = GPIO_AF8_UART5;
+    HAL_GPIO_Init(GPIOC, &gpio);
+
+    // RX: PD2
+    gpio.Pin = GPIO_PIN_2;
+    gpio.Alternate = GPIO_AF8_UART5;
+    HAL_GPIO_Init(GPIOD, &gpio);
 
     /* USART2 config */
-    huart2.Instance = USART2;
+    huart2.Instance = UART5;
     huart2.Init.BaudRate = HOVERBOARD_BAUD;
     huart2.Init.WordLength = UART_WORDLENGTH_8B;
     huart2.Init.StopBits = UART_STOPBITS_1;
@@ -57,12 +65,36 @@ static void hoverboard_backend_init(void) {
     huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
     huart2.Init.OverSampling = UART_OVERSAMPLING_16;
     HAL_UART_Init(&huart2);
+
+    /* Diagnostic: report UART5 register state on USART6 after init */
+    {
+        extern void jetson_uart_send(const uint8_t *data, uint16_t len);
+        char diag[128];
+        uint32_t brr = UART5->BRR;
+        uint32_t cr1 = UART5->CR1;
+        uint32_t isr = UART5->ISR;
+        uint32_t apb1 = HAL_RCC_GetPCLK1Freq();
+        /* Also read GPIOC MODER to verify PC12 is in AF mode (bits 25:24 = 10) */
+        uint32_t moder = GPIOC->MODER;
+        uint32_t pc12_mode = (moder >> 24) & 0x3;  /* 0=input 1=output 2=AF 3=analog */
+        uint32_t afr = GPIOC->AFR[1]; /* AFR high for pins 8-15 */
+        uint32_t pc12_af = (afr >> 16) & 0xF; /* AF for pin 12 */
+        int n = snprintf(diag, sizeof(diag),
+            "UART5: BRR=%lu CR1=0x%lX ISR=0x%lX APB1=%luHz PC12mode=%lu AF=%lu\n",
+            (unsigned long)brr, (unsigned long)cr1, (unsigned long)isr,
+            (unsigned long)apb1, (unsigned long)pc12_mode, (unsigned long)pc12_af);
+        /* Delay to let USART6 finish boot banner first */
+        HAL_Delay(100);
+        jetson_uart_send((uint8_t*)diag, n);
+    }
 }
 
 /*
  * Send motor command via hoverboard protocol.
  * Called at ~50Hz from motor_driver_update().
  */
+static volatile uint32_t hover_tx_count = 0;
+
 static void hoverboard_backend_send(int16_t speed, int16_t steer) {
     hoverboard_cmd_t cmd;
     cmd.start = HOVERBOARD_START_FRAME;
@@ -70,7 +102,17 @@ static void hoverboard_backend_send(int16_t speed, int16_t steer) {
     cmd.speed = speed;
     cmd.checksum = cmd.start ^ cmd.steer ^ cmd.speed;
 
-    HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
+    HAL_StatusTypeDef rc = HAL_UART_Transmit(&huart2, (uint8_t *)&cmd, sizeof(cmd), 5);
+    hover_tx_count++;
+
+    /* Debug: every 50th send, report status on USART6 */
+    if (hover_tx_count % 50 == 1) {
+        extern void jetson_uart_send(const uint8_t *data, uint16_t len);
+        char dbg[64];
+        int n = snprintf(dbg, sizeof(dbg), "ESC tx=%lu rc=%d spd=%d str=%d\n",
+            (unsigned long)hover_tx_count, (int)rc, speed, steer);
+        jetson_uart_send((uint8_t*)dbg, n);
+    }
 }
 
 /*

src/jetson_uart.c

@@ -0,0 +1,285 @@
+/*
+ * 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;
+        }
+
+        /* 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;
+        }
+
+        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);
+}

src/main.c

@@ -17,6 +17,7 @@
 #include "mag.h"
 #include "bno055.h"
 #include "jetson_cmd.h"
+#include "jetson_uart.h"
 #include "jlink.h"
 #include "ota.h"
 #include "audio.h"
@@ -41,6 +42,10 @@ extern volatile uint32_t cdc_rx_count;       /* total CDC packets received */
 extern volatile uint8_t  cdc_estop_request;
 extern volatile uint8_t  cdc_estop_clear_request;
 
+/* Direct motor test (set by W command in jetson_uart.c) */
+volatile int16_t direct_test_speed = 0;
+volatile int16_t direct_test_steer = 0;
+
 /* BNO055 active flag (set if BNO055 initialized successfully) */
 static bool bno055_active = false;
 
@@ -168,6 +173,10 @@ int main(void) {
     /* Init Jetson serial binary protocol on USART1 (PB6/PB7) at 921600 baud */
     jlink_init();
 
+    /* Init Jetson UART command interface on USART6 (PC6/PC7) at 921600 baud.
+     * Mirrors CDC command protocol over hardware UART (fixes USB CDC TX bug). */
+    jetson_uart_init();
+
     /* Init I2S3 audio amplifier (PC10/PA15/PB5, mute=PC5) */
     audio_init();
     audio_play_tone(AUDIO_TONE_STARTUP);
@@ -473,6 +482,9 @@ int main(void) {
                 if (speed >  1000) speed =  1000;
                 if (speed < -1000) speed = -1000;
                 motor_driver_update(&motors, (int16_t)speed, steer, now);
+            } else if (direct_test_speed != 0 || direct_test_steer != 0) {
+                /* Direct motor test mode — set by W command */
+                esc_send(direct_test_speed, direct_test_steer);
             } else {
                 /* Always send zero while disarmed to prevent ESC timeout */
                 motor_driver_update(&motors, 0, 0, now);

src/power_mgmt.c

@@ -33,8 +33,10 @@ static void enable_wake_exti(void)
     HAL_NVIC_SetPriority(EXTI1_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(EXTI1_IRQn);
 
-    /* EXTI7: PB7 (USART1_RX) — SYSCFG EXTICR2[15:12] = 0001 (PB) */
-    SYSCFG->EXTICR[1] = (SYSCFG->EXTICR[1] & ~(0xFu << 12)) | (0x1u << 12);
+    /* EXTI7: PC7 (USART6_RX / Jetson UART) — SYSCFG EXTICR2[15:12] = 0010 (PC)
+     * Changed from PB7 (JLink) to PC7 (Jetson) — Jetson is primary interface.
+     * JLink wake is handled by Jetson timeout instead. */
+    SYSCFG->EXTICR[1] = (SYSCFG->EXTICR[1] & ~(0xFu << 12)) | (0x2u << 12);
     EXTI->FTSR |=  (1u << 7);
     EXTI->RTSR &= ~(1u << 7);
     EXTI->PR    =  (1u << 7);
@@ -64,8 +66,9 @@ static void gate_peripherals(void)
     if (s_peripherals_gated) return;
     __HAL_RCC_SPI3_CLK_DISABLE();    /* I2S3 / audio amplifier */
     __HAL_RCC_SPI2_CLK_DISABLE();    /* OSD MAX7456 */
-    __HAL_RCC_USART6_CLK_DISABLE();  /* legacy Jetson CDC */
-    __HAL_RCC_UART5_CLK_DISABLE();   /* debug UART */
+    // __HAL_RCC_USART6_CLK_DISABLE(); // kept active for Jetson UART  /* legacy Jetson CDC */
+    /* UART5 kept active — hoverboard ESC needs continuous comms */
+    /* __HAL_RCC_UART5_CLK_DISABLE(); */
     s_peripherals_gated = true;
 }