diff --git a/flix/estimate.ino b/flix/estimate.ino index cd8da9f..cd8dcb9 100644 --- a/flix/estimate.ino +++ b/flix/estimate.ino @@ -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 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 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)); diff --git a/flix/parameters.ino b/flix/parameters.ino index db6350f..0738f12 100644 --- a/flix/parameters.ino +++ b/flix/parameters.ino @@ -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]},