Added external magnetometer functionality
Dependencies: HMC58X31 MODI2C MPU6050 MS561101BA
Fork of FreeIMU by
Diff: FreeIMU.cpp
- Revision:
- 2:5c419926dcd7
- Parent:
- 1:794e9cdbc2a0
- Child:
- 3:f9b100a9aa65
--- a/FreeIMU.cpp Tue Nov 05 11:31:39 2013 +0000 +++ b/FreeIMU.cpp Tue Nov 05 13:38:07 2013 +0000 @@ -150,9 +150,9 @@ float xh = values[6]*cos(pitch)+values[7]*sin(roll)*sin(pitch)-values[8]*cos(roll)*sin(pitch); float yh = values[7]*cos(roll)+values[8]*sin(roll); - float yaw = atan2(yh, xh); + float yaw = -atan2(yh, xh); - float rollOver2 = roll * 0.5f; + float rollOver2 = (roll + M_PI) * 0.5f; float sinRollOver2 = (float)sin(rollOver2); float cosRollOver2 = (float)cos(rollOver2); float pitchOver2 = pitch * 0.5f; @@ -162,10 +162,10 @@ float sinYawOver2 = (float)sin(yawOver2); float cosYawOver2 = (float)cos(yawOver2); - q0 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; - q1 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; - q2 = - cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2; - q3 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; + q0 = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; + q1 = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; + q2 = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; + q3 = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; if (q!=NULL){ q[0] = q0;