sl-firmware
fa75c442a7
Archive STM32 firmware to legacy/stm32/: - src/, include/, lib/USB_CDC/, platformio.ini, test stubs, flash_firmware.py - test/test_battery_adc.c, test_hw_button.c, test_pid_schedule.c, test_vesc_can.c, test_can_watchdog.c - USB_CDC_BUG.md Rename: stm32_protocol → esp32_protocol, mamba_protocol → balance_protocol, stm32_cmd_node → esp32_cmd_node, stm32_cmd_params → esp32_cmd_params, stm32_cmd.launch.py → esp32_cmd.launch.py, test_stm32_protocol → test_esp32_protocol, test_stm32_cmd_node → test_esp32_cmd_node Content cleanup across all files: - Mamba F722S → ESP32-S3 BALANCE - BlackPill → ESP32-S3 IO - STM32F722/F7xx → ESP32-S3 - stm32Mode/Version/Port → esp32Mode/Version/Port - STM32 State/Mode labels → ESP32 State/Mode - Jetson Nano → Jetson Orin Nano Super - /dev/stm32 → /dev/esp32 - stm32_bridge → esp32_bridge - STM32 HAL → ESP-IDF docs/SALTYLAB.md: - Update "Drone FC Details" to describe ESP32-S3 BALANCE board (Waveshare ESP32-S3 Touch LCD 1.28) - Replace verbose "Self-Balancing Control" STM32 section with brief note pointing to SAUL-TEE-SYSTEM-REFERENCE.md TEAM.md: Update Embedded Firmware Engineer role to ESP32-S3 / ESP-IDF No new functionality — cleanup only. Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
54 lines
1.6 KiB
C
54 lines
1.6 KiB
C
#ifndef BALANCE_H
|
|
#define BALANCE_H
|
|
|
|
#include <stdint.h>
|
|
#include "mpu6000.h"
|
|
#include "slope_estimator.h"
|
|
|
|
/*
|
|
* SaltyLab Balance Controller
|
|
*
|
|
* Consumes fused IMUData (pitch + pitch_rate from mpu6000 complementary filter)
|
|
* PID controller → motor speed command
|
|
* Safety: tilt cutoff, arming, watchdog
|
|
*/
|
|
|
|
typedef enum {
|
|
BALANCE_DISARMED = 0, /* Motors off, waiting for arm command */
|
|
BALANCE_ARMED = 1, /* Active balancing */
|
|
BALANCE_TILT_FAULT = 2, /* Tilt exceeded limit, motors killed */
|
|
BALANCE_PARKED = 3, /* PID frozen, motors off — quick re-arm via button (Issue #682) */
|
|
} balance_state_t;
|
|
|
|
typedef struct {
|
|
/* State */
|
|
balance_state_t state;
|
|
float pitch_deg; /* Current pitch angle (degrees) */
|
|
float pitch_rate; /* Gyro pitch rate (deg/s) */
|
|
|
|
/* PID internals */
|
|
float integral;
|
|
float prev_error;
|
|
int16_t motor_cmd; /* Output to ESC: -1000..+1000 */
|
|
|
|
/* Tuning */
|
|
float kp, ki, kd;
|
|
float setpoint; /* Target pitch angle (degrees) — tune for COG offset */
|
|
|
|
/* Safety */
|
|
float max_tilt; /* Cutoff angle (degrees) */
|
|
int16_t max_speed; /* Speed limit */
|
|
|
|
/* Slope compensation (Issue #600) */
|
|
slope_estimator_t slope;
|
|
} balance_t;
|
|
|
|
void balance_init(balance_t *b);
|
|
void balance_update(balance_t *b, const IMUData *imu, float dt);
|
|
void balance_arm(balance_t *b);
|
|
void balance_disarm(balance_t *b);
|
|
void balance_park(balance_t *b); /* ARMED -> PARKED: freeze PID, zero motors (Issue #682) */
|
|
void balance_unpark(balance_t *b); /* PARKED -> ARMED if pitch < 20 deg (Issue #682) */
|
|
|
|
#endif
|