sl-controls
5f03e4cbef
Adds a slow-adapting terrain slope estimator (IIR tau=5s) that decouples the robot's balance offset from genuine ground incline. The balance controller subtracts the slope estimate from measured pitch so the PID balances around the slope surface rather than absolute vertical. - include/slope_estimator.h + src/slope_estimator.c: first-order IIR filter clamped to ±15°; JLINK_TLM_SLOPE (0x88) telemetry at 1 Hz - include/jlink.h + src/jlink.c: add JLINK_TLM_SLOPE (0x88), jlink_tlm_slope_t (4 bytes), jlink_send_slope_tlm() - include/balance.h + src/balance.c: integrate slope_estimator into balance_t; update, reset on tilt-fault and disarm - test/test_slope_estimator.c: 35 unit tests, all passing Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
51 lines
1.3 KiB
C
51 lines
1.3 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, /* 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 */
|
|
|
|
/* 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);
|
|
|
|
#endif
|