Merge pull request 'feat(pid): runtime PID tuning via USB + telemetry (bd-18i)' (#3) from sl-controls/bd-18i-pid-tuning into main

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') {
+    if (*len < 1) goto done;
-        cdc_streaming = !cdc_streaming;  /* Toggle streaming */
+
+    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') {
+        default: break;
-        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 */
     }
+
+done:
     USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer);
     USBD_CDC_ReceivePacket(&hUsbDevice);
     return USBD_OK;

src/main.c

@@ -15,6 +15,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) {
@@ -101,6 +135,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,11 +164,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),
-                    "{\"p\":%d,\"r\":%d,\"m\":%d,\"s\":%d}\n",
+                    "{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d}\n",
-                    (int)(imu.pitch * 10),       /* fused pitch degrees x10 */
+                    (int)(bal.pitch_deg * 10),   /* pitch degrees x10 */
-                    (int)(imu.pitch_rate * 10),  /* gyro pitch rate °/s x10 */
+                    (int)(imu.pitch_rate * 10),  /* pitch rate °/s x10 */
-                    (int)bal.motor_cmd,
+                    (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);