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

Simplify code using angularRatesBetweenVectors

Browse Source
This commit is contained in:
Oleg Kalachev
2023-12-13 08:25:11 +03:00
parent 24b62e5145
commit 4fcf2109ce
3 changed files with 9 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
flix/estimate.ino
View File

@@ -36,12 +36,10 @@ void applyAcc()
// 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;
Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
// apply correction
attitude *= Quaternion::fromAngularRates(accCorr);
attitude *= Quaternion::fromAngularRates(correction);
attitude.normalize();
}