[求助]MWC移植问题
本帖最后由 qiguibao 于 2014-4-6 11:51 编辑最近将MWC的PID算法和互补滤波移植到了自己的STM32飞控上，用MWC飞控测试好用的PID参数，结果四旋翼震动很厉害，很快就侧翻。分析可以看出MWC代码的PID参数给的是挺大的，但MWC飞控却不震荡。代码参考的是MWC2.2，PID参数用的是MWC2.2的默认参数，默认参数飞行效果就不错。不知道大家有没有遇到类似的问题~
移植的对应ROLL和PITCH的PID部分代码：
for(axis=0;axis<3;axis++)
{
if(axis<2) //ROLL AND PITCH
{
errorAngle = (s32)(constrain(2*rcCommand ,-500,+500) - angle); //16 bits is ok here
PTermACC = ((s32)errorAngle*dynPIDconf.P8)/100;//2^7=128,// 32 bits is needed for calculation: errorAngle*P8 could exceed 32768 16 bits is ok for result
PTermACC = constrain(PTermACC,-dynPIDconf.D8*5,+dynPIDconf.D8*5);//角度P为90，对应于弧度的P大约是420
errorAngleI = constrain(errorAngleI+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
ITermACC = ((s32)errorAngleI*dynPIDconf.I8)>>12; //I=10,最大10000*10/4096=24// 32 bits is needed for calculation:10000*I8 could exceed 32768 16 bits is ok for result
}
if ( axis<2 ) //ROLL AND PITCH
{
PTerm = PTermACC;
ITerm = ITermACC;
}
else //YAW
{
PTerm = PTermGYRO;
ITerm = ITermGYRO;
}
PTerm -= ((s32)gyroData*dynP8)/10/8; // 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; //连续两次读取的数据差被限制在800以内
deltaSum = delta1+delta2+delta;
delta2 = delta1;
delta1 = delta;
DTerm = ((s32)deltaSum*dynD8)/32; // 32 bits is needed for calculation
g_axisPID =PTerm + ITerm - DTerm;
}
相关变量类型定义：
u8 axis;
s32 angle;
s16 gyroData;
s32 errorAngle;
s32 delta,deltaSum;
s32 PTerm,ITerm,DTerm;
s32 PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
static s16 lastGyro = {0,0,0};
static s32 delta1={0,0,0},delta2={0,0,0};
static s32 errorGyroI = {0,0,0};
static s32 errorAngleI = {0,0,0};
页:
[1]