MWC飞控的实现及代码分析

飞控

近几年，四轴飞行器正在逐渐走入大家的视野；由于四轴飞行器稳定性好，飞行平稳，被广泛应用于航拍中。
四轴飞行器的核心在于其飞控，飞控之于飞行器就相当于CPU之于电脑，它可以实时监测飞行数据，并通过一系列程序实现各种功能，如纠正飞行姿态、定高飞行、GPS定位等；

MWC飞控介绍

MWC是Multiwii Copter的缩写，Multiwii是是控制多旋翼航模的开源固件。Multiwii最初由法国航模爱好者Alex创立，用于控制他自己做的三轴飞行器。后经全世界各大高手改造扩展，现在Multiwii不仅仅支持三轴飞行器、四轴、六轴等多轴系列，还支持固定翼和直升机等航模。功能强大而且简单易用。Multiwii支持的传感器类型极为丰富，而且支持十分广泛的外部设备和飞行模式，这让Multiwii飞控固件发展为国外最受欢迎的多轴开源固件之一。 Multiwii官网 http://www.multiwii.com/

一步一步搭建基于MWC的小四轴平台

四轴飞行器由以下主要部分组成：飞控+机架+电机（桨）+电调+电池+遥控器
一般的大四轴由于电池和遥控器要求较高，一般价格都要上千，本实验将采用一些最核心的部件去搭建一个小四轴演示平台，利用手机和蓝牙模块作为遥控器和接收器，不仅可以展示出四轴飞行器最核心的原理，并且仅需花费约100元。

硬件

飞控部分：Arduino UNO(单片机） + MPU-6050模块（三轴陀螺仪传感器）（10元）
机架：TB买了一个30元的机架；
电机:720空心杯马达 + 55mm螺旋桨 * 4（25元）
电调：N型mos管*4（我买的是IRF640B，3元10个）
电池：350mAH 3.7V锂电池 + 自配USB充电器（20元）
遥控器 + 接收器：一部安卓手机 + HM-11蓝牙模块（30元）

注：为了更好展示细节和调试，本次实验中均采用了较大的模块，因此得到的成品没有飞行能力；一般成熟的MWC飞控均采用PCB板将单片机和传感器高度集合。

电路连接

第一步 刷入固件
第二步 连接Arduino和MPU-6050

将MPU-6050模块上的SCL和SDA分别接在UNO板的A5和A4上，并接好电源。
接下来检验并且校正传感器，将Arduino连接到电脑上，打开MuitiWiiConf软件，点击右上方的串口，并且点击START，接下来就可以在GUI软件中查看到传感器模块的信号了！^_^
然后是校正传感器，将传感器放置在水平位置，再点击CALIB_ACC，等待几秒，即初始化传感器。
详情可见http://blog.sina.com.cn/s/blog_8104d1d00100sw3t.html

第三步 蓝牙模块

蓝牙模块比较小，因为要修改波特率，因此调试会稍显麻烦； 1、首先蓝牙模块会用到4个口，2号口——TX，4号口——RX，9号口——VCC，12号口——GND；分别将这四个口拿导线焊上
2、连接Arduino前首先要修改蓝牙模块的波特率，否则之后无法和手机APP通信；和串口相连接，蓝牙模块的TX对应串口的RX，同理RX与TX连接，另外两个连接串口电源
3、打开串口调试助手，波特率设置在9600，发送AT+BAUD4，看到返回OK:SET4就设置成功了
4、之后再将蓝牙模块和Arduino相连接，和之前连接串口一样，将TX对应UNO的RX（0号），RX对应UNO的TX（1号），电源也接上；
5、接下来打开手机APP，点击蓝牙连接，选择HM-SOFT，再进入飞行控制，如果此时看到了Arduino上TX和RX两个灯在不停闪烁，说明已经接收到信号啦！^_^
6、保持油门处在最低位置，按住锁定，将手机抬起90度，等待UNO板上的LED灯闪烁一下熄灭，放平手机，加速度计校准完毕；然后将油门调到最大值，按住锁定，将手机抬起90度，等待飞机上的灯闪烁一下熄灭，放平手机，陀螺仪计校准完毕。
7、长按解锁，如果LED灯亮起，说明就可以开始操控了！蓝牙模块设置OK！

第四步 电机连接

D3——左前电机; D10——右前电机
D9——左后电机; D11——右后电机
当然，不能直接把D3直接接到电机上去，我们需要一个MOS管，连接如下;
G——arduino输出端Dx； S——电机正极； D——接地； 电极负极接电源正极；
打开APP，按之前的方法重新连接，即可控制电极旋转！

融合算法

加速度传感器：
优点：无累积误差，长时间稳定;缺点：1、对振动非常敏感，电机的转动和位置的转换会使加速度传感器产生很大的噪声；2、加速度计运动时，输出量为运动加速度和重力加速度的混合数据
陀螺仪：
优点：数据噪声小，短时间内误差小；缺点：1、陀螺仪及放大电路有温漂；2、为了得到角度，必然会对角速度进行积分，而积分会产生累计误差，随着时间的推移，这种误差会越来越大
因此，就需要将两者数据进行融合处理，通过取寸之所长来补尺之所短，从而获得比较合适的角度信息，这是进行控制算法的前提。比较常用而有效的滤波算法是卡尔曼滤波算法。

软件

MWC飞控代码：mwc飞控代码.rar
GUI配置程序：配置程序.rar
安卓遥控程序：安卓遥控器.rar

MWC代码解析

MWC PID部分代码

PITCH & ROLL & YAW PID int16_t prop; 

prop = min(max(abs(rcCommand[PITCH]),abs(rcCommand[ROLL])),500); // range [0;500]
for(axis=0;axis<3;axis++) {
  if ((f.ANGLE_MODE || f.HORIZON_MODE) && axis<2 ) { // MODE relying on ACC
    // 50 degrees max inclination
    errorAngle = constrain((rcCommand[axis]<<1) + GPS_angle[axis],-500,+500) + conf.angleTrim[axis] - angle[axis]; 
    //误差角度 = 设定角度 - 实际角度 =（遥控角度 + GPS角度 + 设置倾角）- 实际角度
    //另外，遥控角度和GPS角度被限制在500内，我认为主要原因在于防止由于遥控操作幅度较大时计算出来的误差角也很大，从而使得PID修正过强，导致飞行不稳定；
    PTermACC = ((int32_t)errorAngle*conf.P8[PIDLEVEL])>>7;
    // P项 = 误差角 * 设置的P参数 / 128
    PTermACC = constrain(PTermACC,-conf.D8[PIDLEVEL]*5,+conf.D8[PIDLEVEL]*5);
    // P项需限制在D参数的5倍之内,这是数值的差异
    errorAngleI[axis] = constrain(errorAngleI[axis]+errorAngle,-10000,+10000);
    // WindUp     //16 bits is ok here
    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
  }
  if ( !f.ANGLE_MODE || f.HORIZON_MODE || axis == 2 ) { // MODE relying on GYRO or YAW axis
    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)
                             else error = ((int32_t)rcCommand[axis]<<6)/conf.P8[axis] ; // 32 bits is needed for calculation
    error -= gyroData[axis];
    PTermGYRO = rcCommand[axis];    
    errorGyroI[axis]  = constrain(errorGyroI[axis]+error,-16000,+16000);         // WindUp   16 bits is ok here
    if (abs(gyroData[axis])>640) errorGyroI[axis] = 0;
    ITermGYRO = ((errorGyroI[axis]>>7)*conf.I8[axis])>>6;                        // 16 bits is ok here 16000/125 = 128 ; 128*250 = 32000
  }
  if ( f.HORIZON_MODE && axis<2) {
    PTerm = ((int32_t)PTermACC*(512-prop) + (int32_t)PTermGYRO*prop)>>9;         // the real factor should be 500, but 512 is ok
    ITerm = ((int32_t)ITermACC*(512-prop) + (int32_t)ITermGYRO*prop)>>9;
  } else {
    if ( f.ANGLE_MODE && axis<2) {
      PTerm = PTermACC;
      ITerm = ITermACC;
    } else {
      PTerm = PTermGYRO;
      ITerm = ITermGYRO;
    }
  }
  PTerm -= ((int32_t)gyroData[axis]*dynP8[axis])>>6; // 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];
  deltaSum       = delta1[axis]+delta2[axis]+delta;
  delta2[axis]   = delta1[axis];
  delta1[axis]   = delta;
  DTerm = ((int32_t)deltaSum*dynD8[axis])>>5;        // 32 bits is needed for calculation                      
  axisPID[axis] =  PTerm + ITerm - DTerm;
}
mixTable();
writeServos();
writeMotors();

}

MWC油门映射代码解读

void annexCode() { 

uint16_t tmp,tmp2;
uint8_t axis,prop1,prop2;
//定义几个变量
#define BREAKPOINT 1500
//定义零界点1500
if (rcData[THROTTLE]<BREAKPOINT) {
  prop2 = 100;
} else {
  if (rcData[THROTTLE]<2000) {
    prop2 = 100 - (uint16_t)conf.dynThrPID*(rcData[THROTTLE]-BREAKPOINT)/(2000-BREAKPOINT);
  } else {
    prop2 = 100 - conf.dynThrPID;
  }
}
//如果油门值x小于1500，prop2 = 100
//如果油门值x大于1500小于2000，prop2 = 100 -  conf.dynThrPID * (x-1500)/500   注：dynThrPID四轴默认0，可以GUI设置，dynThrPIDprop的值越小，即油门大小对于PID影响越大。
//首先初始值给定100，之后对遥控器油门进行判断，根据遥控器油门的大小对prop2进行处理。说到代码有趣，是因为conf.dynThrPID的出现。原来动态PID队友们的调节是在这里啊！
for(axis=0;axis<3;axis++) {
  tmp = min(abs(rcData[axis]-MIDRC),500);
  //3个轴上的大小减去中心值即偏差
  #if defined(DEADBAND)
    if (tmp>DEADBAND) { tmp -= DEADBAND; }
    else { tmp=0; }
  #endif
  //如果定义了死区，若偏差小于死区大小，偏差就为0
  if(axis!=2) { //ROLL & PITCH
    tmp2 = tmp/100;
    rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp-tmp2*100) * (lookupPitchRollRC[tmp2+1]-lookupPitchRollRC[tmp2]) / 100;
    prop1 = 100-(uint16_t)conf.rollPitchRate*tmp/500;
    prop1 = (uint16_t)prop1*prop2/100;
  } 
    //axis不等于2，即ROLL&PITCH
    //rcCommand（遥控指令）= 
    else {      // YAW
    rcCommand[axis] = tmp;
    prop1 = 100-(uint16_t)conf.yawRate*tmp/500;
  }
  dynP8[axis] = (uint16_t)conf.P8[axis]*prop1/100;
  dynD8[axis] = (uint16_t)conf.D8[axis]*prop1/100;
  if (rcData[axis]<MIDRC) rcCommand[axis] = -rcCommand[axis];
}
tmp = constrain(rcData[THROTTLE],MINCHECK,2000);
tmp = (uint32_t)(tmp-MINCHECK)*1000/(2000-MINCHECK); // [MINCHECK;2000] -> [0;1000]
tmp2 = tmp/100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp-tmp2*100) * (lookupThrottleRC[tmp2+1]-lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [conf.minthrottle;MAXTHROTTLE]
  • home/whyx/proj/fly_ctrl.txt
  • 最后更改: 2015/06/24 23:26
  • (外部编辑)