Diff: main.cpp

Revision:

1:b213291e5a51

Parent:

0:9f2982964a48

diff -r 9f2982964a48 -r b213291e5a51 main.cpp
--- a/main.cpp	Mon Sep 01 14:26:51 2014 +0000
+++ b/main.cpp	Tue Aug 11 14:41:02 2015 +0000
@@ -2,7 +2,7 @@
 #include "Quaternion.h"
 
 Serial debug(USBTX, USBRX);
-MPU9150 imu(p10, p9, p15);
+MPU9150 imu(p27, p28, p15);
 
 DigitalOut led(LED1);
 
@@ -28,13 +28,17 @@
     Quaternion q1;
     
     while(true){
+        
+    wait(1);
+    
 
         if(imu.getFifoCount() >= 48){
             imu.getFifoBuffer(buffer,  48);
             led = !led;
         }
-        
-        if(timer.read_ms() > 50){
+        q1.decode(buffer);
+    //    wait(1.1);
+        if(timer.read_ms() >50){
             timer.reset();
            
            //This is the format of the data in the fifo, 
@@ -67,10 +71,11 @@
                 (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + ((int32_t)buffer[10] << 8) + buffer[11]))* (1.0 / (1<<30)),
                 (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + ((int32_t)buffer[14] << 8) + buffer[15]))* (1.0 / (1<<30)));
             */
-                
-            q1.decode(buffer);
-            debug.printf("%f, %f, %f, %f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
-
+                //wait(0.01); 
+            
+            debug.printf("w:%f, v.x:%f, v.y:%f, v.z:%f\r\n", q1.w, q1.v.x, q1.v.y, q1.v.z);
+imu.setFifoReset(true);    
+    imu.setDMPEnabled(true);
 
             //TeaPot Demo Packet for MotionFit SDK
             /*
@@ -84,6 +89,8 @@
                 debug.putc((char)0);
             }
             */
+        
         }   
-    }
+   
+  }
 }
\ No newline at end of file