Diff: RTOS-Threads/src/Task1.cpp

Revision:

16:9072cd6fa8d1

Parent:

15:10edc6b12122

Child:

17:18c3bd016e49

--- a/RTOS-Threads/src/Task1.cpp	Sat May 03 00:57:48 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Sat May 03 01:33:28 2014 +0000
@@ -6,7 +6,7 @@
 /* MPU6050 control/status variables: */
 uint8_t mpuIntStatus;       // holds actual interrupt status byte from MPU
 uint16_t fifoCount;         // count of all bytes currently in FIFO
-uint8_t fifoBuffer[64];     // FIFO storage buffer
+uint8_t fifoBuffer[128];     // FIFO storage buffer
 
 /* Orientation/motion variables: */
 Quaternion q;               // [w, x, y, z] quaternion container
@@ -37,9 +37,10 @@
 
     // get current FIFO count
     fifoCount = imu.getFIFOCount();
+    //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
 
     // check for overflow (this should never happen unless our code is too inefficient)
-    if ((mpuIntStatus & 0x10) || fifoCount > 1023) {
+    if ((mpuIntStatus & 0x10) || fifoCount > 126) {
         // reset so we can continue cleanly
         imu.resetFIFO();
         imu.debugSerial.printf("FIFO overflow!");
@@ -49,17 +50,21 @@
         // wait for correct available data length, should be a VERY short wait
         while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
 
+        // while (fifoCount >= packetSize) {
         // read a packet from FIFO
         imu.getFIFOBytes(fifoBuffer, packetSize);
 
         // track FIFO count here in case there is > 1 packet available
         // (this lets us immediately read more without waiting for an interrupt)
         //fifoCount -= packetSize;
+        //}
 
         // display YPR angles in degrees
         imu.dmpGetQuaternion(&q, fifoBuffer);
         imu.dmpGetGravity(&gravity, &q);
         imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        imu.resetFIFO();
+        
         ypr_use[0] = ypr[0] * 180/M_PI;
         ypr_use[1] = ypr[1] * 180/M_PI;
         ypr_use[2] = ypr[2] * 180/M_PI;