sl-controls
6dfb5640db
Add src/mpu6000.c implementing a complementary filter (α=0.98) on top of the existing icm42688 SPI driver. Fixes wrong scale factors in balance.c (was ±250°/s / ±2g; hardware is configured ±2000°/s / ±16g). Fusion now lives in the IMU driver layer; balance_update() consumes IMUData directly. - mpu6000_init(): calls icm42688_init(), seeds filter state - mpu6000_read(): reads raw SPI, applies complementary filter, returns fused pitch (degrees) + pitch_rate (°/s) + accel_x/z (g) - balance.c: removes duplicated fusion code, uses IMUData.pitch - main.c: switches to mpu6000_init()/mpu6000_read(), updates telemetry Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
47 lines
1.2 KiB
C
47 lines
1.2 KiB
C
#ifndef BALANCE_H
|
|
#define BALANCE_H
|
|
|
|
#include <stdint.h>
|
|
#include "mpu6000.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, /* Active balancing */
|
|
BALANCE_TILT_FAULT, /* Tilt exceeded limit, motors killed */
|
|
} 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 */
|
|
} 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);
|
|
|
|
#endif
|