Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Implement `rotate` method for quaternions as replace for multiplication Vector rotating method is renamed from `rotate` to `rotateVector` to avoid inconsistent object and argument order in different `rotate` methods

Oleg Kalachevca032abc03cb
diff --git a/flix/control.ino b/flix/control.ino
index 77c4a87..56fe5bf 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -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);
diff --git a/flix/estimate.ino b/flix/estimate.ino
index b0fae96..ea1834f 100644
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -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));
}
diff --git a/flix/quaternion.h b/flix/quaternion.h
index 19aeda6..62e10d6 100644
--- a/flix/quaternion.h
+++ b/flix/quaternion.h
@@ -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);
}