Diff: main.cpp

Revision:

2:44bc61abdf33

Parent:

1:3723e08bf4fd

--- a/main.cpp	Wed Jun 10 10:00:59 2015 +0000
+++ b/main.cpp	Wed Jun 10 12:56:48 2015 +0000
@@ -92,7 +92,7 @@
 // (in degrees) calculated from the quaternions coming from the FIFO.
 // Note that Euler angles suffer from gimbal lock (for more info, see
 // http://en.wikipedia.org/wiki/Gimbal_lock)
-#define OUTPUT_READABLE_EULER
+//#define OUTPUT_READABLE_EULER
 
 // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
 // pitch/roll angles (in degrees) calculated from the quaternions coming
@@ -155,8 +155,6 @@
                                              };
 volatile bool bleIsConnected = false;
 BLEDevice  ble;
-//UARTService *uartServicePtr;
-MPUService *mpuServicePtr;
 static volatile bool  triggerSensorPolling = false;
 /*
 * ====================================================================================================================================
@@ -184,8 +182,8 @@
 VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
 VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
 VectorFloat gravity;    // [x, y, z]            gravity vector
-float euler[3]={0};         // [psi, theta, phi]    Euler angle container
-float ypr[3]={0};           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+float euler[3]= {0};        // [psi, theta, phi]    Euler angle container
+float ypr[3]= {0};          // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
 
 // packet structure for InvenSense teapot demo
 uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
@@ -223,12 +221,10 @@
 
 void tick(void)
 {
-    if(dmpReady)
-    {
-    green = !green;
-    }
-    else{
-    red  = !red;
+    if(dmpReady) {
+        green = !green;
+    } else {
+        red  = !red;
     }
     triggerSensorPolling = true;
 }
@@ -327,6 +323,10 @@
     while (true) {
         if(dmpReady && (mpuInterrupt || fifoCount >= packetSize)) {
             mpuData();
+            pc.printf("ypr\t");
+            pc.printf("%i\t", (int16_t)ypr[0]*100) ;
+            pc.printf("%i\t", (int16_t)ypr[1]*100);
+            pc.printf("%i\t\r\n", (int16_t)ypr[2]*100);
             mpuServices.updateYawPitchRoll(ypr);
 
         }
@@ -409,7 +409,7 @@
         // 2 = DMP configuration updates failed
         // (if it's going to break, usually the code will be 1)
 
-        pc.printf("DDMP Initialization failed (code ");
+        pc.printf("DMP Initialization failed (code ");
         pc.printf("%d", devStatus);
         pc.printf(")\r\n");
     }
@@ -472,10 +472,15 @@
         mpu.dmpGetQuaternion(&q, fifoBuffer);
         mpu.dmpGetGravity(&gravity, &q);
         mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+        ypr[0] =ypr[0] * 180/M_PI;
+        ypr[1] =ypr[1] * 180/M_PI;
+        ypr[2] =ypr[2] * 180/M_PI;
+        /*
         pc.printf("ypr\t");
-        pc.printf("%f\t", ypr[0] * 180/M_PI);
-        pc.printf("%f\t", ypr[1] * 180/M_PI);
-        pc.printf("%f\t\r\n", ypr[2] * 180/M_PI);
+        pc.printf("%f\t", ypr[0] ;
+        pc.printf("%f\t", ypr[1]);
+        pc.printf("%f\t\r\n", ypr[2]);
+        */
 #endif
 
 #ifdef OUTPUT_READABLE_REALACCEL