MrYoshii/WAVE_ROVER
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix quaternion-to-yaw formula: yaw now starts at 0° instead of 180°

Browse Source 
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 <noreply@anthropic.com>
This commit is contained in:
Josh
2026-04-23 22:30:41 +02:00
co-authored by Claude Sonnet 4.6
parent 996b38778f
commit 2180415f5c
1 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
IMU.cpp
+4 -2
View File
@@ -430,9 +430,11 @@ void imuDataGet(EulerAngles *pstAngles,
pstAngles->pitch = asinf(clampUnit(-2.0f * q1 * q3 + 2.0f * q0 * q2)) * 57.2957795f; 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, pstAngles->roll = atan2f(2.0f * q2 * q3 + 2.0f * q0 * q1,
-2.0f * q1 * q1 - 2.0f * q2 * q2 + 1.0f) * 57.2957795f; -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( const float quaternionYawDeg = wrapDegrees360(
atan2f(-2.0f * q1 * q2 - 2.0f * q0 * q3, atan2f(2.0f * q1 * q2 + 2.0f * q0 * q3,
2.0f * q2 * q2 + 2.0f * q3 * q3 - 1.0f) * kRadToDeg); 1.0f - 2.0f * q2 * q2 - 2.0f * q3 * q3) * kRadToDeg);
float headingDeg = 0.0f; float headingDeg = 0.0f;
if (useMagneticHeading && if (useMagneticHeading &&