mirror of
https://github.com/okalachev/flix.git
synced 2026-06-28 05:56:44 +00:00
Oleg Kalachev
bd270db493
Leverage a priori knowledge that the drone's average attitude is level. Explanation: https://t.me/opensourcequadcopter/158.
56 lines
1.5 KiB
Arduino
56 lines
1.5 KiB
Arduino
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Attitude estimation using 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;
|
|
float levelWeight = 0.0002;
|
|
LowPassFilter<Vector> ratesFilter(0.2); // cutoff frequency ~ 40 Hz
|
|
|
|
void estimate() {
|
|
applyGyro();
|
|
applyAcc();
|
|
applyLevel();
|
|
}
|
|
|
|
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));
|
|
}
|
|
|
|
void applyLevel() {
|
|
if (landed) return;
|
|
|
|
// assume the pilot keeps the drone more or less level in flight
|
|
Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
|
|
Vector correction = Vector::rotationVectorBetween(Vector(0, 0, 1), up) * levelWeight;
|
|
attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
|
|
}
|