aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flix/control.ino
View File

@ -115,8 +115,8 @@ void controlAttitude() {
}
const Vector up(0, 0, 1);
Vector upActual = attitude.rotate(up);
Vector upTarget = attitudeTarget.rotate(up);
Vector upActual = attitude.rotateVector(up);
Vector upTarget = attitudeTarget.rotateVector(up);
Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);

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));
}

11
flix/quaternion.h
View File

@ -168,10 +168,19 @@ public:
}
// Rotate vector by quaternion
Vector rotate(const Vector& v) {
Vector rotateVector(const Vector& v) {
return conjugateInversed(v);
}
// Rotate quaternion by quaternion
Quaternion rotate(const Quaternion& q, const bool normalize = true) {
Quaternion rotated = (*this) * q;
if (normalize) {
rotated.normalize();
}
return rotated;
}
bool finite() const {
return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
}