mirror of
https://github.com/okalachev/flix.git
synced 2025-07-30 04:48:59 +00:00
6b7601c0bd
Make the order or basic methods consistent between Vector and Quaternion. Remove `ZYX` from Euler method names as this is standard for robotics. Rename angular rates to rotation vector, which is more correct. Make rotation methods static, to keep the arguments order consistent. Make `Quaternion::fromAxisAngle` accept Vector for axis. Minor fixes.
43 lines
1.1 KiB
C++
43 lines
1.1 KiB
C++
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Attitude estimation from gyro and accelerometer
|
|
|
|
#include "quaternion.h"
|
|
#include "vector.h"
|
|
#include "lpf.h"
|
|
#include "util.h"
|
|
|
|
#define WEIGHT_ACC 0.003
|
|
#define RATES_LFP_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
|
|
|
LowPassFilter<Vector> ratesFilter(RATES_LFP_ALPHA);
|
|
|
|
void estimate() {
|
|
applyGyro();
|
|
applyAcc();
|
|
}
|
|
|
|
void applyGyro() {
|
|
// filter gyro to get angular rates
|
|
rates = ratesFilter.update(gyro);
|
|
|
|
// apply rates to attitude
|
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
|
|
}
|
|
|
|
void applyAcc() {
|
|
// test should we apply accelerometer gravity correction
|
|
float accNorm = acc.norm();
|
|
landed = !motorsActive() && abs(accNorm - ONE_G) < ONE_G * 0.1f;
|
|
|
|
if (!landed) return;
|
|
|
|
// calculate accelerometer correction
|
|
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
|
Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
|
|
|
|
// apply correction
|
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
|
}
|