Diff: RTOS-Threads/src/Task1.cpp

Revision:

12:953d25061417

Parent:

11:f9fd410c48c2

Child:

15:10edc6b12122

--- a/RTOS-Threads/src/Task1.cpp	Fri May 02 07:22:09 2014 +0000
+++ b/RTOS-Threads/src/Task1.cpp	Fri May 02 17:01:56 2014 +0000
@@ -1,4 +1,4 @@
-/* Sample & YPR (250Hz) */
+/* YPR (100Hz) */
 
 #include "Task1.h"
 #include "setup.h"
@@ -12,7 +12,13 @@
 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];
+
+#ifdef ENABLE_COMPASS
+//int compassX, compassY, compassZ;
+double heading = 0;
+#endif
+
+float altitude, temperature;
 
 int counterTask1 = 0;
 
@@ -21,7 +27,7 @@
 #endif
 
 // ================================================================
-// === SAMPLE & CONVERSION ROUTINE ===
+// === YPR ROUTINE ===
 // ================================================================
 void Task1(void const *argument)
 {
@@ -53,12 +59,22 @@
         imu.dmpGetQuaternion(&q, fifoBuffer);
         imu.dmpGetGravity(&gravity, &q);
         imu.dmpGetYawPitchRoll(ypr, &q, &gravity);
-        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] *= 180/M_PI;
+        ypr[1] *= 180/M_PI;
+        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;
+                */
 
         if (box_demo)
-            BT.printf("Y%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]);
     }
 
     //LED[1] = !LED[1];