[求助]MWC 2.2中PID代码问题

qiguibao

1.   //**** PITCH & ROLL & YAW PID ****
   
2.   int16_t prop;
   
3.   prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]
   
4. 
   
5.   for(axis=0;axis<3;axis++) {
   
6.     if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
   
7.       // 50 degrees max inclination,倾角限定在50度，对应下面语句的【-500，500】,angle角度也是数值乘0.1后的结果。
   
8.       //rcCommand [-500;+500] for ROLL/PITCH/YAW ,‘<<1’扩大到[-1000,1000],与gps角度求和
   
9.       errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) - angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
   
10.       PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7;                          // 32 bits is needed for calculation: errorAngle*P8[PIDLEVEL] could exceed 32768   16 bits is ok for result
    
11.       PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
    
12. 
    
13.       errorAngleI[axis]     = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);    // WindUp     //16 bits is ok here
    
14.       ITermACC              = ((int32_t)errorAngleI[axis]*conf.I8[PIDLEVEL])>>12;            // 32 bits is needed for calculation:10000*I8 could exceed 32768   16 bits is ok for result
    
15.     }
    
16.     if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
    
17.       if (abs(rcCommand[axis])<500) error =          (rcCommand[axis]<<6)/conf.P8[axis] ; // 16 bits is needed for calculation: 500*64 = 32000      16 bits is ok for result if P8>5 (P>0.5)
    
18.                                else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation
    
19. 
    
20.       error -= gyroData[axis];
    
21. 
    
22.       PTermGYRO = rcCommand[axis];
    
23.       
    
24.       errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);         // WindUp   16 bits is ok here
    
25.       if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
    
26.       ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6;                        // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
    
27.     }
    
28.     if ( f.HORIZON_MODE && axis<2) {
    
29.       PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9;         // the real factor should be 500, but 512 is ok
    
30.       ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;
    
31.     } else {
    
32.       if ( f.ANGLE_MODE && axis<2) {
    
33.         PTerm = PTermACC;
    
34.         ITerm = ITermACC;
    
35.       } else {
    
36.         PTerm = PTermGYRO;
    
37.         ITerm = ITermGYRO;
    
38.       }
    
39.     }
    
40. 
    
41.     PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 32 bits is needed for calculation   
    
42. 
    
43.     delta          = gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
    
44.     lastGyro[axis] = gyroData[axis];
    
45.     deltaSum       = delta1[axis]+delta2[axis]+delta;
    
46.     delta2[axis]   = delta1[axis];
    
47.     delta1[axis]   = delta;
    
48. 
    
49.     DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;        // 32 bits is needed for calculation
    
50.                      
    
51.     axisPID[axis] =  PTerm + ITerm - DTerm;
    
52.   }
    

复制代码

    上面代码是MWC 2.2中的PID相关代码，有几个问题，麻烦大家指点下~
1.像“ PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7; ”这条语句里面的移位操作，代码里的位移操作的依据是什么呢呢？
2.“PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 32 bits is needed for calculation”，PTermACC减去这部分的依据是什么呢？