Diff: RTOS-Threads/src/Task3.cpp

Revision:

47:89a7077a70d3

Parent:

45:3847d7bf8b2c

diff -r b11655031d24 -r 89a7077a70d3 RTOS-Threads/src/Task3.cpp
--- a/RTOS-Threads/src/Task3.cpp	Sat May 17 11:57:09 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Sun May 18 09:05:41 2014 +0000
@@ -74,30 +74,24 @@
     }
 
     /* Command decoder: */
-    inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
+    inputYPR[0] = deadbandInputYPR((RCCommand[0]-1500)*9/100)*STICK_GAIN_YAW;
     switch (mode) {
         case RATE:
-            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
-            inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
+            inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*9/100)*STICK_GAIN;
+            inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*9/100)*STICK_GAIN;
             break;
         case ATTITUDE:
         default:
-            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
-            inputYPR[2] = (RCCommand[2]-1500)*-1*9/100*STICK_GAIN;
+            inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*9/100)*STICK_GAIN;
+            inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*-1*9/100)*STICK_GAIN;
             break;
     }
 
     for (int i = 0; i < 3; i++)
-        deadbandInputYPR(inputYPR[i]);
-
-    inputYPR[0] = constrainInputY(inputYPR[0]);
-
-    for (int i = 1; i < 3; i++)
-        inputYPR[i] = constrainInputPR(inputYPR[i]);
-
+        inputYPR[i] = constrainInputYPR(inputYPR[i], i);
 
     /* Telemetry: */
-    if (voltageUpdate > 3) {
+    if (voltageUpdate > 49) {
         if (box_demo) {
             BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
         }
@@ -126,30 +120,33 @@
 
 int deadbandInputYPR(int input)
 {
-    if (input > -4 && input < 4) // if (input > -2 && input < 4)
+    if (input > STICK_INPUT_DEADBAND_MIN && input < STICK_INPUT_DEADBAND_MAX)
         return 0;
     else
         return input;
 }
 
-int constrainInputY(int input)
+int constrainInputYPR(int input, int axis)
 {
-    if (input < -720)
-        return -720;
-    else if (input > 720)
-        return 720;
-    else
-        return input;
-}
-
-int constrainInputPR(int input)
-{
-    if (input < -45)
-        return -45;
-    else if (input > 45)
-        return 45;
-    else
-        return input;
+    switch (axis) {
+        case 0:
+            if (input < -720)
+                return -720;
+            else if (input > 720)
+                return 720;
+            else
+                return input;
+        case 1:
+        case 2:
+            if (input < -45)
+                return -45;
+            else if (input > 45)
+                return 45;
+            else
+                return input;
+        default:
+            return 0;
+    }
 }
 
 void uartDecoder(char input)