mwc飛控PID算法解析

時間 2019-12-09

標籤 mwc pid 算法解析简体版

原文原文鏈接

0.說明

基於mwc2.3的pid算法解析,2.3中增長了一種新的pid算法,在此分別解析.算法

P:比例spa

I:積分code

D:微分blog

1.老版PID代碼

代碼大概在MultiWii.cpp的1350行上下.it

 1   if ( f.HORIZON_MODE ) prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),512);
 2 
 3   // PITCH & ROLL
 4   for(axis = 0; axis < 2; axis++) {
 5     rc = rcCommand[axis]<<1;
 6     error = rc - imu.gyroData[axis];
 7     errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);       // WindUp   16 bits is ok here
 8     if (abs(imu.gyroData[axis])>640) errorGyroI[axis] = 0;
 9 
10     ITerm = (errorGyroI[axis]>>7)*conf.pid[axis].I8>>6;                        // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
11 
12     PTerm = mul(rc,conf.pid[axis].P8)>>6;
13     
14     if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
15       // 50 degrees max inclination
16       errorAngle         = constrain(rc + GPS_angle[axis],-500,+500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
17       errorAngleI[axis]  = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);                                                // WindUp     //16 bits is ok here
18 
19       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
20 
21       int16_t limit      = conf.pid[PIDLEVEL].D8*5;
22       PTermACC           = constrain(PTermACC,-limit,+limit);
23 
24       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
25 
26       ITerm              = ITermACC + ((ITerm-ITermACC)*prop>>9);
27       PTerm              = PTermACC + ((PTerm-PTermACC)*prop>>9);
28     }
29 
30     PTerm -= mul(imu.gyroData[axis],dynP8[axis])>>6; // 32 bits is needed for calculation   
31 
32     delta          = imu.gyroData[axis] - lastGyro[axis];  // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
33     lastGyro[axis] = imu.gyroData[axis];
34     DTerm          = delta1[axis]+delta2[axis]+delta;
35     delta2[axis]   = delta1[axis];
36     delta1[axis]   = delta;
37  
38     DTerm = mul(DTerm,dynD8[axis])>>5;        // 32 bits is needed for calculation
39 
40     axisPID[axis] =  PTerm + ITerm - DTerm;
41   }
42 
43   //YAW
44   #define GYRO_P_MAX 300
45   #define GYRO_I_MAX 250
46 
47   rc = mul(rcCommand[YAW] , (2*conf.yawRate + 30))  >> 5;
48 
49   error = rc - imu.gyroData[YAW];
50   errorGyroI_YAW  += mul(error,conf.pid[YAW].I8);
51   errorGyroI_YAW  = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
52   if (abs(rc) > 50) errorGyroI_YAW = 0;
53   
54   PTerm = mul(error,conf.pid[YAW].P8)>>6;
55   #ifndef COPTER_WITH_SERVO
56     int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
57     PTerm = constrain(PTerm,-limit,+limit);
58   #endif
59   
60   ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);
61   
62   axisPID[YAW] =  PTerm + ITerm;

2.老版PID代碼解析

  1 // 若是是HORIZON模式
  2     if (f.HORIZON_MODE) {
  3         // 俯仰和翻滾遙控值的最大值,同時限制小於512
  4         prop = min(max(abs(rcCommand[PITCH]), abs(rcCommand[ROLL])), 512);
  5     }
  6 
  7     // PITCH --> 1 & ROLL --> 0
  8     for (axis = 0; axis < 2; axis++) {
  9 
 10         // 遙控信號
 11         rc = rcCommand[axis] << 1;
 12 
 13         // 偏差(遙控收到的值 與 傳感器值的差)
 14         error = rc - imu.gyroData[axis];
 15 
 16         // 計算偏差積分
 17         errorGyroI[axis] = constrain(errorGyroI[axis] + error, -16000, +16000);// WindUp 16 bits is ok here
 18 
 19         // 若是傳感器偏差超過640,忽略偏差積分
 20         if (abs(imu.gyroData[axis]) > 640){
 21             errorGyroI[axis] = 0;
 22         }
 23 
 24         // I值計算
 25         ITerm = (errorGyroI[axis] >> 7) * conf.pid[axis].I8 >> 6; // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
 26 
 27         // P值計算
 28         PTerm = (int32_t)rc * conf.pid[axis].P8 >> 6;
 29 
 30         // 若是開啓了自穩模式或HORIZON模式
 31         if (f.ANGLE_MODE || f.HORIZON_MODE) { // axis relying on ACC
 32             // 50 degrees max inclination
 33             // 使用接收器和gps的接收值減去傳感器數據,計算出誤差
 34             errorAngle = constrain(rc + GPS_angle[axis], -500, +500) - att.angle[axis] + conf.angleTrim[axis]; //16 bits is ok here
 35 
 36             // 每次循環時計算誤差積分
 37             errorAngleI[axis] = constrain(errorAngleI[axis] + errorAngle, -10000, +10000);  // WindUp //16 bits is ok here
 38 
 39             // 誤差乘上P，再除以128
 40             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
 41 
 42             // 計算限制值,D值的5倍
 43             int16_t limit = conf.pid[PIDLEVEL].D8 * 5;
 44             //
 45             PTermACC = constrain(PTermACC,-limit,+limit);
 46 
 47             // 誤差積分 * I / 4096
 48             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
 49 
 50             // I值微調(利用prop,這個prop是利用遙控信號算出來的,能夠看代碼最開頭兩行)
 51             ITerm = ITermACC + ((ITerm - ITermACC) * prop >> 9);
 52 
 53             // P值微調(利用prop,這個prop是利用遙控信號算出來的,能夠看代碼最開頭兩行)
 54             PTerm = PTermACC + ((PTerm - PTermACC) * prop >> 9);
 55         }
 56 
 57         // P值減去一個內容 //TODO
 58         PTerm -= ((int32_t)imu.gyroData[axis] * dynP8[axis]) >> 6; // 32 bits is needed for calculation
 59 
 60         // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
 61         // 誤差 等於 陀螺儀數據 - 上一次的陀螺儀數據
 62         delta  = imu.gyroData[axis] - lastGyro[axis];
 63 
 64         // 更新記錄的陀螺儀數據
 65         lastGyro[axis] = imu.gyroData[axis];
 66 
 67         // D值計算(利用前三次循環的D值都參與計算)
 68         DTerm = delta1[axis] + delta2[axis] + delta;
 69 
 70         // 更新以前記錄的D值
 71         delta2[axis] = delta1[axis];
 72         delta1[axis] = delta;
 73 
 74         // 誤差微分 * D / 32
 75         DTerm = ((int32_t)DTerm * dynD8[axis]) >> 5;// 32 bits is needed for calculation
 76 
 77         // 對應馬達的值等於P+I+D(這裏D值定義的是一個負值,減至關於加)
 78         axisPID[axis] = PTerm + ITerm - DTerm;
 79     }
 80 
 81     // 偏航(YAW)
 82     #define GYRO_P_MAX 300  // 陀螺儀最大P值
 83     #define GYRO_I_MAX 250  // 陀螺儀最大I值
 84 
 85     rc = (int32_t)rcCommand[YAW] * (2*conf.yawRate + 30) >> 5;
 86 
 87     error = rc - imu.gyroData[YAW];
 88     errorGyroI_YAW += (int32_t)error*conf.pid[YAW].I8;
 89     errorGyroI_YAW = constrain(errorGyroI_YAW, 2-((int32_t)1<<28), -2+((int32_t)1<<28));
 90     if (abs(rc) > 50) errorGyroI_YAW = 0;
 91 
 92     PTerm = (int32_t)error*conf.pid[YAW].P8>>6;
 93     #ifndef COPTER_WITH_SERVO
 94         int16_t limit = GYRO_P_MAX-conf.pid[YAW].D8;
 95         PTerm = constrain(PTerm,-limit,+limit);
 96     #endif
 97 
 98     ITerm = constrain((int16_t)(errorGyroI_YAW>>13),-GYRO_I_MAX,+GYRO_I_MAX);
 99 
100     axisPID[YAW] = PTerm + ITerm;

相關標籤/搜索

每日一句

每一个你不满意的现在，都有一个你没有努力的曾经。