mirror of
https://github.com/okalachev/flix.git
synced 2026-01-10 04:57:17 +00:00
c8e5e08b03
As it makes the subsystems code easier to understand. Declare the most used variables in main sketch file as forward declarations. Make all control input zero by default (except controlMode). Minor changes.
45 lines
1.2 KiB
C++
45 lines
1.2 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"
|
|
|
|
Vector rates; // estimated angular rates, rad/s
|
|
Quaternion attitude; // estimated attitude
|
|
bool landed;
|
|
|
|
float accWeight = 0.003;
|
|
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
|
|
|
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) * accWeight;
|
|
|
|
// apply correction
|
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
|
}
|