[求助]MWC移植问题

qiguibao

    最近将MWC的PID算法和互补滤波移植到了自己的STM32飞控上，用MWC飞控测试好用的PID参数，结果四旋翼震动很厉害，很快就侧翻。分析可以看出MWC代码的PID参数给的是挺大的，但MWC飞控却不震荡。代码参考的是MWC2.2，PID参数用的是MWC2.2的默认参数，默认参数飞行效果就不错。不知道大家有没有遇到类似的问题~


    移植的对应ROLL和PITCH的PID部分代码：

1.         for(axis=0;axis<3;axis++)
   
2.         {
   
3.                 if(axis<2) //ROLL AND PITCH
   
4.                 {                                                  
   
5.                         errorAngle = (s32)(constrain(2*rcCommand[axis] ,-500,+500) - angle[axis]); //16 bits is ok here
   
6.                         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
   
7.                         PTermACC = constrain(PTermACC,-dynPIDconf.D8[PIDLEVEL]*5,+dynPIDconf.D8[PIDLEVEL]*5);//角度P为90，对应于弧度的P大约是420
   
8.                        
   
9.                         errorAngleI[axis]     = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);    // WindUp     //16 bits is ok here
   
10.                         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
    
11.                 }
    
12.                 if ( axis<2 ) //ROLL AND PITCH
    
13.                 {
    
14.                         PTerm = PTermACC;
    
15.                         ITerm = ITermACC;
    
16.                 }
    
17.                 else                   //YAW
    
18.                 {
    
19.                         PTerm = PTermGYRO;
    
20.                         ITerm = ITermGYRO;
    
21.                 }
    
22. 
    
23.                 PTerm -= ((s32)gyroData[axis]*dynP8[axis])/10/8; // 32 bits is needed for calculation   
    
24.                                    
    
25.                 delta          = gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    
26.                 lastGyro[axis] = gyroData[axis];                                   //连续两次读取的数据差被限制在800以内
    
27.                 deltaSum       = delta1[axis]+delta2[axis]+delta;
    
28.                 delta2[axis]   = delta1[axis];
    
29.                 delta1[axis]   = delta;
    
30.                
    
31.                 DTerm = ((s32)deltaSum*dynD8[axis])/32;        // 32 bits is needed for calculation
    
32.                                    
    
33.                 g_axisPID[axis] =  PTerm + ITerm - DTerm;
    
34.         }

复制代码

    相关变量类型定义：

1. u8 axis;
   
2. s32 angle[3];
   
3. s16 gyroData[3];
   
4. s32 errorAngle;
   
5. s32 delta,deltaSum;
   
6. s32 PTerm,ITerm,DTerm;
   
7. s32 PTermACC = 0 , ITermACC = 0 , PTermGYRO = 0 , ITermGYRO = 0;
   
8. static s16 lastGyro[3] = {0,0,0};
   
9. static s32 delta1[3]={0,0,0},delta2[3]={0,0,0};
   
10. static s32 errorGyroI[3] = {0,0,0};
    
11. static s32 errorAngleI[3] = {0,0,0};

复制代码