Diff: pid.h

Revision:

0:d463d5c04541

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/pid.h	Tue Aug 16 05:32:33 2011 +0000
@@ -0,0 +1,130 @@
+short alti_con, roll_con, yaw_con, pitch_con;
+char yaw_state1 = 0, yaw_state2 = 0, yaw_cnt = 0;
+#define PID_max 80
+#define Int_max 100
+#define Int_min -100
+
+float PIDRoll, PIDPitch, PIDYaw, PIDAlti;
+float Int_Roll, Int_Pitch;
+float Roll_error, Roll_prev_error, Roll_ref = 0, Roll_int_error = 0;
+float Pitch_error, Pitch_prev_error, Pitch_ref = 0, Pitch_int_error = 0;
+float Yaw_error, Yaw_prev_error, Yaw_ref, Yaw_prev_ref;
+float Alti_error, Alti_prev_error, Z_rate_error, Alti_ref, Alti_I_error;
+
+
+void PID_Roll() {
+
+    gainRoll.p = 0.65;//28;  //1.05;
+    gainRoll.d = 0.5;
+    gainRoll.i = 0.035;
+    gainRoll.r = 0.175;//9;
+    //if(fcc.US < 1000){
+    //gainRoll.p = 1.05;   //1.05 Take-off Gain
+    //gainRoll.r = 0.275;   //0.275 Take-off Gain
+    //}
+
+    Roll_ref = (RC.rolls / 7);
+    if (Roll_ref>30) Roll_ref = 30;
+    else if (Roll_ref<-30) Roll_ref = -30;
+   
+    Roll_error = fcc.roll -  Roll_ref;
+    Roll_int_error += Roll_error;
+    
+    if (Roll_int_error > Int_max) Roll_int_error = Int_max;
+    else if (Roll_int_error < Int_min) Roll_int_error = Int_min;
+    Int_Roll = gainRoll.i * Roll_int_error;
+    
+    PIDRoll = gainRoll.p * Roll_error;
+    PIDRoll += Int_Roll;
+    PIDRoll += (gainRoll.d * (Roll_error - Roll_prev_error));
+    PIDRoll += (gainRoll.r * fcc.g_roll);
+
+    if (PIDRoll > PID_max) PIDRoll = PID_max;
+    else if (PIDRoll < -PID_max) PIDRoll = -PID_max;
+
+    Roll_prev_error = Roll_error;
+    return;
+}
+
+void PID_Pitch() {
+
+    gainPitch.p = gainRoll.p;//1.075;  //1.35    //1.05;
+    gainPitch.d = gainRoll.d;//0.9635;
+    gainPitch.i = gainRoll.i;//0.021;
+    gainPitch.r = gainRoll.r;// 0.175;  //0.29
+    //if(fcc.US < 1000){
+    // gainPitch.p = 1.05;  //1.05 Take-off Gain
+    // gainPitch.r = 0.275;  //0.275 Take-off Gain
+    //}
+
+    Pitch_ref = (-RC.pitchs / 6);
+    if (Pitch_ref>30) Pitch_ref = 30;
+    else if (Pitch_ref<-30) Pitch_ref = -30;
+    
+    Pitch_error = fcc.pitch - Pitch_ref;  
+    Pitch_int_error += Pitch_error;   
+        
+    if (Pitch_int_error > Int_max) Pitch_int_error = Int_max;
+    else if (Pitch_int_error < Int_min) Pitch_int_error = Int_min;
+    Int_Pitch = gainPitch.i * Pitch_int_error;
+
+    PIDPitch = (gainPitch.p * Pitch_error);
+    PIDPitch += Int_Pitch;
+    PIDPitch += (gainPitch.d * (Pitch_error - Pitch_prev_error));
+    PIDPitch += (gainPitch.r * fcc.g_pitch);
+
+    if (PIDPitch > PID_max) PIDPitch = PID_max;
+    else if (PIDPitch < -PID_max) PIDPitch = -PID_max;
+
+    Pitch_prev_error = Pitch_error;
+    return;
+}
+
+
+void PID_Yaw() {
+
+    gainYaw.p = 1.35;  //1.35 Take-off Gain   //1.15;
+    //gainYaw.d = 0;
+    gainYaw.r = 0.25;  //0.35 Take-off Gain
+
+    if (RC.yaws>=50 || RC.yaws<=-50) yaw_state1 = 1;
+    if (yaw_state1 == 1)if (RC.yaws>50 || RC.yaws<-50) yaw_state2 = 1;
+    if (yaw_state1 == 1 && yaw_state2 == 1) yaw_cnt++;
+    if (yaw_cnt == 10) {
+        Yaw_ref = fcc.yaw;
+        yaw_state1 = yaw_state2 = yaw_cnt = 0;
+    }
+    Yaw_error = Yaw_ref - fcc.yaw;
+    if (Yaw_error>180) Yaw_error -= 360;
+    if (Yaw_error<-180) Yaw_error += 360;
+
+    PIDYaw = (gainYaw.p * Yaw_error);
+    //PIDYaw += (gainYaw.i * Yaw_error);
+    //PIDYaw += (gainYaw.d * (Yaw_error - Yaw_prev_error));
+    PIDYaw -= (gainYaw.r * fcc.g_yaw);
+
+    if (PIDYaw > PID_max) PIDYaw = PID_max;
+    if (PIDYaw < -PID_max) PIDYaw = -PID_max;
+
+    Yaw_prev_error = Yaw_error;
+    return;
+}
+
+
+
+void pid() {
+
+    alti_con = RC.throttles;
+
+
+    PID_Roll();
+    roll_con = - PIDRoll; //RC.rolls * 0.14 ;//
+
+    PID_Pitch();
+    pitch_con =  PIDPitch; //RC.pitchs  * 0.14; //
+
+    PID_Yaw();
+    yaw_con = (RC.yaws * 0.3) + PIDYaw;
+    if (alti_con < 5) yaw_con = 0;
+
+}