pidAttitudeControl.cpp

00001 #include "pidAttitudeControl.h"
00002 
00003 pidAttitudeControl::pidAttitudeControl()
00004 {
00005     for(int i=0; i<4; i++) PIDFf_gains[i]=0;
00006     for(int i=0; i<4; i++) {
00007         for(int j=0; j<2; j++)
00008             PIDFf_limits[i][j]=0;
00009     }
00010     for(int i=0; i<2; i++) output_limits[i]=0;
00011     for(int i=0; i<2; i++) controlError[i] =0;
00012     for(int i=0; i<4; i++) PIDFf_terms[i]=0;
00013     Summ_term=0; 
00014 
00015     //load default control values from config file
00016     PIDFf_limits[0][0] = (-1.0)*AttPIDFf_lim_P;
00017     PIDFf_limits[0][1] = (1.0)*AttPIDFf_lim_P;
00018 
00019     PIDFf_limits[1][0] = (-1.0)*AttPIDFf_lim_I;
00020     PIDFf_limits[1][1] = (1.0)*AttPIDFf_lim_I;
00021 
00022     PIDFf_limits[2][0] = (-1.0)*AttPIDFf_lim_D;
00023     PIDFf_limits[2][1] = (1.0)*AttPIDFf_lim_D;
00024 
00025     PIDFf_limits[3][0] = (-1.0)*AttPIDFf_lim_Ff;
00026     PIDFf_limits[3][1] = (1.0)*AttPIDFf_lim_Ff;
00027     
00028     output_limits[0]=AttPIDFf_range_min;
00029     output_limits[1]=AttPIDFf_range_max;
00030     
00031     PIDFf_gains[0]=AttPIDFf_kP/1000;
00032     PIDFf_gains[1]=AttPIDFf_kI/1000;
00033     PIDFf_gains[2]=AttPIDFf_kD/1000;
00034     PIDFf_gains[3]=AttPIDFf_kFf/1000;
00035     
00036     prevSetPoint=0.0;
00037 }
00038 
00039 void pidAttitudeControl::setGains(float kP,float kI, float kD, float kFf)
00040 {
00041     PIDFf_gains[0]=kP/1000;
00042     PIDFf_gains[1]=kI/1000;
00043     PIDFf_gains[2]=kD/1000;
00044     PIDFf_gains[3]=kFf/1000;
00045 }
00046 
00047 void pidAttitudeControl::setTermLimits(float P_min, float P_max, float I_min, float I_max, float D_min, float D_max, float Ff_min, float Ff_max)
00048 {
00049     PIDFf_limits[0][0] = P_min;
00050     PIDFf_limits[0][1] = P_max;
00051 
00052     PIDFf_limits[1][0] = I_min;
00053     PIDFf_limits[1][1] = I_max;
00054 
00055     PIDFf_limits[2][0] = D_min;
00056     PIDFf_limits[2][1] = D_max;
00057 
00058     PIDFf_limits[3][0] = Ff_min;
00059     PIDFf_limits[3][1] = Ff_max;
00060 }
00061 
00062 void pidAttitudeControl::setOutputRange(float outMin, float outMax)
00063 {
00064     output_limits[0]=outMin;
00065     output_limits[1]=outMax;
00066 }
00067 
00068 float pidAttitudeControl::calcOutput(float setPoint, float feedback, float servoTime)
00069 {
00070     controlError[1] = setPoint - feedback; //control error
00071     
00072     //proportional term 
00073     PIDFf_terms[0] = PIDFf_gains[0]*controlError[1];
00074     PIDFf_terms[0] = generalFunctions::constrain_f(PIDFf_terms[0], PIDFf_limits[0][0], PIDFf_limits[0][1]);
00075 
00076     
00077     //integral term 
00078     PIDFf_terms[1] += PIDFf_gains[1]*controlError[1]*servoTime;
00079     PIDFf_terms[1] = generalFunctions::constrain_f(PIDFf_terms[1], PIDFf_limits[1][0], PIDFf_limits[1][1]);
00080 
00081     //differential term
00082     PIDFf_terms[2] = PIDFf_gains[2] * (controlError[1] - controlError[0]) / servoTime;
00083     PIDFf_terms[2] = generalFunctions::constrain_f(PIDFf_terms[2], PIDFf_limits[2][0], PIDFf_limits[2][1]);
00084     
00085     //feedforward term
00086     PIDFf_terms[3] = PIDFf_gains[3]*setPoint;
00087     PIDFf_terms[3] = generalFunctions::constrain_f(PIDFf_terms[3], PIDFf_limits[3][0], PIDFf_limits[3][1]);
00088     
00089     //summ term
00090     Summ_term = PIDFf_terms[0] + PIDFf_terms[1] + PIDFf_terms[2] + PIDFf_terms[3];
00091     Summ_term = generalFunctions::constrain_f(Summ_term, output_limits[0], output_limits[1]);
00092     
00093     controlError[0] = controlError[1]; //store control error for usage in next iteration
00094 
00095     return(Summ_term);
00096 }