Diff: RTOS-Threads/src/Task3.cpp

Revision:

45:3847d7bf8b2c

Parent:

44:4be5c01c6de2

Child:

47:89a7077a70d3

Child:

48:9dbdc4144f00

--- a/RTOS-Threads/src/Task3.cpp	Fri May 16 23:34:20 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Sat May 17 09:12:20 2014 +0000
@@ -45,11 +45,21 @@
     RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
     RCCommand[4] = rxModule[4].pulsewidth(); // AUX
 
+    /* Lost signal (throttle): */
+    if (rxModule[2].stallTimer.read_us() > 18820) {
+        for (int i = 0; i < 5; i++)
+            RCCommand[i] = 1000;
+    } else {
+        for (int i = 0; i < 5; i++)
+            RCCommand[i] = constrainRCCommand(RCCommand[i]);
+    }
+
     /* Mode switching: */
     if (RCCommand[4] > 1500) {
         if (mode == RATE) {
         } else if (mode == ATTITUDE) {
             mode = RATE;
+            yawPIDrate.reset();
             //pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
             //rollPIDrate.setTunings(KP_ROLL_RATE, TI_ROLL_RATE, 0.0);
         } else {}
@@ -57,6 +67,7 @@
         if (mode == ATTITUDE) {
         } else if (mode == RATE) {
             mode = ATTITUDE;
+            yawPIDrate.reset();
             //pitchPIDrate.setTunings(KP_PITCH_RATE, TI_PITCH_RATE, 0.0);
             //rollPIDrate.setTunings(KP_ROLL_RATE, TI_PITCH_RATE, 0.0);
         }
@@ -66,28 +77,26 @@
     inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
     switch (mode) {
         case RATE:
-            inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*6/100*STICK_GAIN);
-            inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*6/100*STICK_GAIN);
+            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
+            inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
             break;
         case ATTITUDE:
         default:
-            inputYPR[1] = deadbandInputYPR((RCCommand[1]-1500)*-1*6/100*STICK_GAIN);
-            inputYPR[2] = deadbandInputYPR((RCCommand[2]-1500)*-1*6/100*STICK_GAIN);
+            inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
+            inputYPR[2] = (RCCommand[2]-1500)*-1*9/100*STICK_GAIN;
             break;
     }
 
-    for (int i = 1; i < 3; i++)
-        inputYPR[i] = constrainInputYPR(inputYPR[i]);
+    for (int i = 0; i < 3; i++)
+        deadbandInputYPR(inputYPR[i]);
+
+    inputYPR[0] = constrainInputY(inputYPR[0]);
 
-    /* Lost signal (throttle): */
-    if (rxModule[2].stallTimer.read_us() > 18820) {
-        for (int i = 0; i < 5; i++)
-            RCCommand[i] = 1000;
-    } else {
-        for (int i = 0; i < 5; i++)
-            RCCommand[i] = constrainRCCommand(RCCommand[i]);
-    }
+    for (int i = 1; i < 3; i++)
+        inputYPR[i] = constrainInputPR(inputYPR[i]);
 
+
+    /* Telemetry: */
     if (voltageUpdate > 3) {
         if (box_demo) {
             BT.printf("\nV%2.2f\n", voltageSense*VOLTAGE_SCALE);
@@ -117,18 +126,28 @@
 
 int deadbandInputYPR(int input)
 {
-    if (input > -2 && input < 4)
+    if (input > -4 && input < 4) // if (input > -2 && input < 4)
         return 0;
     else
         return input;
 }
 
-int constrainInputYPR(int input)
+int constrainInputY(int input)
 {
-    if (input < -30)
-        return -30;
-    else if (input > 30)
-        return 30;
+    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;
 }