Diff: RollPitch.h

Revision:

2:99785a1007a4

Parent:

1:620da20b810b

--- a/RollPitch.h	Sat Jun 04 04:06:36 2016 +0000
+++ b/RollPitch.h	Tue Jun 07 05:26:03 2016 +0000
@@ -5,7 +5,8 @@
 MPU6050 mpu;
 bool dmpReady = false;
 uint16_t packetSize;
-
+//extern Serial mpc;
+extern DigitalOut myled;
 bool dmpSetup()
 {
 #if 0
@@ -23,8 +24,10 @@
     } else {
         dmpReady = false;
     }
+    //mpc.printf("packetSize = %d\r\n", packetSize);
     return dmpReady;
 }
+
 #define PI 3.1415926
 uint8_t dmpGetYPR(float *_ypr)
 {
@@ -34,8 +37,18 @@
     if(!dmpReady)
         return 0;
 
+    //myled = !myled;
     if(mpu.getFIFOCount() == 1024) {
+        myled = 1;
         mpu.resetFIFO();
+        while(mpu.getFIFOCount() < packetSize);
+        mpu.getFIFOBytes(fifoBuffer, packetSize);
+        mpu.dmpGetQuaternion(&q, fifoBuffer);
+        mpu.dmpGetGravity(&gravity, &q);
+        mpu.dmpGetYawPitchRoll(_ypr, &q, &gravity);
+        for(int i=0; i<3; i++) {
+            _ypr[i] *= 180/PI;
+        }
     } else {
         while(mpu.getFIFOCount() < packetSize);
         mpu.getFIFOBytes(fifoBuffer, packetSize);
@@ -45,6 +58,7 @@
         for(int i=0; i<3; i++) {
             _ypr[i] *= 180/PI;
         }
+        myled = 0;
     }
     return 0;
 }