diff --git a/lib/USB_CDC/src/usbd_cdc_if.c b/lib/USB_CDC/src/usbd_cdc_if.c index 1aaffcf..a83c532 100644 --- a/lib/USB_CDC/src/usbd_cdc_if.c +++ b/lib/USB_CDC/src/usbd_cdc_if.c @@ -6,6 +6,7 @@ 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_disarm_request = 0; /* set by D command */ +volatile uint8_t cdc_recal_request = 0; /* set by G command — gyro recalibration */ /* * PID tuning command buffer. @@ -138,6 +139,7 @@ static int8_t CDC_Receive(uint8_t *buf, uint32_t *len) { case 'S': cdc_streaming = !cdc_streaming; break; case 'A': cdc_arm_request = 1; break; case 'D': cdc_disarm_request = 1; break; + case 'G': cdc_recal_request = 1; break; /* gyro recalibration */ case 'R': request_bootloader(); break; /* never returns */ /* diff --git a/src/main.c b/src/main.c index c5f87d5..583eb2b 100644 --- a/src/main.c +++ b/src/main.c @@ -23,6 +23,7 @@ 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 */ +extern volatile uint8_t cdc_recal_request; /* set by G command */ /* * Apply a PID tuning command string from the USB terminal. @@ -207,6 +208,14 @@ int main(void) { balance_disarm(&bal); } + /* Gyro recalibration — disarm first, then re-sample bias (~1s blocked) */ + if (cdc_recal_request) { + cdc_recal_request = 0; + safety_arm_cancel(); + balance_disarm(&bal); + if (imu_ret == 0) mpu6000_calibrate(); + } + /* Handle PID tuning commands from USB (P/I/D/T/M/?) */ if (cdc_cmd_ready) { cdc_cmd_ready = 0; diff --git a/src/mpu6000.c b/src/mpu6000.c index 443e671..0ec3f4b 100644 --- a/src/mpu6000.c +++ b/src/mpu6000.c @@ -12,6 +12,7 @@ #include "mpu6000.h" #include "icm42688.h" #include "config.h" +#include "safety.h" #include "stm32f7xx_hal.h" #include @@ -67,6 +68,8 @@ void mpu6000_calibrate(void) { sum_gy += raw.gy; sum_gz += raw.gz; HAL_Delay(1); + /* Refresh IWDG every 40ms — safe during re-cal with watchdog running */ + if (i % 40 == 39) safety_refresh(); } s_bias_gx = (float)sum_gx / GYRO_CAL_SAMPLES; diff --git a/src/safety.c b/src/safety.c index fdb93de..3e1d51f 100644 --- a/src/safety.c +++ b/src/safety.c @@ -42,7 +42,7 @@ void safety_init(void) { } void safety_refresh(void) { - HAL_IWDG_Refresh(&hiwdg); + if (hiwdg.Instance) HAL_IWDG_Refresh(&hiwdg); } bool safety_rc_alive(uint32_t now) { diff --git a/ui/index.html b/ui/index.html index fecb0dd..b9d2544 100644 --- a/ui/index.html +++ b/ui/index.html @@ -26,6 +26,8 @@ #arm-btn.armed { background: #ff2222; } #dfu-btn { background: #555; display: none; } #dfu-btn:hover { background: #777; } + #gyrocal-btn { background: #1a4a6a; display: none; } + #gyrocal-btn:hover { background: #2a6a9a; } #status { margin-top: 8px; font-size: 12px; color: #666; } #state-badge { display: inline-block; padding: 2px 8px; border-radius: 3px; @@ -62,6 +64,7 @@ +
WebSerial ready
@@ -311,6 +314,7 @@ window.toggleSerial = async function() { document.getElementById('arm-btn').style.display = 'none'; document.getElementById('dfu-btn').style.display = 'none'; document.getElementById('yaw-btn').style.display = 'none'; + document.getElementById('gyrocal-btn').style.display = 'none'; document.getElementById('status').textContent = 'Disconnected'; return; } @@ -325,6 +329,7 @@ window.toggleSerial = async function() { document.getElementById('arm-btn').style.display = 'inline-block'; document.getElementById('dfu-btn').style.display = 'inline-block'; document.getElementById('yaw-btn').style.display = 'inline-block'; + document.getElementById('gyrocal-btn').style.display = 'inline-block'; document.getElementById('status').textContent = 'Connected — streaming'; readLoop(); @@ -349,6 +354,11 @@ window.enterDFU = async function() { document.getElementById('status').textContent = 'Rebooting to DFU...'; }; +window.gyroRecal = async function() { + await sendCmd('G'); + document.getElementById('status').textContent = 'Calibrating gyro — hold robot still for 1s...'; +}; + async function readLoop() { const decoder = new TextDecoderStream(); port.readable.pipeTo(decoder.writable);