Diff: RTOS-Threads/src/Task3.cpp

Revision:

24:54a8cdf17378

Parent:

22:ef8aa9728013

Child:

25:a7cfe421cb4a

--- a/RTOS-Threads/src/Task3.cpp	Thu May 08 10:49:38 2014 +0000
+++ b/RTOS-Threads/src/Task3.cpp	Thu May 08 13:00:50 2014 +0000
@@ -3,10 +3,10 @@
  * Date:        May 2014
  * Purpose:     Thread3: RC & BT Command, and Telemetry
  * Settings:    50Hz
- */ 
+ */
 
 #define STICK_GAIN 2
-#define STICK_GAIN_YAW 4
+#define STICK_GAIN_YAW 8
 
 #include "tasks.h"
 #include "setup.h"
@@ -30,6 +30,14 @@
 /* Decoded input: [YAW PITCH ROLL] */
 int inputYPR[3];
 
+//typedef enum {
+enum FLIGHT_MODE {
+    RATE,
+    STABLE
+};
+
+FLIGHT_MODE mode;
+
 void Task3(void const *argument)
 {
     if (BT.readable()) {
@@ -186,6 +194,47 @@
                 rollPIDrate.setTunings(KP_ROLL_RATE, PID_TI_RATE, 0.0);
                 BT.printf("KP R rate: %2.5f\n", KP_ROLL_RATE);
                 break;
+
+            case '6':
+                box_demo = false;
+                rc_out = false;
+                gyro_out = false;
+                ESC_check = false;
+                KP_PITCH_STABLE += 0.1;
+                pitchPIDstable.setTunings(KP_PITCH_STABLE, PID_TI_STABLE, 0.0);
+                BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
+                break;
+            case 'Y':
+            case 'y':
+                box_demo = false;
+                rc_out = false;
+                gyro_out = false;
+                ESC_check = false;
+                KP_PITCH_STABLE -= 0.1;
+                pitchPIDstable.setTunings(KP_PITCH_STABLE, PID_TI_STABLE, 0.0);
+                BT.printf("KP P stable: %2.5f\n", KP_PITCH_STABLE);
+                break;
+
+            case '7':
+                box_demo = false;
+                rc_out = false;
+                gyro_out = false;
+                ESC_check = false;
+                KP_ROLL_STABLE += 0.1;
+                rollPIDstable.setTunings(KP_ROLL_STABLE, PID_TI_STABLE, 0.0);
+                BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
+                break;
+            case 'U':
+            case 'u':
+                box_demo = false;
+                rc_out = false;
+                gyro_out = false;
+                ESC_check = false;
+                KP_ROLL_STABLE -= 0.1;
+                rollPIDstable.setTunings(KP_ROLL_STABLE, PID_TI_STABLE, 0.0);
+                BT.printf("KP R stable: %2.5f\n", KP_ROLL_STABLE);
+                break;
+
             case 'A':
                 if (!armed) {
                     pitchPIDstable.reset();
@@ -283,9 +332,12 @@
     RCCommand[0] = rxModule[3].pulsewidth(); // Yaw
     RCCommand[4] = rxModule[4].pulsewidth(); // AUX
 
+    if (RCCommand[4] > 1500)
+        mode = RATE;
+
     inputYPR[0] = (RCCommand[0]-1500)*9/100*STICK_GAIN_YAW;
     inputYPR[1] = (RCCommand[1]-1500)*-1*9/100*STICK_GAIN;
-    inputYPR[2] = (RCCommand[2]-1500)*9/100*STICK_GAIN;
+    inputYPR[2] = (RCCommand[2]-1500)*-1*9/100*STICK_GAIN;
 
     if (rxModule[1].stallTimer.read_us() > 18820) {
         //armed = false;