mwc飞控PID算法解析
0.说明
基于mwc2.3的pid算法解析,2.3中增加了一种新的pid算法,在此分别解析.
P:比例
I:积分
D:微分
1.老版PID代码
代码大概在MultiWii.cpp的1350行上下.
1 if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512); 2 3 // PITCH & ROLL 4 for(axis = 0; axis < 2; axis++) { 5 rc = rcCommand[axis]<<1; 6 error = rc - imu.gyroData[axis]; 7 errorGyroI[axis] = constrain(errorGyroI[axis]+error,-16000,+16000); // WindUp 16 bits is ok here 8 if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0; 9 10 ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 11 12 PTerm = mul(rc,conf.pid[axis].P8)>>6; 13 14 if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC 15 // 50 degrees max inclination 16 errorAngle = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here 17 errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here 18 19 PTermACC = mul(errorAngle,conf.pid[PIDLEVEL].P8)>>7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result 20 21 int16_t limit = conf.pid[PIDLEVEL].D8*5; 22 PTermACC = constrain(PTermACC,-limit,+limit); 23 24 ITermACC = mul(errorAngleI[axis],conf.pid[PIDLEVEL].I8)>>12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result 25 26 ITerm = ITermACC + ((ITerm-ITermACC)*prop>>9); 27 PTerm = PTermACC + ((PTerm-PTermACC)*prop>>9); 28 } 29 30 PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation 31 32 delta = imu.gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 33 lastGyro[axis] = imu.gyroData[axis]; 34 DTerm = delta1[axis]+delta2[axis]+delta; 35 delta2[axis] = delta1[axis]; 36 delta1[axis] = delta; 37 38 DTerm = mul(DTerm,dynD8[axis])>>5; // 32 bits is needed for calculation 39 40 axisPID[axis] = PTerm + ITerm - DTerm; 41 } 42 43 //YAW 44 #define GYRO_P_MAX 300 45 #define GYRO_I_MAX 250 46 47 rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30)) >> 5; 48 49 error = rc - imu.gyroData[YAW]; 50 errorGyroI_YAW += mul(error,conf.pid[YAW].I8); 51 errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28)); 52 if (abs(rc) > 50) errorGyroI_YAW = 0; 53 54 PTerm = mul(error,conf.pid[YAW].P8)>>6; 55 #ifndef COPTER_WITH_SERVO 56 int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8; 57 PTerm = constrain(PTerm,-limit,+limit); 58 #endif 59 60 ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX); 61 62 axisPID[YAW] = PTerm + ITerm;
2.老版PID代码解析
1 // 如果是HORIZON模式 2 if (f.HORIZON_MODE) { 3 // 俯仰和翻滚遥控值的最大值,同时限制小于512 4 prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512); 5 } 6 7 // PITCH --> 1 & ROLL --> 0 8 for (axis = 0; axis < 2; axis++) { 9 10 // 遥控信号 11 rc = rcCommand[axis] << 1; 12 13 // 误差(遥控收到的值 与 传感器值的差) 14 error = rc - imu.gyroData[axis]; 15 16 // 计算误差积分 17 errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000);// WindUp 16 bits is ok here 18 19 // 如果传感器误差超过640,忽略误差积分 20 if (abs(imu.gyroData[axis]) > 640){ 21 errorGyroI[axis] = 0; 22 } 23 24 // I值计算 25 ITerm = (errorGyroI[axis] >> 7) * conf.pid[axis].I8 >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000 26 27 // P值计算 28 PTerm = (int32_t)rc * conf.pid[axis].P8 >> 6; 29 30 // 如果开启了自稳模式或HORIZON模式 31 if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC 32 // 50 degrees max inclination 33 // 使用接收器和gps的接收值减去传感器数据,计算出偏差 34 errorAngle = constrain(rc + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here 35 36 // 每次循环时计算偏差积分 37 errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000); // WindUp //16 bits is ok here 38 39 // 偏差乘上P，再除以128 40 PTermACC = ((int32_t)errorAngle * conf.pid[PIDLEVEL].P8) >> 7; // 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result 41 42 // 计算限制值,D值的5倍 43 int16_t limit = conf.pid[PIDLEVEL].D8 * 5; 44 // 45 PTermACC = constrain(PTermACC,-limit,+limit); 46 47 // 偏差积分 * I / 4096 48 ITermACC = ((int32_t)errorAngleI[axis] * conf.pid[PIDLEVEL].I8) >> 12; // 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result 49 50 // I值微调(利用prop,这个prop是利用遥控信号算出来的,可以看代码最开头两行) 51 ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9); 52 53 // P值微调(利用prop,这个prop是利用遥控信号算出来的,可以看代码最开头两行) 54 PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9); 55 } 56 57 // P值减去一个内容 //TODO 58 PTerm -= ((int32_t)imu.gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation 59 60 // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800 61 // 偏差 等于 陀螺仪数据 - 上一次的陀螺仪数据 62 delta = imu.gyroData[axis] - lastGyro[axis]; 63 64 // 更新记录的陀螺仪数据 65 lastGyro[axis] = imu.gyroData[axis]; 66 67 // D值计算(利用前三次循环的D值都参与计算) 68 DTerm = delta1[axis] + delta2[axis] + delta; 69 70 // 更新之前记录的D值 71 delta2[axis] = delta1[axis]; 72 delta1[axis] = delta; 73 74 // 偏差微分 * D / 32 75 DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;// 32 bits is needed for calculation 76 77 // 对应马达的值等于P+I+D(这里D值定义的是一个负值,减相当于加) 78 axisPID[axis] = PTerm + ITerm - DTerm; 79 } 80 81 // 偏航(YAW) 82 #define GYRO_P_MAX 300 // 陀螺仪最大P值 83 #define GYRO_I_MAX 250 // 陀螺仪最大I值 84 85 rc = (int32_t)rcCommand[YAW] * (2*conf.yawRate + 30) >> 5; 86 87 error = rc - imu.gyroData[YAW]; 88 errorGyroI_YAW += (int32_t)error*conf.pid[YAW].I8; 89 errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28)); 90 if (abs(rc) > 50) errorGyroI_YAW = 0; 91 92 PTerm = (int32_t)error*conf.pid[YAW].P8>>6; 93 #ifndef COPTER_WITH_SERVO 94 int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8; 95 PTerm = constrain(PTerm,-limit,+limit); 96 #endif 97 98 ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX); 99 100 axisPID[YAW] = PTerm + ITerm;
