Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

Improve vector and quaternion libraries Make the order or basic methods consistent between Vector and Quaternion. Remove `ZYX` from Euler method names as this is standard for robotics. Rename angular rates to rotation vector, which is more correct. Make rotation methods static, to keep the arguments order consistent. Make `Quaternion::fromAxisAngle` accept Vector for axis. Minor fixes.

Oleg Kalachev6b7601c0bd31
diff --git a/flix/cli.ino b/flix/cli.ino
index 36d281d..a21a746 100644
--- a/flix/cli.ino
+++ b/flix/cli.ino
@@ -101,7 +101,7 @@ void doCommand(String str, bool echo = false) {
print("Loop rate: %.0f\n", loopRate);
print("dt: %f\n", dt);
} else if (command == "ps") {
- Vector a = attitude.toEulerZYX();
+ Vector a = attitude.toEuler();
print("roll: %f pitch: %f yaw: %f\n", degrees(a.x), degrees(a.y), degrees(a.z));
} else if (command == "psq") {
print("qx: %f qy: %f qz: %f qw: %f\n", attitude.x, attitude.y, attitude.z, attitude.w);
diff --git a/flix/control.ino b/flix/control.ino
index dd02bbb..0b3ff32 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -95,7 +95,7 @@ void interpretRC() {
} else if (mode == STAB) {
yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE;
- attitudeTarget = Quaternion::fromEulerZYX(Vector(
+ attitudeTarget = Quaternion::fromEuler(Vector(
controls[rollChannel] * tiltMax,
controls[pitchChannel] * tiltMax,
attitudeTarget.getYaw()));
@@ -122,10 +122,10 @@ void controlAttitude() {
}
const Vector up(0, 0, 1);
- Vector upActual = attitude.rotateVector(up);
- Vector upTarget = attitudeTarget.rotateVector(up);
+ Vector upActual = Quaternion::rotateVector(up, attitude);
+ Vector upTarget = Quaternion::rotateVector(up, attitudeTarget);
- Vector error = Vector::angularRatesBetweenVectors(upTarget, upActual);
+ Vector error = Vector::rotationVectorBetween(upTarget, upActual);
ratesTarget.x = rollPID.update(error.x, dt);
ratesTarget.y = pitchPID.update(error.y, dt);
diff --git a/flix/estimate.ino b/flix/estimate.ino
index 3d947d2..086fb34 100644
--- a/flix/estimate.ino
+++ b/flix/estimate.ino
@@ -23,7 +23,7 @@ void applyGyro() {
rates = ratesFilter.update(gyro);
// apply rates to attitude
- attitude = attitude.rotate(Quaternion::fromAngularRates(rates * dt));
+ attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(rates * dt));
}
void applyAcc() {
@@ -34,9 +34,9 @@ void applyAcc() {
if (!landed) return;
// calculate accelerometer correction
- Vector up = attitude.rotateVector(Vector(0, 0, 1));
- Vector correction = Vector::angularRatesBetweenVectors(acc, up) * WEIGHT_ACC;
+ Vector up = Quaternion::rotateVector(Vector(0, 0, 1), attitude);
+ Vector correction = Vector::rotationVectorBetween(acc, up) * WEIGHT_ACC;
// apply correction
- attitude = attitude.rotate(Quaternion::fromAngularRates(correction));
+ attitude = Quaternion::rotate(attitude, Quaternion::fromRotationVector(correction));
}
diff --git a/flix/log.ino b/flix/log.ino
index 0257da4..1a942ef 100644
--- a/flix/log.ino
+++ b/flix/log.ino
@@ -41,8 +41,8 @@ float logBuffer[LOG_SIZE][logColumns];
void prepareLogData() {
tFloat = t;
- attitudeEuler = attitude.toEulerZYX();
- attitudeTargetEuler = attitudeTarget.toEulerZYX();
+ attitudeEuler = attitude.toEuler();
+ attitudeTargetEuler = attitudeTarget.toEuler();
}
void logData() {
diff --git a/flix/quaternion.h b/flix/quaternion.h
index d8ab190..b61eeb8 100644
--- a/flix/quaternion.h
+++ b/flix/quaternion.h
@@ -15,22 +15,22 @@ public:
Quaternion(float w, float x, float y, float z): w(w), x(x), y(y), z(z) {};
- static Quaternion fromAxisAngle(float a, float b, float c, float angle) {
+ static Quaternion fromAxisAngle(const Vector& axis, float angle) {
float halfAngle = angle * 0.5;
float sin2 = sin(halfAngle);
float cos2 = cos(halfAngle);
- float sinNorm = sin2 / sqrt(a * a + b * b + c * c);
- return Quaternion(cos2, a * sinNorm, b * sinNorm, c * sinNorm);
+ float sinNorm = sin2 / axis.norm();
+ return Quaternion(cos2, axis.x * sinNorm, axis.y * sinNorm, axis.z * sinNorm);
}
- static Quaternion fromAngularRates(const Vector& rates) {
- if (rates.zero()) {
+ static Quaternion fromRotationVector(const Vector& rotation) {
+ if (rotation.zero()) {
return Quaternion();
}
- return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
+ return Quaternion::fromAxisAngle(rotation, rotation.norm());
}
- static Quaternion fromEulerZYX(const Vector& euler) {
+ static Quaternion fromEuler(const Vector& euler) {
float cx = cos(euler.x / 2);
float cy = cos(euler.y / 2);
float cz = cos(euler.z / 2);
@@ -60,14 +60,37 @@ public:
return ret;
}
- void toAxisAngle(float& a, float& b, float& c, float& angle) const {
+ bool finite() const {
+ return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
+ }
+
+ float norm() const {
+ return sqrt(w * w + x * x + y * y + z * z);
+ }
+
+ void normalize() {
+ float n = norm();
+ w /= n;
+ x /= n;
+ y /= n;
+ z /= n;
+ }
+
+ void toAxisAngle(Vector& axis, float& angle) const {
angle = acos(w) * 2;
- a = x / sin(angle / 2);
- b = y / sin(angle / 2);
- c = z / sin(angle / 2);
+ axis.x = x / sin(angle / 2);
+ axis.y = y / sin(angle / 2);
+ axis.z = z / sin(angle / 2);
+ }
+
+ Vector toRotationVector() const {
+ float angle;
+ Vector axis;
+ toAxisAngle(axis, angle);
+ return angle * axis;
}
- Vector toEulerZYX() const {
+ Vector toEuler() const {
// https://github.com/ros/geometry2/blob/589caf083cae9d8fae7effdb910454b4681b9ec1/tf2/include/tf2/impl/utils.h#L87
Vector euler;
float sqx = x * x;
@@ -112,18 +135,9 @@ public:
void setYaw(float yaw) {
// TODO: optimize?
- Vector euler = toEulerZYX();
+ Vector euler = toEuler();
euler.z = yaw;
- (*this) = Quaternion::fromEulerZYX(euler);
- }
-
- Quaternion& operator *= (const Quaternion& q) {
- Quaternion ret(
- w * q.w - x * q.x - y * q.y - z * q.z,
- w * q.x + x * q.w + y * q.z - z * q.y,
- w * q.y + y * q.w + z * q.x - x * q.z,
- w * q.z + z * q.w + x * q.y - y * q.x);
- return (*this = ret);
+ (*this) = Quaternion::fromEuler(euler);
}
Quaternion operator * (const Quaternion& q) const {
@@ -143,18 +157,6 @@ public:
-z * normSqInv);
}
- float norm() const {
- return sqrt(w * w + x * x + y * y + z * z);
- }
-
- void normalize() {
- float n = norm();
- w /= n;
- x /= n;
- y /= n;
- z /= n;
- }
-
Vector conjugate(const Vector& v) const {
Quaternion qv(0, v.x, v.y, v.z);
Quaternion res = (*this) * qv * inversed();
@@ -167,22 +169,27 @@ public:
return Vector(res.x, res.y, res.z);
}
- // Rotate vector by quaternion
- Vector rotateVector(const Vector& v) const {
- return conjugateInversed(v);
- }
-
// Rotate quaternion by quaternion
- Quaternion rotate(const Quaternion& q, const bool normalize = true) const {
- Quaternion rotated = (*this) * q;
+ static Quaternion rotate(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
+ Quaternion rotated = a * b;
if (normalize) {
rotated.normalize();
}
return rotated;
}
- bool finite() const {
- return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
+ // Rotate vector by quaternion
+ static Vector rotateVector(const Vector& v, const Quaternion& q) {
+ return q.conjugateInversed(v);
+ }
+
+ // Quaternion between two quaternions a and b
+ static Quaternion between(const Quaternion& a, const Quaternion& b, const bool normalize = true) {
+ Quaternion q = a * b.inversed();
+ if (normalize) {
+ q.normalize();
+ }
+ return q;
}
size_t printTo(Print& p) const {
diff --git a/flix/vector.h b/flix/vector.h
index 2dc4a0a..b2d9a89 100644
--- a/flix/vector.h
+++ b/flix/vector.h
@@ -13,14 +13,18 @@ public:
Vector(float x, float y, float z): x(x), y(y), z(z) {};
- float norm() const {
- return sqrt(x * x + y * y + z * z);
- }
-
bool zero() const {
return x == 0 && y == 0 && z == 0;
}
+ bool finite() const {
+ return isfinite(x) && isfinite(y) && isfinite(z);
+ }
+
+ float norm() const {
+ return sqrt(x * x + y * y + z * z);
+ }
+
void normalize() {
float n = norm();
x /= n;
@@ -74,10 +78,6 @@ public:
return !(*this == b);
}
- bool finite() const {
- return isfinite(x) && isfinite(y) && isfinite(z);
- }
-
static float dot(const Vector& a, const Vector& b) {
return a.x * b.x + a.y * b.y + a.z * b.z;
}
@@ -86,18 +86,18 @@ public:
return Vector(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
- static float angleBetweenVectors(const Vector& a, const Vector& b) {
+ static float angleBetween(const Vector& a, const Vector& b) {
return acos(constrain(dot(a, b) / (a.norm() * b.norm()), -1, 1));
}
- static Vector angularRatesBetweenVectors(const Vector& a, const Vector& b) {
+ static Vector rotationVectorBetween(const Vector& a, const Vector& b) {
Vector direction = cross(a, b);
if (direction.zero()) {
// vectors are opposite, return any perpendicular vector
return cross(a, Vector(1, 0, 0));
}
direction.normalize();
- float angle = angleBetweenVectors(a, b);
+ float angle = angleBetween(a, b);
return direction * angle;
}