Diff: RTOS-Threads/src/Task1.cpp

Revision:

30:d9b988f8d84f

Parent:

27:18b6580eb0b1

Child:

31:3dde2201e54d

--- a/RTOS-Threads/src/Task1.cpp	Sat May 10 04:06:53 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Mon May 12 04:43:38 2014 +0000
@@ -3,8 +3,8 @@
  * Date:        May 2014
  * Purpose:     Thread1: Code to read Yaw Pitch Roll angles from MPU6050 DMP.
  * Settings:    100Hz
+ * Timing:      1440us
  */
-
 #include "Task1.h"
 #include "setup.h"
 
@@ -31,70 +31,93 @@
 double heading = 0;
 #endif
 
+#ifdef TIME_TASK1
+Timer _t1;
+#endif
 // ================================================================
-// === YPR ROUTINE ===
+// === YPR THREAD ===
 // ================================================================
 void Task1(void const *argument)
 {
-    switch (mode) {
-        case RATE:
-            break;
+    switch (box_demo) {
+        case true:
+            AHRSSample();
             
-        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);
-
-            // 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();
-
-                while (fifoCount > 41) {
-                    // read a packet from FIFO
-                    imu.getFIFOBytes(fifoBuffer, packetSize);
+            BT.printf("
+            break;
 
-                    // 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);
-
-                ypr[0] = ypr[0] * 180/M_PI;
-                ypr[1] = ypr[1] * 180/M_PI;
-                ypr[2] = ypr[2] * 180/M_PI;
+        case false:
+#ifdef TIME_TASK1
+            _t1.reset();
+            _t1.start();
+#endif
+            if (armed) {
+                switch (mode) {
+                    case RATE:
+                        break;
 
-                /*
-                if (compass.getDataReady()) {
-                    // compass.getValues(&compass_x, &compass_y, &compass_z);
-                    heading = compass.getHeadingXY() * 180/M_PI;
-                }
+                    case ATTITUDE:
+                    default:
+                        AHRSSample();
 
-                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]);
-
-                counterTask1 = true;
+                        counterTask1 = true;
+                        break;
+                }
             }
-            break;
     }
 }
+
+// ================================================================
+// === Helper functions ===
+// ================================================================
+
+void AHRSSample(void)
+{
+    // 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);
+
+    // 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();
+
+        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;
+        }
+
+        // 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;
+
+        /*
+        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;
+        */
+    }
+}
\ No newline at end of file