|
本帖最后由 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[axis] ,-500,+500) - angle[axis]); //16 bits is ok here
- PTermACC = ((s32)errorAngle*dynPIDconf.P8[PIDLEVEL])/100;//2^7=128,// 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768 16 bits is ok for result
- PTermACC = constrain(PTermACC,-dynPIDconf.D8[PIDLEVEL]*5,+dynPIDconf.D8[PIDLEVEL]*5);//角度P为90,对应于弧度的P大约是420
-
- errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000); // WindUp //16 bits is ok here
- ITermACC = ((s32)errorAngleI[axis]*dynPIDconf.I8[PIDLEVEL])>>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[axis]*dynP8[axis])/10/8; // 32 bits is needed for calculation
-
- delta = gyroData[axis] - lastGyro[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
- lastGyro[axis] = gyroData[axis]; //连续两次读取的数据差被限制在800以内
- deltaSum = delta1[axis]+delta2[axis]+delta;
- delta2[axis] = delta1[axis];
- delta1[axis] = delta;
-
- DTerm = ((s32)deltaSum*dynD8[axis])/32; // 32 bits is needed for calculation
-
- g_axisPID[axis] = PTerm + ITerm - DTerm;
- }
复制代码
相关变量类型定义:
- u8 axis;
- s32 angle[3];
- s16 gyroData[3];
- s32 errorAngle;
- s32 delta,deltaSum;
- s32 PTerm,ITerm,DTerm;
- s32 PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
- static s16 lastGyro[3] = {0,0,0};
- static s32 delta1[3]={0,0,0},delta2[3]={0,0,0};
- static s32 errorGyroI[3] = {0,0,0};
- static s32 errorAngleI[3] = {0,0,0};
复制代码 |
阿莫论坛20周年了!感谢大家的支持与爱护!!
你熬了10碗粥,别人一桶水倒进去,淘走90碗,剩下10碗给你,你看似没亏,其实你那10碗已经没有之前的裹腹了,人家的一桶水换90碗,继续卖。说白了,通货膨胀就是,你的钱是挣来的,他的钱是印来的,掺和在一起,你的钱就贬值了。
|