Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
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.inoindex 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.inoindex 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.hindex 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);}