From 2180415f5c26690c7bd04a3e0654448670d99315 Mon Sep 17 00:00:00 2001 From: Josh Date: Thu, 23 Apr 2026 22:30:41 +0200 Subject: [PATCH] =?UTF-8?q?Fix=20quaternion-to-yaw=20formula:=20yaw=20now?= =?UTF-8?q?=20starts=20at=200=C2=B0=20instead=20of=20180=C2=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The previous formula had both atan2 arguments negated relative to the standard ZYX Euler extraction: wrong: atan2(-2*q1*q2 - 2*q0*q3, 2*q2^2 + 2*q3^2 - 1) correct: atan2( 2*q1*q2 + 2*q0*q3, 1 - 2*q2^2 - 2*q3^2) Since atan2(-y,-x) = atan2(y,x) ± 180°, the identity quaternion [1,0,0,0] produced atan2(0,-1) = 180° instead of atan2(0,1) = 0°. This bug caused yaw to always initialise at 180° when no magnetometer calibration is available. Roll and pitch formulas are unaffected. Co-Authored-By: Claude Sonnet 4.6 --- IMU.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/IMU.cpp b/IMU.cpp index 9516cee..1017059 100644 --- a/IMU.cpp +++ b/IMU.cpp @@ -430,9 +430,11 @@ void imuDataGet(EulerAngles *pstAngles, pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f; pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1, -2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f; + // Standard ZYX Euler: yaw = atan2(2*(q0*q3 + q1*q2), 1 - 2*(q2^2 + q3^2)) + // Previous formula had both signs inverted → always started at 180° instead of 0°. const float quaternionYawDeg = wrapDegrees360( - atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3, - 2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * kRadToDeg); + atan2f(2.0f * q1 * q2 + 2.0f * q0 * q3, + 1.0f - 2.0f * q2 * q2 - 2.0f * q3 * q3) * kRadToDeg); float headingDeg = 0.0f; if (useMagneticHeading &&