pid.h

00001 short alti_con, roll_con, yaw_con, pitch_con;
00002 char yaw_state1 = 0, yaw_state2 = 0, yaw_cnt = 0;
00003 #define PID_max 80
00004 #define Int_max 100
00005 #define Int_min -100
00006 
00007 float PIDRoll, PIDPitch, PIDYaw, PIDAlti;
00008 float Int_Roll, Int_Pitch;
00009 float Roll_error, Roll_prev_error, Roll_ref = 0, Roll_int_error = 0;
00010 float Pitch_error, Pitch_prev_error, Pitch_ref = 0, Pitch_int_error = 0;
00011 float Yaw_error, Yaw_prev_error, Yaw_ref, Yaw_prev_ref;
00012 float Alti_error, Alti_prev_error, Z_rate_error, Alti_ref, Alti_I_error;
00013 
00014 
00015 void PID_Roll() {
00016 
00017     gainRoll.p = 0.65;//28;  //1.05;
00018     gainRoll.d = 0.5;
00019     gainRoll.i = 0.035;
00020     gainRoll.r = 0.175;//9;
00021     //if(fcc.US < 1000){
00022     //gainRoll.p = 1.05;   //1.05 Take-off Gain
00023     //gainRoll.r = 0.275;   //0.275 Take-off Gain
00024     //}
00025 
00026     Roll_ref = (RC.rolls / 7);
00027     if (Roll_ref>30) Roll_ref = 30;
00028     else if (Roll_ref<-30) Roll_ref = -30;
00029    
00030     Roll_error = fcc.roll -  Roll_ref;
00031     Roll_int_error += Roll_error;
00032     
00033     if (Roll_int_error > Int_max) Roll_int_error = Int_max;
00034     else if (Roll_int_error < Int_min) Roll_int_error = Int_min;
00035     Int_Roll = gainRoll.i * Roll_int_error;
00036     
00037     PIDRoll = gainRoll.p * Roll_error;
00038     PIDRoll += Int_Roll;
00039     PIDRoll += (gainRoll.d * (Roll_error - Roll_prev_error));
00040     PIDRoll += (gainRoll.r * fcc.g_roll);
00041 
00042     if (PIDRoll > PID_max) PIDRoll = PID_max;
00043     else if (PIDRoll < -PID_max) PIDRoll = -PID_max;
00044 
00045     Roll_prev_error = Roll_error;
00046     return;
00047 }
00048 
00049 void PID_Pitch() {
00050 
00051     gainPitch.p = gainRoll.p;//1.075;  //1.35    //1.05;
00052     gainPitch.d = gainRoll.d;//0.9635;
00053     gainPitch.i = gainRoll.i;//0.021;
00054     gainPitch.r = gainRoll.r;// 0.175;  //0.29
00055     //if(fcc.US < 1000){
00056     // gainPitch.p = 1.05;  //1.05 Take-off Gain
00057     // gainPitch.r = 0.275;  //0.275 Take-off Gain
00058     //}
00059 
00060     Pitch_ref = (-RC.pitchs / 6);
00061     if (Pitch_ref>30) Pitch_ref = 30;
00062     else if (Pitch_ref<-30) Pitch_ref = -30;
00063     
00064     Pitch_error = fcc.pitch - Pitch_ref;  
00065     Pitch_int_error += Pitch_error;   
00066         
00067     if (Pitch_int_error > Int_max) Pitch_int_error = Int_max;
00068     else if (Pitch_int_error < Int_min) Pitch_int_error = Int_min;
00069     Int_Pitch = gainPitch.i * Pitch_int_error;
00070 
00071     PIDPitch = (gainPitch.p * Pitch_error);
00072     PIDPitch += Int_Pitch;
00073     PIDPitch += (gainPitch.d * (Pitch_error - Pitch_prev_error));
00074     PIDPitch += (gainPitch.r * fcc.g_pitch);
00075 
00076     if (PIDPitch > PID_max) PIDPitch = PID_max;
00077     else if (PIDPitch < -PID_max) PIDPitch = -PID_max;
00078 
00079     Pitch_prev_error = Pitch_error;
00080     return;
00081 }
00082 
00083 
00084 void PID_Yaw() {
00085 
00086     gainYaw.p = 1.35;  //1.35 Take-off Gain   //1.15;
00087     //gainYaw.d = 0;
00088     gainYaw.r = 0.25;  //0.35 Take-off Gain
00089 
00090     if (RC.yaws>=50 || RC.yaws<=-50) yaw_state1 = 1;
00091     if (yaw_state1 == 1)if (RC.yaws>50 || RC.yaws<-50) yaw_state2 = 1;
00092     if (yaw_state1 == 1 && yaw_state2 == 1) yaw_cnt++;
00093     if (yaw_cnt == 10) {
00094         Yaw_ref = fcc.yaw;
00095         yaw_state1 = yaw_state2 = yaw_cnt = 0;
00096     }
00097     Yaw_error = Yaw_ref - fcc.yaw;
00098     if (Yaw_error>180) Yaw_error -= 360;
00099     if (Yaw_error<-180) Yaw_error += 360;
00100 
00101     PIDYaw = (gainYaw.p * Yaw_error);
00102     //PIDYaw += (gainYaw.i * Yaw_error);
00103     //PIDYaw += (gainYaw.d * (Yaw_error - Yaw_prev_error));
00104     PIDYaw -= (gainYaw.r * fcc.g_yaw);
00105 
00106     if (PIDYaw > PID_max) PIDYaw = PID_max;
00107     if (PIDYaw < -PID_max) PIDYaw = -PID_max;
00108 
00109     Yaw_prev_error = Yaw_error;
00110     return;
00111 }
00112 
00113 
00114 
00115 void pid() {
00116 
00117     alti_con = RC.throttles;
00118 
00119 
00120     PID_Roll();
00121     roll_con = - PIDRoll; //RC.rolls * 0.14 ;//
00122 
00123     PID_Pitch();
00124     pitch_con =  PIDPitch; //RC.pitchs  * 0.14; //
00125 
00126     PID_Yaw();
00127     yaw_con = (RC.yaws * 0.3) + PIDYaw;
00128     if (alti_con < 5) yaw_con = 0;
00129 
00130 }