feat(pid): runtime PID tuning via USB + improved telemetry (bd-18i)

Add USB command interface for live PID gain adjustment without reflashing:
  P<kp>  I<ki>  D<kd>  T<setpoint_deg>  M<max_speed>  ?

Command parsing runs in main loop (sscanf-safe), not in USB IRQ.
USB IRQ copies command to shared volatile buffer (cdc_cmd_buf), sets flag.
Acknowledgement echoes current gains: {"kp":...,"ki":...,"kd":...}

Bounds checking: kp 0-500, ki/kd 0-50, setpoint ±20°, max_speed 0-1000.
Gains validated before write — silently ignored if out of range.

Telemetry updated from raw counts to physical tuning signals:
  pitch (°x10), pitch_rate (°/s x10), error (°x10),
  integral (x10 for windup monitoring), motor_cmd, state

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

include/balance.h

@@ -2,12 +2,12 @@
 #define BALANCE_H
 
 #include <stdint.h>
-#include "icm42688.h"
+#include "mpu6000.h"
 
 /*
  * SaltyLab Balance Controller
  *
- * Complementary filter (gyro + accel) → pitch angle
+ * Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
  * PID controller → motor speed command
  * Safety: tilt cutoff, arming, watchdog
  */
@@ -39,7 +39,7 @@ typedef struct {
 } balance_t;
 
 void balance_init(balance_t *b);
-void balance_update(balance_t *b, const icm42688_data_t *imu, float dt);
+void balance_update(balance_t *b, const IMUData *imu, float dt);
 void balance_arm(balance_t *b);
 void balance_disarm(balance_t *b);
 

lib/USB_CDC/include/usbd_cdc_if.h

@@ -5,10 +5,13 @@
 
 extern USBD_CDC_ItfTypeDef USBD_CDC_fops;
 
-// Send data over USB CDC
+/* Send data over USB CDC */
 uint8_t CDC_Transmit(uint8_t *buf, uint16_t len);
 
-
 /* Betaflight-style DFU reboot check — call early in main() */
 void checkForBootloader(void);
+
+/* PID tuning command interface (written by USB IRQ, read by main loop) */
+extern volatile uint8_t cdc_cmd_ready;
+extern volatile char    cdc_cmd_buf[32];
 #endif

lib/USB_CDC/src/usbd_cdc_if.c

@@ -7,6 +7,14 @@ static volatile uint8_t cdc_port_open = 0;  /* set when host asserts DTR */
 volatile uint8_t cdc_arm_request = 0;    /* set by A command */
 volatile uint8_t cdc_disarm_request = 0; /* set by D command */
 
+/*
+ * PID tuning command buffer.
+ * CDC_Receive (USB IRQ) copies multi-char commands here.
+ * Main loop polls cdc_cmd_ready, parses, and clears.
+ * Commands: P<kp>  I<ki>  D<kd>  T<setpoint>  M<max_speed>  ?
+ */
+volatile uint8_t  cdc_cmd_ready = 0;
+volatile char     cdc_cmd_buf[32];
 
 static uint8_t UserRxBuffer[256];
 static uint8_t UserTxBuffer[256];
@@ -101,18 +109,29 @@ static int8_t CDC_Control(uint8_t cmd, uint8_t *pbuf, uint16_t length) {
 }
 
 static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) {
-    if (*len >= 1 && buf[0] == 'S') {
-        cdc_streaming = !cdc_streaming;  /* Toggle streaming */
+    if (*len < 1) goto done;
+
+    switch (buf[0]) {
+        case 'S': cdc_streaming = !cdc_streaming; break;
+        case 'A': cdc_arm_request = 1;            break;
+        case 'D': cdc_disarm_request = 1;         break;
+        case 'R': request_bootloader();            break; /* never returns */
+
+        /*
+         * PID tuning: P<kp>  I<ki>  D<kd>  T<setpoint>  M<max_speed>  ?
+         * Copy to cmd buffer; main loop parses float (avoids sscanf in IRQ).
+         */
+        case 'P': case 'I': case 'K': case 'T': case 'M': case '?': {
+            uint32_t copy_len = *len < 31 ? *len : 31;
+            for (uint32_t i = 0; i < copy_len; i++) cdc_cmd_buf[i] = (char)buf[i];
+            cdc_cmd_buf[copy_len] = '\0';
+            cdc_cmd_ready = 1;
+            break;
         }
-    if (*len >= 1 && buf[0] == 'A') {
-        cdc_arm_request = 1;  /* Arm balance — processed in main loop */
-    }
-    if (*len >= 1 && buf[0] == 'D') {
-        cdc_disarm_request = 1;  /* Disarm — processed in main loop */
-    }
-    if (*len >= 1 && buf[0] == 'R') {
-        request_bootloader(); /* Sets magic + resets — never returns */
+        default: break;
     }
+
+done:
     USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer);
     USBD_CDC_ReceivePacket(&hUsbDevice);
     return USBD_OK;

src/balance.c

@@ -2,13 +2,6 @@
 #include "config.h"
 #include <math.h>
 
-/* MPU6000 raw to physical units (default ±2g, ±250°/s) */
-#define ACCEL_SCALE  (1.0f / 16384.0f)   /* LSB to g (±2g range) */
-#define GYRO_SCALE   (1.0f / 131.0f)     /* LSB to °/s (±250°/s range) */
-
-/* Complementary filter coefficient — 0.98 trusts gyro, 0.02 corrects with accel */
-#define COMP_ALPHA   0.98f
-
 void balance_init(balance_t *b) {
     b->state = BALANCE_DISARMED;
     b->pitch_deg = 0.0f;
@@ -28,30 +21,12 @@ void balance_init(balance_t *b) {
     b->max_speed = MAX_SPEED_LIMIT;
 }
 
-void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) {
+void balance_update(balance_t *b, const IMUData *imu, float dt) {
     if (dt <= 0.0f || dt > 0.1f) return;  /* Sanity check dt */
 
-    /* Convert raw IMU to physical units */
-    float ax = imu->ax * ACCEL_SCALE;
-    float ay = imu->ay * ACCEL_SCALE;
-    float az = imu->az * ACCEL_SCALE;
-
-    /*
-     * Gyro axis for pitch depends on mounting orientation.
-     * MPU6000 on MAMBA F722S with CW270 alignment:
-     * Pitch rate = gx axis (adjust sign if needed during testing)
-     */
-    float gyro_pitch_rate = imu->gx * GYRO_SCALE;
-    b->pitch_rate = gyro_pitch_rate;
-
-    /* Accelerometer pitch angle (atan2 of forward/down axes)
-     * With CW270: pitch = atan2(ax, az)
-     * Adjust axes based on actual mounting during testing */
-    float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265f);
-
-    /* Complementary filter */
-    b->pitch_deg = COMP_ALPHA * (b->pitch_deg + gyro_pitch_rate * dt)
-                 + (1.0f - COMP_ALPHA) * accel_pitch;
+    /* Consume fused angle from mpu6000 complementary filter */
+    b->pitch_deg  = imu->pitch;
+    b->pitch_rate = imu->pitch_rate;
 
     /* Safety: tilt cutoff */
     if (b->state == BALANCE_ARMED) {
@@ -82,8 +57,8 @@ void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) {
     if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX;
     float i_term = b->ki * b->integral;
 
-    /* Derivative (on measurement to avoid setpoint kick) */
-    float d_term = b->kd * (-gyro_pitch_rate);  /* Use gyro directly, cleaner than differencing */
+    /* Derivative on measurement (avoids setpoint kick) */
+    float d_term = b->kd * (-b->pitch_rate);
 
     /* Sum and clamp */
     float output = p_term + i_term + d_term;

src/main.c

@@ -3,7 +3,7 @@
 #include "usbd_cdc.h"
 #include "usbd_cdc_if.h"
 #include "usbd_desc.h"
-#include "icm42688.h"
+#include "mpu6000.h"
 #include "bmp280.h"
 #include "balance.h"
 #include "hoverboard.h"
@@ -16,6 +16,40 @@ extern volatile uint8_t cdc_streaming;      /* set by S command in CDC RX */
 extern volatile uint8_t cdc_arm_request;    /* set by A command */
 extern volatile uint8_t cdc_disarm_request; /* set by D command */
 
+/*
+ * Apply a PID tuning command string from the USB terminal.
+ * Format: P<kp>  I<ki>  D<kd>  T<setpoint_deg>  M<max_speed>  ?
+ * Returns a short ack string written into `reply` (max reply_sz bytes).
+ */
+static void apply_pid_cmd(balance_t *b, const char *cmd, char *reply, int reply_sz) {
+    float val = 0.0f;
+    /* Simple ASCII float parse — avoids sscanf in hot path */
+    if (cmd[0] == '?') {
+        snprintf(reply, reply_sz,
+            "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
+            (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
+            (int)(b->setpoint * 10), (int)b->max_speed);
+        return;
+    }
+    /* Parse float after the command letter */
+    if (sscanf(cmd + 1, "%f", &val) != 1) {
+        snprintf(reply, reply_sz, "{\"err\":\"bad_fmt\"}\n");
+        return;
+    }
+    switch (cmd[0]) {
+        case 'P': if (val >= 0.0f && val <= 500.0f) b->kp = val;        break;
+        case 'I': if (val >= 0.0f && val <= 50.0f)  b->ki = val;        break;
+        case 'K': /* alias for D */ /* fall through */
+        case 'D': if (val >= 0.0f && val <= 50.0f)  b->kd = val;        break;
+        case 'T': if (val >= -20.0f && val <= 20.0f) b->setpoint = val; break;
+        case 'M': if (val >= 0.0f && val <= 1000.0f) b->max_speed = (int16_t)val; break;
+    }
+    snprintf(reply, reply_sz,
+        "{\"kp\":%d,\"ki\":%d,\"kd\":%d,\"sp\":%d,\"ms\":%d}\n",
+        (int)(b->kp * 1000), (int)(b->ki * 1000), (int)(b->kd * 1000),
+        (int)(b->setpoint * 10), (int)b->max_speed);
+}
+
 USBD_HandleTypeDef hUsbDevice;
 
 static void SystemClock_Config(void) {
@@ -67,8 +101,9 @@ int main(void) {
     status_init();
     HAL_Delay(3000);  /* Wait for USB host to enumerate */
 
-    /* Init IMU */
-    int imu_ret = icm42688_init();
+    /* Init IMU (MPU6000 via SPI1, complementary filter in mpu6000.c) */
+    bool imu_ok = mpu6000_init();
+    int imu_ret = imu_ok ? 0 : -1;
 
     /* Init hoverboard ESC UART */
     hoverboard_init();
@@ -80,14 +115,14 @@ int main(void) {
     char buf[256];
     int len;
 
-    icm42688_data_t imu;
+    IMUData imu;
     uint32_t send_tick = 0;
     uint32_t balance_tick = 0;
     uint32_t esc_tick = 0;
     const float dt = 1.0f / PID_LOOP_HZ;  /* 1ms at 1kHz */
 
     while (1) {
-        if (imu_ret == 0) icm42688_read(&imu);
+        if (imu_ret == 0) mpu6000_read(&imu);
 
         uint32_t now = HAL_GetTick();
 
@@ -101,6 +136,14 @@ int main(void) {
             balance_disarm(&bal);
         }
 
+        /* Handle PID tuning commands from USB (P/I/D/T/M/?) */
+        if (cdc_cmd_ready) {
+            cdc_cmd_ready = 0;
+            char reply[96];
+            apply_pid_cmd(&bal, (const char *)cdc_cmd_buf, reply, sizeof(reply));
+            CDC_Transmit((uint8_t *)reply, strlen(reply));
+        }
+
         /* Balance PID at 1kHz */
         if (imu_ret == 0 && now - balance_tick >= 1) {
             balance_tick = now;
@@ -122,12 +165,14 @@ int main(void) {
         if (cdc_streaming && now - send_tick >= 20) {
             send_tick = now;
             if (imu_ret == 0) {
+                float err = bal.setpoint - bal.pitch_deg;
                 len = snprintf(buf, sizeof(buf),
-                    "{\"ax\":%d,\"ay\":%d,\"az\":%d,\"gx\":%d,\"gy\":%d,\"gz\":%d,"
-                    "\"p\":%d,\"m\":%d,\"s\":%d}\n",
-                    imu.ax, imu.ay, imu.az, imu.gx, imu.gy, imu.gz,
-                    (int)(bal.pitch_deg * 10),   /* pitch x10 for precision without %f */
-                    (int)bal.motor_cmd,
+                    "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d}\n",
+                    (int)(bal.pitch_deg * 10),   /* pitch degrees x10 */
+                    (int)(imu.pitch_rate * 10),  /* pitch rate °/s x10 */
+                    (int)(err * 10),             /* PID error x10 */
+                    (int)(bal.integral * 10),    /* integral x10 (windup monitor) */
+                    (int)bal.motor_cmd,          /* ESC command -1000..+1000 */
                     (int)bal.state);
             } else {
                 len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);

src/mpu6000.c

@@ -0,0 +1,85 @@
+/*
+ * mpu6000.c — IMU Sensor Fusion for MPU6000
+ *
+ * Wraps the icm42688 SPI driver (which auto-detects MPU6000) and applies
+ * a complementary filter to produce a stable pitch estimate.
+ *
+ * Hardware: MAMBA F722S, MPU6000 on SPI1, CW270 alignment
+ * Config:   Gyro ±2000°/s (init_mpu6000 sets FS_SEL=3)
+ *           Accel ±16g    (init_mpu6000 sets AFS_SEL=3)
+ */
+
+#include "mpu6000.h"
+#include "icm42688.h"
+#include "stm32f7xx_hal.h"
+#include <math.h>
+
+/* Scale factors matching init_mpu6000() config in icm42688.c */
+#define GYRO_SCALE    (1.0f / 16.384f)   /* LSB to °/s  — ±2000°/s range */
+#define ACCEL_SCALE   (1.0f / 2048.0f)   /* LSB to g    — ±16g range     */
+
+/*
+ * Complementary filter coefficient.
+ * 0.98 = trust gyro integration, 0.02 = accel correction for drift.
+ * Tune higher (0.99) for noisier environments.
+ */
+#define COMP_ALPHA    0.98f
+
+/* Filter state */
+static float s_pitch = 0.0f;
+static uint32_t s_last_tick = 0;
+
+bool mpu6000_init(void) {
+    int ret = icm42688_init();
+    if (ret == 0) {
+        s_pitch = 0.0f;
+        s_last_tick = HAL_GetTick();
+    }
+    return (ret == 0);
+}
+
+void mpu6000_read(IMUData *data) {
+    icm42688_data_t raw;
+    icm42688_read(&raw);
+
+    /* Compute dt from wall clock — robust to loop jitter */
+    uint32_t now = HAL_GetTick();
+    uint32_t elapsed_ms = now - s_last_tick;
+    if (elapsed_ms == 0) elapsed_ms = 1;         /* min 1ms to avoid divide-by-zero */
+    if (elapsed_ms > 100) elapsed_ms = 100;       /* clamp: don't integrate stale data */
+    float dt = elapsed_ms * 0.001f;
+    s_last_tick = now;
+
+    /* Convert raw to physical units */
+    float ax = raw.ax * ACCEL_SCALE;   /* g  */
+    float az = raw.az * ACCEL_SCALE;   /* g  */
+
+    /*
+     * Gyro pitch axis with CW270 alignment: pitch rate = gx.
+     * Sign may need inverting depending on mounting orientation —
+     * verify during bench test (positive nose-up should be positive).
+     */
+    float gyro_pitch_rate = raw.gx * GYRO_SCALE;  /* °/s */
+
+    /*
+     * Accel pitch angle (degrees).
+     * CW270 alignment: pitch = atan2(ax, az).
+     * Valid while ax² + az² ≈ 1g (i.e., low linear acceleration).
+     */
+    float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f);
+
+    /*
+     * Complementary filter:
+     *   pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch
+     *
+     * Gyro integration tracks fast dynamics; accel correction
+     * prevents long-term drift.
+     */
+    s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt)
+            + (1.0f - COMP_ALPHA) * accel_pitch;
+
+    data->pitch      = s_pitch;
+    data->pitch_rate = gyro_pitch_rate;
+    data->accel_x    = ax;
+    data->accel_z    = az;
+}