It is the library published by sparkfun, edited accordingly to make it work under mbed platform.
Dependents: MPU9250-dmp-bluepill MPU9250-dmp
Diff: SparkFunMPU9250-DMP.cpp
- Revision:
- 3:6f87862d2d68
- Parent:
- 2:c35f8379f2cb
--- a/SparkFunMPU9250-DMP.cpp Fri Aug 11 07:43:08 2017 +0000 +++ b/SparkFunMPU9250-DMP.cpp Fri Aug 11 08:31:27 2017 +0000 @@ -620,21 +620,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) {