Diff: RTOS-Threads/src/Task2_Master.cpp

Revision:

47:89a7077a70d3

Parent:

39:02782ad251db

--- a/RTOS-Threads/src/Task2_Master.cpp	Sat May 17 11:57:09 2014 +0000
+++ b/RTOS-Threads/src/Task2_Master.cpp	Sun May 18 09:05:41 2014 +0000
@@ -19,7 +19,7 @@
 Quaternion q;               // [w, x, y, z] quaternion container
 VectorFloat gravity;        // [x, y, z] gravity vector
 float ypr_rad[3];           // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
-volatile float ypr[3];      
+volatile float ypr[3];
 
 float altitude, temperature;
 
@@ -35,6 +35,9 @@
 /* YPR Adjust */
 volatile float adjust_attitude[3] = {0.0, 0.0, 0.0};
 
+int altiUpdate = 0;
+volatile float altiDistance = 0.0;
+volatile float altiTemp = 0.0;
 
 
 
@@ -47,6 +50,13 @@
 //Timer
     AHRSSample();
 
+    if (altiUpdate > 1) {
+        altiDistance = altimeter.Altitude_m();
+        altiTemp = altimeter.Temp_C();
+        altiUpdate = 0;
+    } else
+        altiUpdate++;
+
     if (armed) {
         switch (mode) {
             case RATE:
@@ -113,8 +123,9 @@
 
         // display YPR angles in degrees
         imu.dmpGetQuaternion(&q, fifoBuffer);
-        imu.dmpGetGravity(&gravity, &q);
-        imu.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
+        //imu.dmpGetGravity(&gravity, &q);
+        //imu.dmpGetYawPitchRoll(ypr_rad, &q, &gravity);
+        imu.dmpGetEuler(ypr_rad, &q);
 
         for (int i = 0; i < 3; i++)
             ypr[i] = ypr_rad[i] * 180/M_PI;