Diff: RTOS-Threads/src/Task1.cpp

Revision:

21:b642c18eccd1

Parent:

18:af657c4c3944

Child:

22:ef8aa9728013

--- a/RTOS-Threads/src/Task1.cpp	Sat May 03 02:55:24 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Thu May 08 09:39:12 2014 +0000
@@ -6,13 +6,13 @@
 /* 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[128];     // FIFO storage buffer
+uint8_t fifoBuffer[64];     // FIFO storage buffer
 
 /* Orientation/motion variables: */
 Quaternion q;               // [w, x, y, z] quaternion container
 VectorFloat gravity;        // [x, y, z] gravity vector
 float ypr[3];               // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
-volatile float ypr_use[3];
+//volatile float ypr_use[3];
 
 #ifdef ENABLE_COMPASS
 //int compassX, compassY, compassZ;
@@ -21,7 +21,7 @@
 
 float altitude, temperature;
 
-int counterTask1 = 0;
+bool counterTask1 = false;
 
 #ifndef M_PI
 #define M_PI 3.1415
@@ -50,26 +50,26 @@
         // wait for correct available data length, should be a VERY short wait
         while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
 
-         while (fifoCount > 41) {
-        // read a packet from FIFO
-        imu.getFIFOBytes(fifoBuffer, packetSize);
+        while (fifoCount > 41) {
+            // 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;
+            // 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);
-        
+
         //if (fifoCount > 126)
-       // imu.resetFIFO();
+        // 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;
+        ypr[0] = ypr[0] * 180/M_PI;
+        ypr[1] = ypr[1] * 180/M_PI;
+        ypr[2] = ypr[2] * 180/M_PI;
 
         /*
                 if (compass.getDataReady()) {
@@ -82,10 +82,11 @@
                 */
 
         if (box_demo)
-            BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr_use[0] - ypr_offset[0],
-                      ypr_use[1] - ypr_offset[1], ypr_use[2] - ypr_offset[2]);
+            BT.printf("\nY%3.2f\nP%3.2f\nR%3.2f\n", ypr[0] - ypr_offset[0],
+                      ypr[1] - ypr_offset[1], ypr[2] - ypr_offset[2]);
 
         //counterTask1++;
+        counterTask1 = true;
     }
 
     //LED[1] = !LED[1];