114 lines
3.3 KiB
C
114 lines
3.3 KiB
C
#include "balance.h"
|
|
#include "config.h"
|
|
#include <math.h>
|
|
|
|
/* MPU6000 raw to physical units (default ±2g, ±250°/s) */
|
|
#define ACCEL_SCALE (1.0f / 16384.0f) /* LSB to g (±2g range) */
|
|
#define GYRO_SCALE (1.0f / 131.0f) /* LSB to °/s (±250°/s range) */
|
|
|
|
/* Complementary filter coefficient — 0.98 trusts gyro, 0.02 corrects with accel */
|
|
#define COMP_ALPHA 0.98f
|
|
|
|
void balance_init(balance_t *b) {
|
|
b->state = BALANCE_DISARMED;
|
|
b->pitch_deg = 0.0f;
|
|
b->pitch_rate = 0.0f;
|
|
b->integral = 0.0f;
|
|
b->prev_error = 0.0f;
|
|
b->motor_cmd = 0;
|
|
|
|
/* Default PID — conservative starting point */
|
|
b->kp = PID_KP;
|
|
b->ki = PID_KI;
|
|
b->kd = PID_KD;
|
|
b->setpoint = 0.0f;
|
|
|
|
/* Safety defaults from config */
|
|
b->max_tilt = MAX_TILT_DEG;
|
|
b->max_speed = MAX_SPEED_LIMIT;
|
|
}
|
|
|
|
void balance_update(balance_t *b, const icm42688_data_t *imu, float dt) {
|
|
if (dt <= 0.0f || dt > 0.1f) return; /* Sanity check dt */
|
|
|
|
/* Convert raw IMU to physical units */
|
|
float ax = imu->ax * ACCEL_SCALE;
|
|
float ay = imu->ay * ACCEL_SCALE;
|
|
float az = imu->az * ACCEL_SCALE;
|
|
|
|
/*
|
|
* Gyro axis for pitch depends on mounting orientation.
|
|
* MPU6000 on MAMBA F722S with CW270 alignment:
|
|
* Pitch rate = gx axis (adjust sign if needed during testing)
|
|
*/
|
|
float gyro_pitch_rate = imu->gx * GYRO_SCALE;
|
|
b->pitch_rate = gyro_pitch_rate;
|
|
|
|
/* Accelerometer pitch angle (atan2 of forward/down axes)
|
|
* With CW270: pitch = atan2(ax, az)
|
|
* Adjust axes based on actual mounting during testing */
|
|
float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265f);
|
|
|
|
/* Complementary filter */
|
|
b->pitch_deg = COMP_ALPHA * (b->pitch_deg + gyro_pitch_rate * dt)
|
|
+ (1.0f - COMP_ALPHA) * accel_pitch;
|
|
|
|
/* Safety: tilt cutoff */
|
|
if (b->state == BALANCE_ARMED) {
|
|
if (fabsf(b->pitch_deg) > b->max_tilt) {
|
|
b->state = BALANCE_TILT_FAULT;
|
|
b->motor_cmd = 0;
|
|
b->integral = 0.0f;
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (b->state != BALANCE_ARMED) {
|
|
b->motor_cmd = 0;
|
|
b->integral = 0.0f;
|
|
b->prev_error = 0.0f;
|
|
return;
|
|
}
|
|
|
|
/* PID */
|
|
float error = b->setpoint - b->pitch_deg;
|
|
|
|
/* Proportional */
|
|
float p_term = b->kp * error;
|
|
|
|
/* Integral with anti-windup */
|
|
b->integral += error * dt;
|
|
if (b->integral > PID_INTEGRAL_MAX) b->integral = PID_INTEGRAL_MAX;
|
|
if (b->integral < -PID_INTEGRAL_MAX) b->integral = -PID_INTEGRAL_MAX;
|
|
float i_term = b->ki * b->integral;
|
|
|
|
/* Derivative (on measurement to avoid setpoint kick) */
|
|
float d_term = b->kd * (-gyro_pitch_rate); /* Use gyro directly, cleaner than differencing */
|
|
|
|
/* Sum and clamp */
|
|
float output = p_term + i_term + d_term;
|
|
if (output > (float)b->max_speed) output = (float)b->max_speed;
|
|
if (output < -(float)b->max_speed) output = -(float)b->max_speed;
|
|
|
|
b->motor_cmd = (int16_t)output;
|
|
b->prev_error = error;
|
|
}
|
|
|
|
void balance_arm(balance_t *b) {
|
|
if (b->state == BALANCE_DISARMED) {
|
|
/* Only arm if roughly upright */
|
|
if (fabsf(b->pitch_deg) < 10.0f) {
|
|
b->integral = 0.0f;
|
|
b->prev_error = 0.0f;
|
|
b->motor_cmd = 0;
|
|
b->state = BALANCE_ARMED;
|
|
}
|
|
}
|
|
}
|
|
|
|
void balance_disarm(balance_t *b) {
|
|
b->state = BALANCE_DISARMED;
|
|
b->motor_cmd = 0;
|
|
b->integral = 0.0f;
|
|
}
|