aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2026-02-18 08:02:36 +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 "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));