From 5ec6b5e6651f41b2c704e1c15cdc6b7166668ac9 Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 20 Feb 2024 04:51:59 +0300
Subject: [PATCH] Make fromEulerZYX accept Vector instead of x, y, z

---
 flix/control.ino  |  4 ++--
 flix/quaternion.h | 19 +++++++++----------
 2 files changed, 11 insertions(+), 12 deletions(-)

diff --git a/flix/control.ino b/flix/control.ino
index 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.h
index 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) {