Diff: RTOS-Threads/src/Task3.cpp

Revision:

4:01921a136f58

Parent:

3:605fbcb54e75

Child:

5:4879ef0e6d41

--- a/RTOS-Threads/src/Task3.cpp	Tue Apr 29 14:53:32 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Wed Apr 30 06:37:42 2014 +0000
@@ -2,16 +2,12 @@
 
 #include "tasks.h"
 #include "setup.h"
+#include "PwmIn.h"
 
 float ypr_offset[3];
 bool box_demo = false;
 
-Timer stallTime;
-Timer inputTime;
-
-AnalogIn rxModule[] = {p16, p17, p18, p19, p20};
-
-int rxIn[5] = {0, 0, 0, 0, 0};
+PwmIn rxModule[] = {p14, p15, p16, p17, p18};
 
 /* [YAW PITCH ROLL THROTTLE AUX] */
 int RCCommand[5] = {0, 0, 0, 0, 0};
@@ -21,7 +17,7 @@
     if (BT.readable()) {
         char data = BT.getc();
         if (data == 'B') {
-            box_demo = !box_demo;
+            box_demo = true;
         } else if (data == 'Z') {
             ypr_offset[0] = ypr[0];
             ypr_offset[1] = ypr[1];
@@ -30,51 +26,17 @@
             box_demo = false;
         }
     }
-
-    for (int i = 0; i < 5; i++) {
-        stallTime.reset();
-        inputTime.reset();
-
-        stallTime.start();
-        while (!rxModule[i] && (stallTime.read_us() < 1010)); // wait for high
-
-        if (stallTime.read_us() < 1050) {
-            stallTime.stop();
-            stallTime.reset();
-
-            inputTime.start();
-            stallTime.start();
-
-            while (rxModule[i] && (stallTime.read_us() < 1010)); // wait for low
-            if (stallTime.read_us() < 1050) {
-                inputTime.stop();
-                stallTime.stop();
+    RCCommand[0] = rxModule[0].pulsewidth();
+    RCCommand[1] = rxModule[1].pulsewidth();
+    RCCommand[2] = rxModule[2].pulsewidth();
+    RCCommand[3] = rxModule[3].pulsewidth();
 
-                rxIn[i] = inputTime.read_us();
-            } else {
-                stallTime.stop();
-                rxIn[i] = 0;
-            }
-        } else {
-            stallTime.stop();
-            rxIn[i] = 0;
-        }
+    if (rxModule[1].stallTimer.read_us() > 18900) {
+        for (int i = 0; i < 4; i++)
+            RCCommand[i] = 0;
     }
+    
+    //BT.printf("%5d %5d %5d %5d\n", RCCommand[0], RCCommand[1], RCCommand[2], RCCommand[3]);
 
-    RCCommand[0] = rxIn[0];
-    RCCommand[1] = rxIn[2];
-    RCCommand[2] = rxIn[3];
-    RCCommand[3] = 1000;//rxIn[1];
-    RCCommand[4] = rxIn[4];
-
-/*
-    for (int i = 0; i < 5; i++)
-        RCCommand[i] = 1000;
-        
-    RCCommand[0] = 0;
-    RCCommand[1] = 0;
-    RCCommand[2] = 0;
-    */
-    
     //LED[3] = !LED[3];
 }