Oğuz Özdemir / SparkFunMPU9250-DMP

It is the library published by sparkfun, edited accordingly to make it work under mbed platform.

Dependents:   MPU9250-dmp-bluepill MPU9250-dmp

Files at this revision

API Documentation at this revision

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)
 	{