seb/saltylab-firmware
1
0
Fork 0
Code Issues Pull Requests 3 Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
sl-jetson 2026-02-28 13:12:13 -05:00
commit c719cf5467
3 changed files with 85 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
lib/USB_CDC/include/usbd_cdc_if.h
View File

@ -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

39
lib/USB_CDC/src/usbd_cdc_if.c
View File

@ -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;

53
src/main.c
View File

@ -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",
(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);