Revision 2:c35f8379f2cb, committed 2017-08-11

Comitter:

mbedoguz

Date:

Fri Aug 11 07:43:08 2017 +0000

Parent:

1:80a269cb9d4d

Child:

3:6f87862d2d68

Child:

4:2c4e849b8ecf

Commit message:

Little changes that is made to debug in the library are reverted since problem is solved.

Changed in this revision

--- a/SparkFunMPU9250-DMP.cpp	Tue Aug 08 14:14:58 2017 +0000
+++ b/SparkFunMPU9250-DMP.cpp	Fri Aug 11 07:43:08 2017 +0000
@@ -39,7 +39,6 @@
 {
 	inv_error_t result;
     struct int_param_s int_param;
-	imu_init();
 	result = mpu_init(&int_param);
 	if (result)
 		return result;
@@ -408,7 +407,7 @@
 	return dmp_set_fifo_rate(rate);
 }
 
-inv_error_t MPU9250_DMP::dmpUpdateFifo(long DEBUG[4])
+inv_error_t MPU9250_DMP::dmpUpdateFifo(void)
 {
 	short gyro[3];
 	short accel[3];
@@ -442,10 +441,6 @@
 		qy = quat[2];
 		qz = quat[3];
 	}
-	DEBUG[0]=quat[0];
-	DEBUG[1]=quat[1];
-	DEBUG[2]=quat[2];
-	DEBUG[3]=quat[3];
 	
 	time = timestamp;
 	
@@ -621,18 +616,18 @@
 
 void MPU9250_DMP::computeEulerAngles(bool degrees)
 {
-    double dqw = qToFloat(qw, 30);
-    double dqx = qToFloat(qx, 30);
-    double dqy = qToFloat(qy, 30);
-    double dqz = qToFloat(qz, 30);
+    float dqw = qToFloat(qw, 30);
+    float dqx = qToFloat(qx, 30);
+    float dqy = qToFloat(qy, 30);
+    float dqz = qToFloat(qz, 30);
     
-    double ysqr = dqy * dqy;
-    double t0 = -2.0f * (ysqr + dqz * dqz) + 1.0f;
-    double t1 = +2.0f * (dqx * dqy - dqw * dqz);
-    double t2 = -2.0f * (dqx * dqz + dqw * dqy);
-    double t3 = +2.0f * (dqy * dqz - dqw * dqx);
-    double t4 = -2.0f * (dqx * dqx + ysqr) + 1.0f;
-
+    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;

--- a/SparkFunMPU9250-DMP.h	Tue Aug 08 14:14:58 2017 +0000
+++ b/SparkFunMPU9250-DMP.h	Fri Aug 11 07:43:08 2017 +0000
@@ -264,7 +264,7 @@
 	// quaternion, and time public variables (depending on how the DMP is configured).
 	// Should be called whenever an MPU interrupt is detected
 	// Output: INV_SUCCESS (0) on success, otherwise error
-	inv_error_t dmpUpdateFifo(long *DEBUG); 
+	inv_error_t dmpUpdateFifo(void); 
 	
 	// dmpEnableFeatures -- Enable one, or multiple DMP features.
 	// Input: An OR'd list of features (see dmpBegin)