Diff: RTOS-Threads/src/Task1.cpp

Revision:

27:18b6580eb0b1

Parent:

24:54a8cdf17378

Child:

30:d9b988f8d84f

--- a/RTOS-Threads/src/Task1.cpp	Sat May 10 01:04:17 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Sat May 10 01:39:02 2014 +0000
@@ -36,57 +36,65 @@
 // ================================================================
 void Task1(void const *argument)
 {
-    // reset interrupt flag and get INT_STATUS byte
-    mpuIntStatus = imu.getIntStatus();
+    switch (mode) {
+        case RATE:
+            break;
+            
+        case ATTITUDE:
+        default:
+            // reset interrupt flag and get INT_STATUS byte
+            mpuIntStatus = imu.getIntStatus();
 
-    // get current FIFO count
-    fifoCount = imu.getFIFOCount();
-    //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
+            // get current FIFO count
+            fifoCount = imu.getFIFOCount();
+            //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
 
-    // check for overflow
-    // Only keep a max of 2 packets in buffer.
-    if ((mpuIntStatus & 0x10) || fifoCount > 84) {
-        // reset so we can continue cleanly
-        imu.resetFIFO();
-        imu.debugSerial.printf("FIFO overflow!");
+            // check for overflow
+            // Only keep a max of 2 packets in buffer.
+            if ((mpuIntStatus & 0x10) || fifoCount > 84) {
+                // reset so we can continue cleanly
+                imu.resetFIFO();
+                imu.debugSerial.printf("FIFO overflow!");
 
-        // otherwise, check for DMP data ready interrupt (this should happen frequently)
-    } else if (mpuIntStatus & 0x02) {
-        // wait for correct available data length, should be a VERY short wait
-        while (fifoCount < packetSize) fifoCount = imu.getFIFOCount();
+                // otherwise, check for DMP data ready interrupt (this should happen frequently)
+            } else if (mpuIntStatus & 0x02) {
+                // 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);
+                // display YPR angles in degrees
+                imu.dmpGetQuaternion(&q, fifoBuffer);
+                imu.dmpGetGravity(&gravity, &q);
+                imu.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;
+                ypr[0] = ypr[0] * 180/M_PI;
+                ypr[1] = ypr[1] * 180/M_PI;
+                ypr[2] = ypr[2] * 180/M_PI;
 
-        /*
-        if (compass.getDataReady()) {
-            // compass.getValues(&compass_x, &compass_y, &compass_z);
-            heading = compass.getHeadingXY() * 180/M_PI;
-        }
+                /*
+                if (compass.getDataReady()) {
+                    // compass.getValues(&compass_x, &compass_y, &compass_z);
+                    heading = compass.getHeadingXY() * 180/M_PI;
+                }
 
-        ypr[0] *= 0.98;
-        ypr[0] += 0.02*heading;
-        */
+                ypr[0] *= 0.98;
+                ypr[0] += 0.02*heading;
+                */
 
-        if (box_demo)
-            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]);
+                if (box_demo)
+                    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 = true;
+                counterTask1 = true;
+            }
+            break;
     }
 }