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

Implement rotate method for quaternions as replace for multiplication

Browse Source 
Vector rotating method is renamed from `rotate` to `rotateVector` to avoid inconsistent object and argument order in different `rotate` methods
This commit is contained in:
Oleg Kalachev
2025-01-09 09:56:49 +03:00
parent 5d10446aaf
commit ca032abc03
3 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
flix/estimate.ino
View File

@@ -22,8 +22,7 @@ void applyGyro() {
rates = ratesFilter.update(gyro);
// apply rates to attitude
attitude *= Quaternion::fromAngularRates(rates * dt);
attitude.normalize();
attitude = attitude.rotate(Quaternion::fromAngularRates(rates * dt));
}
void applyAcc() {
@@ -34,10 +33,9 @@ void applyAcc() {
if (!landed) return;
// calculate accelerometer correction
Vector up = attitude.rotate(Vector(0, 0, 1));
Vector up = attitude.rotateVector(Vector(0, 0, 1));
Vector correction = Vector::angularRatesBetweenVectors(acc, up) * dt * WEIGHT_ACC;
// apply correction
attitude *= Quaternion::fromAngularRates(correction);
attitude.normalize();
attitude = attitude.rotate(Quaternion::fromAngularRates(correction));
}