Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Simplify code using angularRatesBetweenVectors

Oleg Kalachev4fcf2109cea3
diff --git a/flix/control.ino b/flix/control.ino
index 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.ino
index 713d538..b8445a7 100644
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -36,12 +36,10 @@ void applyAcc()
// calculate accelerometer correction
Vector 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.h
index 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 {