sl-firmware
bd30e2b40d
Add 'G' CDC command that disarms and re-runs gyro bias calibration. safety_refresh() added to calibration loop (every 40ms) so IWDG does not trip during the 1s blocking re-cal when watchdog is running. GYRO CAL button in ui/index.html sends 'G' and shows status feedback. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
331 lines
13 KiB
C
331 lines
13 KiB
C
#include "stm32f7xx_hal.h"
|
||
#include "usbd_core.h"
|
||
#include "usbd_cdc.h"
|
||
#include "usbd_cdc_if.h"
|
||
#include "usbd_desc.h"
|
||
#include "mpu6000.h"
|
||
#include "balance.h"
|
||
#include "hoverboard.h"
|
||
#include "motor_driver.h"
|
||
#include "mode_manager.h"
|
||
#include "config.h"
|
||
#include "status.h"
|
||
#include "safety.h"
|
||
#include "crsf.h"
|
||
#include "i2c1.h"
|
||
#include "bmp280.h"
|
||
#include "mag.h"
|
||
#include "jetson_cmd.h"
|
||
#include <math.h>
|
||
#include <string.h>
|
||
#include <stdio.h>
|
||
|
||
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.
|
||
* 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) {
|
||
RCC_OscInitTypeDef osc = {0};
|
||
RCC_ClkInitTypeDef clk = {0};
|
||
RCC_PeriphCLKInitTypeDef pclk = {0};
|
||
__HAL_RCC_PWR_CLK_ENABLE();
|
||
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
|
||
osc.OscillatorType = RCC_OSCILLATORTYPE_HSE;
|
||
osc.HSEState = RCC_HSE_ON;
|
||
osc.PLL.PLLState = RCC_PLL_ON;
|
||
osc.PLL.PLLSource = RCC_PLLSOURCE_HSE;
|
||
osc.PLL.PLLM = 8; osc.PLL.PLLN = 432; osc.PLL.PLLP = 2; osc.PLL.PLLQ = 9;
|
||
if (HAL_RCC_OscConfig(&osc) != HAL_OK) {
|
||
osc.OscillatorType = RCC_OSCILLATORTYPE_HSI;
|
||
osc.HSIState = RCC_HSI_ON; osc.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
|
||
osc.HSEState = RCC_HSE_OFF; osc.PLL.PLLSource = RCC_PLLSOURCE_HSI; osc.PLL.PLLM = 16;
|
||
if (HAL_RCC_OscConfig(&osc) != HAL_OK) while (1);
|
||
}
|
||
HAL_PWREx_EnableOverDrive();
|
||
pclk.PeriphClockSelection = RCC_PERIPHCLK_CLK48 | RCC_PERIPHCLK_I2C1;
|
||
pclk.Clk48ClockSelection = RCC_CLK48SOURCE_PLLSAIP;
|
||
pclk.PLLSAI.PLLSAIN = 384; pclk.PLLSAI.PLLSAIQ = 7; pclk.PLLSAI.PLLSAIP = RCC_PLLSAIP_DIV8;
|
||
pclk.I2c1ClockSelection = RCC_I2C1CLKSOURCE_PCLK1;
|
||
if (HAL_RCCEx_PeriphCLKConfig(&pclk) != HAL_OK) while (1);
|
||
clk.ClockType = RCC_CLOCKTYPE_SYSCLK|RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
|
||
clk.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
|
||
clk.AHBCLKDivider = RCC_SYSCLK_DIV1;
|
||
clk.APB1CLKDivider = RCC_HCLK_DIV4; clk.APB2CLKDivider = RCC_HCLK_DIV2;
|
||
HAL_RCC_ClockConfig(&clk, FLASH_LATENCY_7);
|
||
}
|
||
|
||
extern PCD_HandleTypeDef hpcd;
|
||
void OTG_FS_IRQHandler(void) { HAL_PCD_IRQHandler(&hpcd); }
|
||
void SysTick_Handler(void) { HAL_IncTick(); }
|
||
|
||
int main(void) {
|
||
SCB_EnableICache();
|
||
/* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */
|
||
checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */
|
||
HAL_Init();
|
||
SystemClock_Config();
|
||
|
||
/* USB CDC */
|
||
USBD_Init(&hUsbDevice, &SaltyLab_Desc, 0);
|
||
USBD_RegisterClass(&hUsbDevice, &USBD_CDC);
|
||
USBD_CDC_RegisterInterface(&hUsbDevice, &USBD_CDC_fops);
|
||
USBD_Start(&hUsbDevice);
|
||
|
||
status_init();
|
||
HAL_Delay(3000); /* Wait for USB host to enumerate */
|
||
|
||
/* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */
|
||
int imu_ret = mpu6000_init() ? 0 : -1;
|
||
|
||
/*
|
||
* Gyro bias calibration — hold board still for ~1s.
|
||
* Must run before safety_init() (IWDG not yet started).
|
||
* Blocks arming and streaming until complete.
|
||
*/
|
||
if (imu_ret == 0) mpu6000_calibrate();
|
||
|
||
/* Init hoverboard ESC UART */
|
||
hoverboard_init();
|
||
|
||
/* Init balance controller */
|
||
balance_t bal;
|
||
balance_init(&bal);
|
||
|
||
/* Init motor driver */
|
||
motor_driver_t motors;
|
||
motor_driver_init(&motors);
|
||
|
||
/* Init mode manager (RC/autonomous blend; CH6 mode switch) */
|
||
mode_manager_t mode;
|
||
mode_manager_init(&mode);
|
||
|
||
/* Probe I2C1 for optional sensors — skip gracefully if not found */
|
||
int baro_chip = 0; /* chip_id: 0x58=BMP280, 0x60=BME280, 0=absent */
|
||
mag_type_t mag_type = MAG_NONE;
|
||
if (i2c1_init() == 0) {
|
||
int chip = bmp280_init();
|
||
baro_chip = (chip > 0) ? chip : 0;
|
||
mag_type = mag_init();
|
||
}
|
||
|
||
/*
|
||
* IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init()
|
||
* which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
|
||
* Once started, safety_refresh() MUST be called every WATCHDOG_TIMEOUT_MS
|
||
* or MCU resets. IWDG cannot be stopped once started (hardware enforced).
|
||
*/
|
||
safety_init();
|
||
|
||
char buf[256];
|
||
int len;
|
||
|
||
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) mpu6000_read(&imu);
|
||
|
||
uint32_t now = HAL_GetTick();
|
||
|
||
/* Feed hardware watchdog — must happen every WATCHDOG_TIMEOUT_MS */
|
||
safety_refresh();
|
||
|
||
/* Mode manager: update RC liveness, CH6 mode selection, blend ramp */
|
||
mode_manager_update(&mode, now);
|
||
|
||
/* RC CH5 kill switch: disarm immediately if RC is alive and CH5 off.
|
||
* Applies regardless of active mode (CH5 always has kill authority). */
|
||
if (mode.rc_alive && !crsf_state.armed && bal.state == BALANCE_ARMED) {
|
||
safety_arm_cancel();
|
||
balance_disarm(&bal);
|
||
}
|
||
/* RC signal lost while armed — disarm for safety */
|
||
if (bal.state == BALANCE_ARMED && !safety_rc_alive(now)) {
|
||
/* Uncomment when CRSF is wired (last_rx_ms stays 0 on stub): */
|
||
/* balance_disarm(&bal); */
|
||
}
|
||
|
||
/* Tilt fault buzzer alert (one-shot on fault edge) */
|
||
safety_alert_tilt_fault(bal.state == BALANCE_TILT_FAULT);
|
||
|
||
/* Handle arm/disarm requests from USB with arm-hold interlock */
|
||
if (cdc_arm_request) {
|
||
cdc_arm_request = 0;
|
||
/* Block arming until gyro calibration is complete */
|
||
if (mpu6000_is_calibrated() &&
|
||
bal.state == BALANCE_DISARMED && fabsf(bal.pitch_deg) < 10.0f) {
|
||
safety_arm_start(now);
|
||
}
|
||
}
|
||
/* Complete arming once hold timer expires */
|
||
if (safety_arm_ready(now) && bal.state == BALANCE_DISARMED) {
|
||
safety_arm_cancel();
|
||
balance_arm(&bal);
|
||
}
|
||
if (cdc_disarm_request) {
|
||
cdc_disarm_request = 0;
|
||
safety_arm_cancel();
|
||
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;
|
||
char reply[96];
|
||
apply_pid_cmd(&bal, (const char *)cdc_cmd_buf, reply, sizeof(reply));
|
||
CDC_Transmit((uint8_t *)reply, strlen(reply));
|
||
}
|
||
|
||
/* Handle Jetson C<speed>,<steer> command (parsed here, not in ISR) */
|
||
if (jetson_cmd_ready) {
|
||
jetson_cmd_ready = 0;
|
||
jetson_cmd_process();
|
||
}
|
||
|
||
/* Balance PID at 1kHz — apply Jetson speed offset when active */
|
||
if (imu_ret == 0 && now - balance_tick >= 1) {
|
||
balance_tick = now;
|
||
float base_sp = bal.setpoint;
|
||
if (jetson_cmd_is_active(now)) bal.setpoint += jetson_cmd_sp_offset();
|
||
balance_update(&bal, &imu, dt);
|
||
bal.setpoint = base_sp;
|
||
}
|
||
|
||
/* Latch estop on tilt fault or disarm */
|
||
if (bal.state == BALANCE_TILT_FAULT) {
|
||
motor_driver_estop(&motors);
|
||
} else if (bal.state == BALANCE_DISARMED && motors.estop) {
|
||
motor_driver_estop_clear(&motors);
|
||
}
|
||
|
||
/* Feed autonomous steer from Jetson into mode manager.
|
||
* mode_manager_get_steer() blends it with RC CH4 per active mode. */
|
||
if (jetson_cmd_is_active(now))
|
||
mode_manager_set_auto_cmd(&mode, jetson_cmd_steer(), 0);
|
||
|
||
/* Send to hoverboard ESC at 50Hz (every 20ms) */
|
||
if (now - esc_tick >= 20) {
|
||
esc_tick = now;
|
||
if (bal.state == BALANCE_ARMED) {
|
||
/* Blended steer (RC ↔ auto per mode) + RC speed bias */
|
||
int16_t steer = mode_manager_get_steer(&mode);
|
||
int16_t spd_bias = mode_manager_get_speed_bias(&mode);
|
||
int32_t speed = (int32_t)bal.motor_cmd + spd_bias;
|
||
if (speed > 1000) speed = 1000;
|
||
if (speed < -1000) speed = -1000;
|
||
motor_driver_update(&motors, (int16_t)speed, steer, now);
|
||
} else {
|
||
/* Always send zero while disarmed to prevent ESC timeout */
|
||
motor_driver_update(&motors, 0, 0, now);
|
||
}
|
||
}
|
||
|
||
/* USB telemetry at 50Hz (only when streaming enabled and calibration done) */
|
||
if (cdc_streaming && mpu6000_is_calibrated() && now - send_tick >= 20) {
|
||
send_tick = now;
|
||
if (imu_ret == 0) {
|
||
float err = bal.setpoint - bal.pitch_deg;
|
||
/* Build JSON incrementally; append optional sensor fields */
|
||
char *p = buf;
|
||
int rem = (int)sizeof(buf);
|
||
int n;
|
||
n = snprintf(p, rem,
|
||
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d",
|
||
(int)(bal.pitch_deg * 10), /* pitch degrees x10 */
|
||
(int)(imu.roll * 10), /* roll degrees 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,
|
||
(int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated) */
|
||
p += n; rem -= n;
|
||
n = snprintf(p, rem, ",\"md\":%d",
|
||
(int)mode_manager_active(&mode)); /* 0=MANUAL,1=ASSISTED,2=AUTO */
|
||
p += n; rem -= n;
|
||
if (mag_type != MAG_NONE) {
|
||
int16_t hd = mag_read_heading();
|
||
if (hd >= 0)
|
||
n = snprintf(p, rem, ",\"hd\":%d", hd); /* heading deg×10 */
|
||
else
|
||
n = snprintf(p, rem, ",\"hd\":-1"); /* not ready */
|
||
p += n; rem -= n;
|
||
}
|
||
if (baro_chip) {
|
||
int32_t pres_pa; int16_t temp_x10;
|
||
bmp280_read(&pres_pa, &temp_x10);
|
||
int32_t alt_cm = bmp280_pressure_to_alt_cm(pres_pa);
|
||
/* alt cm, temp °C×10, pressure hPa×10 (Pa÷10 = hPa×10) */
|
||
n = snprintf(p, rem, ",\"alt\":%ld,\"t\":%d,\"pa\":%ld",
|
||
(long)alt_cm, (int)temp_x10, (long)(pres_pa / 10));
|
||
p += n; rem -= n;
|
||
if (baro_chip == 0x60) { /* BME280: add humidity %RH×10 */
|
||
int16_t hum_x10 = bmp280_read_humidity();
|
||
if (hum_x10 >= 0) {
|
||
n = snprintf(p, rem, ",\"h\":%d", (int)hum_x10);
|
||
p += n; rem -= n;
|
||
}
|
||
}
|
||
}
|
||
n = snprintf(p, rem, "}\n");
|
||
len = (int)(p + n - buf);
|
||
} else {
|
||
len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
|
||
}
|
||
CDC_Transmit((uint8_t *)buf, len);
|
||
}
|
||
|
||
status_update(now, (imu_ret == 0),
|
||
(bal.state == BALANCE_ARMED),
|
||
(bal.state == BALANCE_TILT_FAULT));
|
||
HAL_Delay(1);
|
||
}
|
||
}
|