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 &&