aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-01-10 21:16:50 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add parameters for acc weight and rates lpf alpha

Browse Source
This commit is contained in:
Oleg Kalachev
2025-12-24 05:43:55 +03:00
parent c02dba6812
commit 0547ea548b
2 changed files with 6 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
flix/estimate.ino
View File

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

3
flix/parameters.ino
View File

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