mwc飞控PID算法解析

0.说明

1.老版PID代码

if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512);

// PITCH & ROLL
for(axis = 0; axis < 2; axis++) {
rc = rcCommand[axis]<<1;
error = rc - imu.gyroData[axis];
errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);       // WindUp   16 bits is ok here
if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0;

ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6;                        // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000

PTerm = mul(rc,conf.pid[axis].P8)>>6;

if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
// 50 degrees max inclination
errorAngle         = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);                                                // WindUp     //16 bits is ok here

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

int16_t limit      = conf.pid[PIDLEVEL].D8*5;
PTermACC           = constrain(PTermACC,-limit,+limit);

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

ITerm              = ITermACC + ((ITerm-ITermACC)*prop>>9);
PTerm              = PTermACC + ((PTerm-PTermACC)*prop>>9);
}

PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation

delta          = imu.gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyro[axis] = imu.gyroData[axis];
DTerm          = delta1[axis]+delta2[axis]+delta;
delta2[axis]   = delta1[axis];
delta1[axis]   = delta;

DTerm = mul(DTerm,dynD8[axis])>>5;        // 32 bits is needed for calculation

axisPID[axis] =  PTerm + ITerm - DTerm;
}

//YAW
#define GYRO_P_MAX 300
#define GYRO_I_MAX 250

rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30))  >> 5;

error = rc - imu.gyroData[YAW];
errorGyroI_YAW  += mul(error,conf.pid[YAW].I8);
errorGyroI_YAW  = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
if (abs(rc) > 50) errorGyroI_YAW = 0;

PTerm = mul(error,conf.pid[YAW].P8)>>6;
#ifndef COPTER_WITH_SERVO
int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
PTerm = constrain(PTerm,-limit,+limit);
#endif

ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);

axisPID[YAW] =  PTerm + ITerm;

2.老版PID代码解析

// 如果是HORIZON模式
if (f.HORIZON_MODE) {
// 俯仰和翻滚遥控值的最大值,同时限制小于512
prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512);
}

// PITCH --> 1 & ROLL --> 0
for (axis = 0; axis < 2; axis++) {

// 遥控信号
rc = rcCommand[axis] << 1;

// 误差(遥控收到的值 与 传感器值的差)
error = rc - imu.gyroData[axis];

// 计算误差积分
errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000);// WindUp 16 bits is ok here

// 如果传感器误差超过640,忽略误差积分
if (abs(imu.gyroData[axis]) > 640){
errorGyroI[axis] = 0;
}

// I值计算
ITerm = (errorGyroI[axis] >> 7) * conf.pid[axis].I8 >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000

// P值计算
PTerm = (int32_t)rc * conf.pid[axis].P8 >> 6;

// 如果开启了自稳模式或HORIZON模式
if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
// 50 degrees max inclination
// 使用接收器和gps的接收值减去传感器数据,计算出偏差
errorAngle = constrain(rc + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here

// 每次循环时计算偏差积分
errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);  // WindUp //16 bits is ok here

// 偏差乘上P，再除以128
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

// 计算限制值,D值的5倍
int16_t limit = conf.pid[PIDLEVEL].D8 * 5;
//
PTermACC = constrain(PTermACC,-limit,+limit);

// 偏差积分 * I / 4096
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

// I值微调(利用prop,这个prop是利用遥控信号算出来的,可以看代码最开头两行)
ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);

// P值微调(利用prop,这个prop是利用遥控信号算出来的,可以看代码最开头两行)
PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
}

// P值减去一个内容 //TODO
PTerm -= ((int32_t)imu.gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation

// 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
// 偏差 等于 陀螺仪数据 - 上一次的陀螺仪数据
delta  = imu.gyroData[axis] - lastGyro[axis];

// 更新记录的陀螺仪数据
lastGyro[axis] = imu.gyroData[axis];

// D值计算(利用前三次循环的D值都参与计算)
DTerm = delta1[axis] + delta2[axis] + delta;

// 更新之前记录的D值
delta2[axis] = delta1[axis];
delta1[axis] = delta;

// 偏差微分 * D / 32
DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;// 32 bits is needed for calculation

// 对应马达的值等于P+I+D(这里D值定义的是一个负值,减相当于加)
axisPID[axis] = PTerm + ITerm - DTerm;
}

// 偏航(YAW)
#define GYRO_P_MAX 300  // 陀螺仪最大P值
#define GYRO_I_MAX 250  // 陀螺仪最大I值

rc = (int32_t)rcCommand[YAW] * (2*conf.yawRate + 30) >> 5;

error = rc - imu.gyroData[YAW];
errorGyroI_YAW += (int32_t)error*conf.pid[YAW].I8;
errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
if (abs(rc) > 50) errorGyroI_YAW = 0;

PTerm = (int32_t)error*conf.pid[YAW].P8>>6;
#ifndef COPTER_WITH_SERVO
int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
PTerm = constrain(PTerm,-limit,+limit);
#endif

ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);

axisPID[YAW] = PTerm + ITerm;