Diff: RTOS-Threads/src/Task3.cpp

Revision:

39:02782ad251db

Parent:

38:ef65533cca32

Child:

40:27e344adfdb9

--- a/RTOS-Threads/src/Task3.cpp	Tue May 13 13:05:03 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Wed May 14 12:42:39 2014 +0000
@@ -4,9 +4,6 @@
  * Purpose:     Thread3: RC & BT Command
  * Settings:    50Hz
  */
-#define STICK_GAIN 2
-#define STICK_GAIN_YAW 16
-
 #include "tasks.h"
 #include "setup.h"
 #include "PwmIn.h"
@@ -125,10 +122,6 @@
         return input;
 }
 
-int constrainInputYaw(int input)
-{
-}
-
 int constrainInputYPR(int input)
 {
     if (input < -45)
@@ -153,8 +146,8 @@
             calibration_mode = false;
             adjust_check = false;
 
-            pitchPIDstable.reset();
-            rollPIDstable.reset();
+            pitchPIDattitude.reset();
+            rollPIDattitude.reset();
             yawPIDrate.reset();
             pitchPIDrate.reset();
             rollPIDrate.reset();
@@ -177,8 +170,8 @@
             ypr_offset[1] = ypr[1];
             ypr_offset[2] = ypr[2];
 
-            pitchPIDstable.reset();
-            rollPIDstable.reset();
+            pitchPIDattitude.reset();
+            rollPIDattitude.reset();
             yawPIDrate.reset();
             pitchPIDrate.reset();
             rollPIDrate.reset();
@@ -298,9 +291,9 @@
             rc_out = false;
             gyro_out = false;
             ESC_check = false;
-            KP_PITCH_STABLE += 0.1;
-            pitchPIDstable.setKP(KP_PITCH_STABLE);
-            BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
+            KP_PITCH_ATTITUDE += 0.1;
+            pitchPIDattitude.setKP(KP_PITCH_ATTITUDE);
+            BT.printf("KP P attitude: %2.5f\n", KP_PITCH_ATTITUDE);
             break;
         case 'Y':
         case 'y':
@@ -308,9 +301,9 @@
             rc_out = false;
             gyro_out = false;
             ESC_check = false;
-            KP_PITCH_STABLE -= 0.1;
-            pitchPIDstable.setKP(KP_PITCH_STABLE);
-            BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
+            KP_PITCH_ATTITUDE -= 0.1;
+            pitchPIDattitude.setKP(KP_PITCH_ATTITUDE);
+            BT.printf("KP P attitude: %2.5f\n", KP_PITCH_ATTITUDE);
             break;
 
         case '7':
@@ -318,9 +311,9 @@
             rc_out = false;
             gyro_out = false;
             ESC_check = false;
-            KP_ROLL_STABLE += 0.1;
-            rollPIDstable.setKP(KP_ROLL_STABLE);
-            BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
+            KP_ROLL_ATTITUDE += 0.1;
+            rollPIDattitude.setKP(KP_ROLL_ATTITUDE);
+            BT.printf("KP R attitude: %2.5f\n", KP_ROLL_ATTITUDE);
             break;
         case 'U':
         case 'u':
@@ -328,16 +321,16 @@
             rc_out = false;
             gyro_out = false;
             ESC_check = false;
-            KP_ROLL_STABLE -= 0.1;
-            rollPIDstable.setKP(KP_ROLL_STABLE);
-            BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
+            KP_ROLL_ATTITUDE -= 0.1;
+            rollPIDattitude.setKP(KP_ROLL_ATTITUDE);
+            BT.printf("KP R attitude: %2.5f\n", KP_ROLL_ATTITUDE);
             break;
 
         case 'A':
             if (!armed) {
                 if (RCCommand[3] < 1001) {
-                    pitchPIDstable.reset();
-                    rollPIDstable.reset();
+                    pitchPIDattitude.reset();
+                    rollPIDattitude.reset();
                     yawPIDrate.reset();
                     pitchPIDrate.reset();
                     rollPIDrate.reset();
@@ -369,8 +362,8 @@
                 ypr_offset[0] = ypr[0];
                 ypr_offset[1] = ypr[1];
                 ypr_offset[2] = ypr[2];
-                pitchPIDstable.reset();
-                rollPIDstable.reset();
+                pitchPIDattitude.reset();
+                rollPIDattitude.reset();
             } else {
                 BT.printf("ALREADY DISARMED!!!\n");
             }