2 Commits

Author SHA1 Message Date
Oleg Kalachev
0547ea548b Add parameters for acc weight and rates lpf alpha 2025-12-24 05:43:55 +03:00
Oleg Kalachev
c02dba6812 Rename gyroCalibrationFilter to gyroBiasFilter
Which seems a better name
2025-12-24 05:36:43 +03:00
3 changed files with 8 additions and 6 deletions

View File

@@ -8,8 +8,8 @@
#include "lpf.h"
#include "util.h"
#define WEIGHT_ACC 0.003
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
float accWeight = 0.003;
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
void estimate() {
applyGyro();
@@ -18,7 +18,6 @@ void estimate() {
void applyGyro() {
// filter gyro to get angular rates
static LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
rates = ratesFilter.update(gyro);
// apply rates to attitude
@@ -34,7 +33,7 @@ void applyAcc() {
// calculate accelerometer correction
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
Vector correction = Vector::rotationVectorBetween(acc, up) * accWeight;
// apply correction
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));

View File

@@ -53,8 +53,8 @@ void calibrateGyroOnce() {
static Delay landedDelay(2);
if (!landedDelay.update(landed)) return; // calibrate only if definitely stationary
static LowPassFilter<Vector> gyroCalibrationFilter(0.001);
gyroBias = gyroCalibrationFilter.update(gyro);
static LowPassFilter<Vector> gyroBiasFilter(0.001);
gyroBias = gyroBiasFilter.update(gyro);
}
void calibrateAccel() {

View File

@@ -49,6 +49,9 @@ Parameter parameters[] = {
{"IMU_ACC_SCALE_X", &accScale.x},
{"IMU_ACC_SCALE_Y", &accScale.y},
{"IMU_ACC_SCALE_Z", &accScale.z},
// estimate
{"EST_ACC_WEIGHT", &accWeight},
{"EST_RATES_LPF_A", &ratesFilter.alpha},
// rc
{"RC_ZERO_0", &channelZero[0]},
{"RC_ZERO_1", &channelZero[1]},