Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Simplify code using angularRatesBetweenVectors
Oleg Kalachev4fcf2109cea3
diff --git a/flix/control.ino b/flix/control.inoindex 387a12b..9ac1510 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -115,12 +115,10 @@ void controlAttitude()Vector upActual = attitude.rotate(up);Vector upTarget = attitudeTarget.rotate(up);- float angle = Vector::angleBetweenVectors(upTarget, upActual);- Vector ratesTargetDir = Vector::angularRatesBetweenVectors(upTarget, upActual);- ratesTargetDir.normalize();+ Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);- ratesTarget.x = rollPID.update(ratesTargetDir.x * angle, dt);- ratesTarget.y = pitchPID.update(ratesTargetDir.y * angle, dt);+ ratesTarget.x = rollPID.update(error.x, dt);+ ratesTarget.y = pitchPID.update(error.y, dt);if (yawMode == YAW) {ratesTarget.z = yawPID.update(wrapAngle(attitudeTarget.getYaw() - attitude.getYaw()), dt);diff --git a/flix/estimate.ino b/flix/estimate.inoindex 713d538..b8445a7 100644--- a/flix/estimate.ino+++ b/flix/estimate.ino@@ -36,12 +36,10 @@ void applyAcc()// calculate accelerometer correctionVector 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();}diff --git a/flix/vector.h b/flix/vector.hindex 105f6df..577d46c 100644--- a/flix/vector.h+++ b/flix/vector.h@@ -79,7 +79,10 @@ public:static Vector angularRatesBetweenVectors(const Vector& u, const Vector& v){- return cross(u, v);+ Vector direction = cross(u, v);+ direction.normalize();+ float angle = angleBetweenVectors(u, v);+ return direction * angle;}size_t printTo(Print& p) const {