aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 17:49:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Make fromEulerZYX accept Vector instead of x, y, z

Browse Source
This commit is contained in:
Oleg Kalachev 2024-02-20 04:51:59 +03:00
parent 85182ac2b8
commit 5ec6b5e665
2 changed files with 11 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flix/control.ino
View File

@ -89,10 +89,10 @@ void interpretRC() {
} else if (mode == STAB) { } else if (mode == STAB) {
yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE; 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_ROLL] * MAX_TILT,
-controls[RC_CHANNEL_PITCH] * MAX_TILT, -controls[RC_CHANNEL_PITCH] * MAX_TILT,
attitudeTarget.getYaw()); attitudeTarget.getYaw()));
ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX; ratesTarget.z = controls[RC_CHANNEL_YAW] * YAWRATE_MAX;
} else if (mode == MANUAL) { } else if (mode == MANUAL) {

19
flix/quaternion.h
View File

@ -30,13 +30,13 @@ public:
return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm()); return Quaternion::fromAxisAngle(rates.x, rates.y, rates.z, rates.norm());
} }
static Quaternion fromEulerZYX(float x, float y, float z) { static Quaternion fromEulerZYX(const Vector& euler) {
float cx = cos(x / 2); float cx = cos(euler.x / 2);
float cy = cos(y / 2); float cy = cos(euler.y / 2);
float cz = cos(z / 2); float cz = cos(euler.z / 2);
float sx = sin(x / 2); float sx = sin(euler.x / 2);
float sy = sin(y / 2); float sy = sin(euler.y / 2);
float sz = sin(z / 2); float sz = sin(euler.z / 2);
return Quaternion( return Quaternion(
cx * cy * cz + sx * sy * sz, cx * cy * cz + sx * sy * sz,
@ -99,9 +99,7 @@ public:
float sqy = y * y; float sqy = y * y;
float sqz = z * z; float sqz = z * z;
float sqw = w * w; float sqw = w * w;
double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw); double sarg = -2 * (x * z - w * y) / (sqx + sqy + sqz + sqw);
if (sarg <= -0.99999) { if (sarg <= -0.99999) {
yaw = -2 * atan2(y, x); yaw = -2 * atan2(y, x);
} else if (sarg >= 0.99999) { } else if (sarg >= 0.99999) {
@ -115,7 +113,8 @@ public:
void setYaw(float yaw) { void setYaw(float yaw) {
// TODO: optimize? // TODO: optimize?
Vector euler = toEulerZYX(); Vector euler = toEulerZYX();
(*this) = Quaternion::fromEulerZYX(euler.x, euler.y, yaw); euler.z = yaw;
(*this) = Quaternion::fromEulerZYX(euler);
} }
Quaternion& operator *= (const Quaternion& q) { Quaternion& operator *= (const Quaternion& q) {