Diff: Sensor.cpp

Revision:

3:e658293d4b83

Parent:

2:dae13ba06097

Child:

4:f817736140b6

diff -r dae13ba06097 -r e658293d4b83 Sensor.cpp
--- a/Sensor.cpp	Fri Jun 13 18:27:35 2014 +0000
+++ b/Sensor.cpp	Wed Oct 15 04:59:43 2014 +0000
@@ -52,6 +52,7 @@
     result = mpu_initialize(); 
     if (result) return result;
     result = dmp_initialize();
+    if (result) return result;
     run_self_test();
     return 0;  
 }
@@ -109,6 +110,11 @@
     return 0;
 }
 
+void Sensor::reset()
+{
+    dmp.reset();
+}
+
 
 void Sensor::run_self_test()
 {
@@ -139,10 +145,7 @@
     unsigned char more;
     short sensors;
     result = dmp.dmp_read_fifo(gyro, accel, quat, NULL, &sensors, &more);
-    while (result == -2)
-    {
-        result = dmp.dmp_read_fifo(gyro, accel, quat, NULL, &sensors, &more);
-    }
+
     if (result == 0)
     {
         q.set(quat);
@@ -150,7 +153,7 @@
         dmp.dmpGetYawPitchRoll(ypr,&q,&gravity);
     }
 //    PRINTF("More: %d\n\r",more);
-    if (more > 4) dmp.mpu_reset_fifo();
+    if (more > 2) dmp.mpu_reset_fifo();
     return more;
 }