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