Diff: RTOS-Setup/src/setup.cpp

Revision:

39:02782ad251db

Parent:

38:ef65533cca32

Child:

48:9dbdc4144f00

--- a/RTOS-Setup/src/setup.cpp	Tue May 13 13:05:03 2014 +0000
+++ b/RTOS-Setup/src/setup.cpp	Wed May 14 12:42:39 2014 +0000
@@ -3,19 +3,6 @@
  * Date:    May 2014
  * Purpose: Setup code to initialise all device
  */
-float KP_PITCH_STABLE = 0.5;//0.8;    // 0.5;
-float KP_ROLL_STABLE = 0.5;//0.8;     // 0.5;
-
-float KP_YAW_RATE = 8.5;        // 8.5 for RATE MODE
-float KP_PITCH_RATE = 7.2;//7.6;      // 8.5 for RATE MODE
-float KP_ROLL_RATE = 7.2;//7.5;//7.6;       // 8.5 for RATE MODE
-
-float TI_YAW_RATE = 0.0;//2.0;
-float TI_PITCH_RATE = 1.0;//2.0;
-float TI_ROLL_RATE = 1.0;//2.7;
-
-float PID_TI_STABLE = 0.0;//1.0;
-
 #include "setup.h"
 #include "tasks.h"
 
@@ -27,13 +14,27 @@
 HMC5883L compass(p9, p10);
 #endif
 
-PID pitchPIDstable(KP_PITCH_STABLE, PID_TI_STABLE, 0.0, TASK2_MASTER_PERIOD/1000.0);
-PID rollPIDstable(KP_ROLL_STABLE, PID_TI_STABLE, 0.0, TASK2_MASTER_PERIOD/1000.0);
+float KP_YAW_RATE           =   P_Y_RATE;                
+float KP_PITCH_RATE         =   P_P_RATE;
+float KP_ROLL_RATE          =   P_R_RATE;
+
+float TI_YAW_RATE           =   TI_Y_RATE;
+float TI_PITCH_RATE         =   TI_P_RATE;
+float TI_ROLL_RATE          =   TI_R_RATE;
 
-PID yawPIDrate(KP_YAW_RATE, 0, 0.0, TASK2_SLAVE_PERIOD/1000.0);
+float KP_PITCH_ATTITUDE     =   P_P_ATTITUDE;
+float KP_ROLL_ATTITUDE      =   P_R_ATTITUDE;
+
+float TI_PITCH_ATTITUDE     =   TI_P_ATTITUDE;
+float TI_ROLL_ATTITUDE      =   TI_R_ATTITUDE;
+
+PID yawPIDrate(KP_YAW_RATE, TI_Y_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
 PID pitchPIDrate(KP_PITCH_RATE, TI_PITCH_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
 PID rollPIDrate(KP_ROLL_RATE, TI_ROLL_RATE, 0.0, TASK2_SLAVE_PERIOD/1000.0);
 
+PID pitchPIDattitude(KP_PITCH_ATTITUDE, TI_PITCH_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);
+PID rollPIDattitude(KP_ROLL_ATTITUDE, TI_ROLL_ATTITUDE, 0.0, TASK2_MASTER_PERIOD/1000.0);
+
 PwmOut ESC[4] = {p21, p22, p23, p24};
 
 
@@ -118,15 +119,15 @@
 // ****************************************************************
 bool setup_PID(void)
 {
-    pitchPIDstable.setInputLimits(-90.0, 90.0);
-    pitchPIDstable.setOutputLimits(-250, 250.0);
-    pitchPIDstable.setBias(0.0);
-    pitchPIDstable.setMode(AUTO_MODE);
+    pitchPIDattitude.setInputLimits(-90.0, 90.0);
+    pitchPIDattitude.setOutputLimits(-250, 250.0);
+    pitchPIDattitude.setBias(0.0);
+    pitchPIDattitude.setMode(AUTO_MODE);
 
-    rollPIDstable.setInputLimits(-90.0, 90.0);
-    rollPIDstable.setOutputLimits(-250, 250.0);
-    rollPIDstable.setBias(0.0);
-    rollPIDstable.setMode(AUTO_MODE);
+    rollPIDattitude.setInputLimits(-90.0, 90.0);
+    rollPIDattitude.setOutputLimits(-250, 250.0);
+    rollPIDattitude.setBias(0.0);
+    rollPIDattitude.setMode(AUTO_MODE);
 
     yawPIDrate.setInputLimits(-500.0, 500.0);
     yawPIDrate.setOutputLimits(-200.0, 200.0);
@@ -143,8 +144,8 @@
     rollPIDrate.setBias(0.0);
     rollPIDrate.setMode(AUTO_MODE);
     
-    pitchPIDstable.reset();
-    rollPIDstable.reset();
+    pitchPIDattitude.reset();
+    rollPIDattitude.reset();
     yawPIDrate.reset();
     pitchPIDrate.reset();
     rollPIDrate.reset();