Ian Hua / Quadcopter-mbedRTOS
Revision:
17:18c3bd016e49
Parent:
15:10edc6b12122
Child:
18:af657c4c3944
--- a/RTOS-Threads/src/Task3.cpp	Sat May 03 01:33:28 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Sat May 03 01:47:54 2014 +0000
@@ -56,6 +56,9 @@
     if (rxModule[1].stallTimer.read_us() > 18820) {
         for (int i = 0; i < 5; i++)
             RCCommand[i] = 0;
+    } else {
+        for (int i = 0; i < 5; i++)
+            RCCommand[i] = constrainRCCommand(RCCommand[i]);
     }
 
     if (box_demo) {
@@ -67,3 +70,13 @@
 
     //LED[3] = !LED[3];
 }
+
+int constrainRCCommand(int input)
+{
+    if (input < 1000)
+        return 1000;
+    else if (input > 2000)
+        return 2000;
+    else
+        return input;
+}