It is the library published by sparkfun, edited accordingly to make it work under mbed platform.
Dependents: MPU9250-dmp-bluepill MPU9250-dmp
Revision 4:2c4e849b8ecf, committed 2017-08-15
- Comitter:
- mbedoguz
- Date:
- Tue Aug 15 10:49:44 2017 +0000
- Parent:
- 2:c35f8379f2cb
- Commit message:
- computeEulerAngles are corrected to below issue ; https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5
Changed in this revision
SparkFunMPU9250-DMP.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/SparkFunMPU9250-DMP.cpp Fri Aug 11 07:43:08 2017 +0000 +++ b/SparkFunMPU9250-DMP.cpp Tue Aug 15 10:49:44 2017 +0000 @@ -8,12 +8,11 @@ It is based on their Emedded MotionDriver 6.12 library. https://www.invensense.com/developers/software-downloads/ -Development environment specifics: -Arduino IDE 1.6.12 -SparkFun 9DoF Razor IMU M0 - Supported Platforms: -- ATSAMD21 (Arduino Zero, SparkFun SAMD21 Breakouts) +any mbed enabled system. +Corrections: +computeEulerAngles function under Sparkfun-DMP-lib is corrected according to issue below. +Now angles are correct too. https://github.com/sparkfun/SparkFun_MPU-9250-DMP_Arduino_Library/issues/5 ******************************************************************************/ #include "SparkFunMPU9250-DMP.h" #include "MPU9250_RegisterMap.h" @@ -620,21 +619,30 @@ float dqx = qToFloat(qx, 30); float dqy = qToFloat(qy, 30); float dqz = qToFloat(qz, 30); - + + float norm = sqrt(dqw*dqw + dqx*dqx + dqy*dqy + dqz*dqz); + dqw = dqw/norm; + dqx = dqx/norm; + dqy = dqy/norm; + dqz = dqz/norm; + float ysqr = dqy * dqy; - float t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f; - float t1 = +2.0f * (dqx * dqy - dqw * dqz); - float t2 = -2.0f * (dqx * dqz + dqw * dqy); - float t3 = +2.0f * (dqy * dqz - dqw * dqx); - float t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f; - - // Keep t2 within range of asin (-1, 1) - t2 = t2 > 1.0f ? 1.0f : t2; - t2 = t2 < -1.0f ? -1.0f : t2; - - pitch = asin(t2) * 2; - roll = atan2(t3, t4); - yaw = atan2(t1, t0); + + // roll (x-axis rotation) + float t0 = +2.0 * (dqw * dqx + dqy * dqz); + float t1 = +1.0 - 2.0 * (dqx * dqx + ysqr); + roll = atan2(t0, t1); + + // pitch (y-axis rotation) + float t2 = +2.0 * (dqw * dqy - dqz * dqx); + t2 = t2 > 1.0 ? 1.0 : t2; + t2 = t2 < -1.0 ? -1.0 : t2; + pitch = asin(t2); + + // yaw (z-axis rotation) + float t3 = +2.0 * (dqw * dqz + dqx * dqy); + float t4 = +1.0 - 2.0 * (ysqr + dqz * dqz); + yaw = atan2(t3, t4); if (degrees) {