Diff: RTOS-Threads/src/Task3.cpp

Revision:

5:4879ef0e6d41

Parent:

4:01921a136f58

Child:

6:1a5654c14b5b

diff -r 01921a136f58 -r 4879ef0e6d41 RTOS-Threads/src/Task3.cpp
--- a/RTOS-Threads/src/Task3.cpp	Wed Apr 30 06:37:42 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Wed Apr 30 07:13:11 2014 +0000
@@ -6,6 +6,8 @@
 
 float ypr_offset[3];
 bool box_demo = false;
+bool rc_out = false;
+bool gyro_out = false;
 
 PwmIn rxModule[] = {p14, p15, p16, p17, p18};
 
@@ -18,25 +20,42 @@
         char data = BT.getc();
         if (data == 'B') {
             box_demo = true;
+            rc_out = false;
+            gyro_out = false;
         } else if (data == 'Z') {
+            box_demo = true;
+            rc_out = false;
+            gyro_out = false;
             ypr_offset[0] = ypr[0];
             ypr_offset[1] = ypr[1];
             ypr_offset[2] = ypr[2];
-        } else if (data == 'O') {
+        } else if (data == 'R') {
+            box_demo = false;
+            rc_out = true;
+            gyro_out = false;
+        } else if (data == 'G') {
             box_demo = false;
+            rc_out = false;
+            gyro_out = true;
+        } else if (data == '0') {
+            box_demo = false;
+            rc_out = false;
+            gyro_out = false;
         }
     }
-    RCCommand[0] = rxModule[0].pulsewidth();
-    RCCommand[1] = rxModule[1].pulsewidth();
-    RCCommand[2] = rxModule[2].pulsewidth();
-    RCCommand[3] = rxModule[3].pulsewidth();
+    RCCommand[2] = rxModule[0].pulsewidth(); // Roll
+    RCCommand[3] = rxModule[1].pulsewidth(); // Throttle
+    RCCommand[1] = rxModule[2].pulsewidth(); // Pitch
+    RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
+    RCCommand[4] = rxModule[4].pulsewidth(); // AUX
 
-    if (rxModule[1].stallTimer.read_us() > 18900) {
-        for (int i = 0; i < 4; i++)
+    if (rxModule[1].stallTimer.read_us() > 18820) {
+        for (int i = 0; i < 5; i++)
             RCCommand[i] = 0;
     }
-    
-    //BT.printf("%5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3]);
+
+    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];
 }