Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Make fromEulerZYX accept Vector instead of x, y, z
Oleg Kalachev5ec6b5e6651f
diff --git a/flix/control.ino b/flix/control.inoindex 225e592..3f97a7f 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -89,10 +89,10 @@ void interpretRC() {} else if (mode == STAB) {yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;- attitudeTarget = Quaternion::fromEulerZYX(+ attitudeTarget = Quaternion::fromEulerZYX(Vector(controls[RC_CHANNEL_ROLL] * MAX_TILT,-controls[RC_CHANNEL_PITCH] * MAX_TILT,- attitudeTarget.getYaw());+ attitudeTarget.getYaw()));ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;} else if (mode == MANUAL) {diff --git a/flix/quaternion.h b/flix/quaternion.hindex c535d63..07d8504 100644--- a/flix/quaternion.h+++ b/flix/quaternion.h@@ -30,13 +30,13 @@ public:return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());}- static Quaternion fromEulerZYX(float x, float y, float z) {- float cx = cos(x / 2);- float cy = cos(y / 2);- float cz = cos(z / 2);- float sx = sin(x / 2);- float sy = sin(y / 2);- float sz = sin(z / 2);+ static Quaternion fromEulerZYX(const Vector& euler) {+ float cx = cos(euler.x / 2);+ float cy = cos(euler.y / 2);+ float cz = cos(euler.z / 2);+ float sx = sin(euler.x / 2);+ float sy = sin(euler.y / 2);+ float sz = sin(euler.z / 2);return Quaternion(cx * cy * cz + sx * sy * sz,@@ -99,9 +99,7 @@ public:float sqy = y * y;float sqz = z * z;float sqw = w * w;-double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);-if (sarg <= -0.99999) {yaw = -2 * atan2(y, x);} else if (sarg >= 0.99999) {@@ -115,7 +113,8 @@ public:void setYaw(float yaw) {// TODO: optimize?Vector euler = toEulerZYX();- (*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw);+ euler.z = yaw;+ (*this) = Quaternion::fromEulerZYX(euler);}Quaternion& operator *= (const Quaternion& q) {