Diff: RTOS-Threads/src/Task3.cpp

Revision:

12:953d25061417

Parent:

10:ef5fe86f67fe

Child:

15:10edc6b12122

--- a/RTOS-Threads/src/Task3.cpp	Fri May 02 07:22:09 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Fri May 02 17:01:56 2014 +0000
@@ -1,4 +1,4 @@
-/* RC & BT Command (50Hz) */
+/* RC & BT Command & Telemetry (50Hz) */
 
 #include "tasks.h"
 #include "setup.h"
@@ -10,6 +10,9 @@
 bool gyro_out = false;
 
 PwmIn rxModule[] = {p14, p15, p16, p17, p18};
+AnalogIn voltageSense(p20);
+
+float vIn = 0.0;
 
 /* [YAW PITCH ROLL THROTTLE AUX] */
 int RCCommand[5] = {0, 0, 0, 0, 0};
@@ -26,9 +29,9 @@
             box_demo = true;
             rc_out = false;
             gyro_out = false;
-            ypr_offset[0] = ypr_use[0];
-            ypr_offset[1] = ypr_use[1];
-            ypr_offset[2] = ypr_use[2];
+            ypr_offset[0] = ypr[0];
+            ypr_offset[1] = ypr[1];
+            ypr_offset[2] = ypr[2];
         } else if (data == 'R') {
             box_demo = false;
             rc_out = true;
@@ -43,7 +46,7 @@
             gyro_out = false;
         }
     }
-    
+
     RCCommand[2] = rxModule[0].pulsewidth(); // Roll
     RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
     RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
@@ -55,9 +58,12 @@
             RCCommand[i] = 0;
     }
 
-    if (rc_out)
+    if (box_demo) {
+        BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
+        BT.printf("\nA%3.2f\nT%2.2f\n", altimeter.Altitude_m(), altimeter.Temp_C());
+    } else if (rc_out)
         BT.printf("%5d %5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3], RCCommand[4]);
-        
+
 
     //LED[3] = !LED[3];
 }