47 lines
1.2 KiB
C
47 lines
1.2 KiB
C
#ifndef BALANCE_H
|
|
#define BALANCE_H
|
|
|
|
#include <stdint.h>
|
|
#include "icm42688.h"
|
|
|
|
/*
|
|
* SaltyLab Balance Controller
|
|
*
|
|
* Complementary filter (gyro + accel) → pitch angle
|
|
* 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 icm42688_data_t *imu, float dt);
|
|
void balance_arm(balance_t *b);
|
|
void balance_disarm(balance_t *b);
|
|
|
|
#endif
|