[求助]MWC 2.2中PID代码问题
//**** PITCH & ROLL & YAW PID ****int16_t prop;
prop = min(max(abs(rcCommand),abs(rcCommand)),500); // range
for(axis=0;axis<3;axis++) {
if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
// 50 degrees max inclination,倾角限定在50度，对应下面语句的【-500，500】,angle角度也是数值乘0.1后的结果。
//rcCommand [-500;+500] for ROLL/PITCH/YAW ,‘<<1’扩大到[-1000,1000],与gps角度求和
errorAngle = constrain((rcCommand<<1) + GPS_angle,-500,+500) - angle + conf.angleTrim; //16 bits is ok here
PTermACC = ((int32_t)errorAngle*conf.P8)>>7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC,-conf.D8*5,+conf.D8*5);
errorAngleI = constrain(errorAngleI+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITermACC = ((int32_t)errorAngleI*conf.I8)>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
if (abs(rcCommand)<500) error = (rcCommand<<6)/conf.P8 ; // 16 bits is needed for calculation: 500*64 = 32000 16 bits is ok for result if P8>5 (P>0.5)
else error = ((int32_t)rcCommand<<6)/conf.P8 ; // 32 bits is needed for calculation
error -= gyroData;
PTermGYRO = rcCommand;
errorGyroI= constrain(errorGyroI+error,-16000,+16000); // WindUp 16 bits is ok here
if (abs(gyroData)>640) errorGyroI = 0;
ITermGYRO = ((errorGyroI>>7)*conf.I8)>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
}
if ( f.HORIZON_MODE && axis<2) {
PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9; // the real factor should be 500, but 512 is ok
ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;
} else {
if ( f.ANGLE_MODE && axis<2) {
PTerm = PTermACC;
ITerm = ITermACC;
} else {
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
}
PTerm -= ((int32_t)gyroData*dynP8)>>6; // 32 bits is needed for calculation
delta = gyroData - lastGyro;// 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro = gyroData;
deltaSum = delta1+delta2+delta;
delta2 = delta1;
delta1 = delta;
DTerm = ((int32_t)deltaSum*dynD8)>>5; // 32 bits is needed for calculation
axisPID =PTerm + ITerm - DTerm;
}
上面代码是MWC 2.2中的PID相关代码，有几个问题，麻烦大家指点下~
1.像“ PTermACC = ((int32_t)errorAngle*conf.P8)>>7; ”这条语句里面的移位操作，代码里的位移操作的依据是什么呢呢？
2.“PTerm -= ((int32_t)gyroData*dynP8)>>6; // 32 bits is needed for calculation”，PTermACC减去这部分的依据是什么呢？
页:
[1]