mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 17:49:33 +00:00
53 lines
1.2 KiB
C++
53 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"
|
|
|
|
#define ONE_G 9.807f
|
|
#define ACC_MIN 0.9f
|
|
#define ACC_MAX 1.1f
|
|
#define WEIGHT_ACC 0.5f
|
|
|
|
void estimate()
|
|
{
|
|
applyGyro();
|
|
applyAcc();
|
|
signalizeHorizontality();
|
|
}
|
|
|
|
void applyGyro()
|
|
{
|
|
// applying gyro
|
|
attitude *= Quaternion::fromAngularRates(rates * dt);
|
|
attitude.normalize();
|
|
}
|
|
|
|
void applyAcc()
|
|
{
|
|
// test should we apply accelerometer gravity correction
|
|
float accNorm = acc.norm();
|
|
if (accNorm < ACC_MIN * ONE_G || accNorm > ACC_MAX * ONE_G) {
|
|
// use accelerometer only when we're not accelerating
|
|
return;
|
|
}
|
|
|
|
// calculate accelerometer correction
|
|
Vector up = attitude.rotate(Vector(0, 0, -1));
|
|
Vector accCorrDirection = Vector::angularRatesBetweenVectors(acc, up);
|
|
accCorrDirection.normalize();
|
|
Vector accCorr = accCorrDirection * Vector::angleBetweenVectors(up, acc) * dt * WEIGHT_ACC;
|
|
|
|
// apply correction
|
|
attitude *= Quaternion::fromAngularRates(accCorr);
|
|
attitude.normalize();
|
|
}
|
|
|
|
void signalizeHorizontality()
|
|
{
|
|
float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
|
|
setLED(angle < 15 * DEG_TO_RAD);
|
|
}
|