Diff: RTOS-Threads/src/Task1.cpp

Revision:

22:ef8aa9728013

Parent:

21:b642c18eccd1

Child:

24:54a8cdf17378

diff -r b642c18eccd1 -r ef8aa9728013 RTOS-Threads/src/Task1.cpp
--- a/RTOS-Threads/src/Task1.cpp	Thu May 08 09:39:12 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Thu May 08 10:33:43 2014 +0000
@@ -1,4 +1,9 @@
-/* YPR (100Hz) */
+/* File:        Task1.cpp
+ * Author:      Trung Tin Ian HUA
+ * Date:        May 2014
+ * Purpose:     Thread1: Code to read Yaw Pitch Roll angles from MPU6050 DMP.
+ * Settings:    100Hz
+ */
 
 #include "Task1.h"
 #include "setup.h"
@@ -12,21 +17,19 @@
 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];
+
+float altitude, temperature;
+bool counterTask1 = false;
+
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
 
 #ifdef ENABLE_COMPASS
 //int compassX, compassY, compassZ;
 double heading = 0;
 #endif
 
-float altitude, temperature;
-
-bool counterTask1 = false;
-
-#ifndef M_PI
-#define M_PI 3.1415
-#endif
-
 // ================================================================
 // === YPR ROUTINE ===
 // ================================================================
@@ -39,7 +42,8 @@
     fifoCount = imu.getFIFOCount();
     //imu.debugSerial.printf("FIFO Count: %d\n", fifoCount);
 
-    // check for overflow (this should never happen unless our code is too inefficient)
+    // 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();
@@ -64,30 +68,24 @@
         imu.dmpGetGravity(&gravity, &q);
         imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
 
-        //if (fifoCount > 126)
-        // imu.resetFIFO();
-
         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]);
 
-        //counterTask1++;
         counterTask1 = true;
     }
-
-    //LED[1] = !LED[1];
 }