码迷,mamicode.com
首页 > 编程语言 > 详细

mwc飞控PID算法解析

时间:2015-06-12 20:45:53      阅读:644      评论:0      收藏:0      [点我收藏+]

标签:

0.说明

基于mwc2.3的pid算法解析,2.3中增加了一种新的pid算法,在此分别解析.

P:比例

I:积分

D:微分

1.老版PID代码

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

 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;

 

mwc飞控PID算法解析

标签:

原文地址:http://www.cnblogs.com/beenoisy/p/4570568.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!