From 34fdb5d11b1a453d5595450ed22a831f18b412b1 Mon Sep 17 00:00:00 2001 From: sl-controls Date: Sat, 28 Feb 2026 12:30:46 -0500 Subject: [PATCH] feat(pid): runtime PID tuning via USB + improved telemetry (bd-18i) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add USB command interface for live PID gain adjustment without reflashing: P I D T M ? 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 --- lib/USB_CDC/include/usbd_cdc_if.h | 7 ++-- lib/USB_CDC/src/usbd_cdc_if.c | 43 ++++++++++++++++++------- src/main.c | 53 ++++++++++++++++++++++++++++--- 3 files changed, 85 insertions(+), 18 deletions(-) diff --git a/lib/USB_CDC/include/usbd_cdc_if.h b/lib/USB_CDC/include/usbd_cdc_if.h index 19da466..4ca837c 100644 --- a/lib/USB_CDC/include/usbd_cdc_if.h +++ 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 diff --git a/lib/USB_CDC/src/usbd_cdc_if.c b/lib/USB_CDC/src/usbd_cdc_if.c index 4e9ddc6..e947d55 100644 --- a/lib/USB_CDC/src/usbd_cdc_if.c +++ b/lib/USB_CDC/src/usbd_cdc_if.c @@ -4,9 +4,17 @@ extern USBD_HandleTypeDef hUsbDevice; volatile uint8_t cdc_streaming = 1; /* auto-stream */ 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_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 I D T M ? + */ +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 && 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 */ + 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 I D T M ? + * 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; + } + default: break; } + +done: USBD_CDC_SetRxBuffer(&hUsbDevice, UserRxBuffer); USBD_CDC_ReceivePacket(&hUsbDevice); return USBD_OK; diff --git a/src/main.c b/src/main.c index 73fa8a4..40c6441 100644 --- a/src/main.c +++ b/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 I D T M ? + * 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", - (int)(imu.pitch * 10), /* fused pitch degrees x10 */ - (int)(imu.pitch_rate * 10), /* gyro pitch rate °/s x10 */ - (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);