GD32F103+MPU9150四轴飞行器第二步：PID<内含试飞视频完整代码>

只看该作者 · 2014-4-20 22:29

本帖最后由 zkcaptain 于 2014-6-18 10:31 编辑

第一步姿态融合见帖子：https://bbs.21ic.com/icview-605405-1-2.html，以后的更新也只在这个帖子里更。
--------------------------------------------------------------------------------------------------------------------------------------
2014年6月16日更新见主楼位。
--------------------------------------------------------------------------------------------------------------------------------------
今天本来也没做好起飞的准备，不过感觉老是在屋里调有些东西是观察不到的，因此就冒险去户外飞了一下。果然。。。。。。坠机了，原因是其中一个非自紧螺旋桨的螺丝松了，稍后上图，之前也松过一次，没处理好，这次得到教训了。这一摔就是400块钱没了，维修也得1到2个星期。

目前的功能：
1.开启传感器低通滤波98Hz，滤掉电机震动对加速度计和陀螺仪的影响。
2.磁力计现场校准，代码直接完成，不需要PC干预。换个地方飞不用再带个电脑做校准了。
3.陀螺仪零偏自校准
4.加速度计零偏和灵敏度校准
4.代码时间片管理，姿态融合500Hz，PID控制200Hz
5.位置式PID控制，控制参数采用kp，Ti，Td，从今天飞行状况来看，此功能日后需大力改进
6.固定航向模式。日后加入可控航向。
7.串口在线调试，支持在线参数保存和读取，采用GD32F103内部Flash来保存参数，不用外扩存储芯片，也方便调试PID参数和磁力计校准。
8.飞行模式：x模式

先上图：
下面这张是自己设计的飞控板第二版，兼容F10x/F2xx/F4xx系列MCU，正面：外扩4个PWMin外接遥控接收机，4个PWMout外接motor，4个PWM备用，1个DCMI摄像头接口，1个USART1接蓝牙模块，1个SPI可接SPI nand。

这张是反面：外接TF卡用于存储视频

下面这张是整机：两个白色轴中间是机头，右边白色轴上是之前做的蓝牙模块，整机标配：450机架，980kv电机，好盈20A电调，11.1V-3S动力电池，7通道接收机，锂电池低压报警器，桨1047

下面这张是调PID的：此时yaw pitch和roll的PID都调的差不多了，已经很稳了

下面这张是坠机的原因：螺丝松了，轴也摔弯了

下面这张是飞控板被摔后的，万幸可以修复：

飞行截图：

最后上坠机视频文件，mp4格式，今天飞了3次，这次是失败的例子，就放上来了，目前的情况是飞的还算稳定，不过基本都是它玩我，而我不敢玩它，桨的螺丝松了也是个大的教训，一定要用桨夹：

坠机视频.rar (2.12 MB)

--------------------------------------------------------------------------------------------------------------------------------------------
2014年6月16日更新：
声明在前：以下更新只针对新手，为了让新手对整个四轴有个大致全面的认识，现将整个代码放上来，仅供参考，代码的PID部分是标准的PID，可以飞，但是操控性等于0，或者说需要很小心很有经验的去操控，不要拿来移植完了就用，后果就是去淘宝再来500块钱的材料。原因是D项是对角度进行微分，操控起来四轴的动作太猛，之所以先这么做，也是为了验证大家的说法和增加自己的体验，一般把D项改为对角速度的微分，手感就好很多了。

下面说一些乱七八糟的东西，但是个人感觉很有用的东西，包括PID、整机等等。

PID分两种位置式和增量式，我采用的是位置式，将Ki和Kd用Ti和Td的形式来表达，并带入到式2-3中，ek=角度的期望-传感器输出的角度，所用的PID三个参数是：Kp、Ti、Td。公式如图所示：

四轴PID控制的目的就是将接收到的遥控的控制信号（一般有油门THR、升降舵ELE（对应pitch）、副翼Ail（对应roll）、方向舵RUD（对应yaw））与飞控板本身计算得出pitch、roll、yaw做比较，分别得出它们之间的误差值，然后将Kp、Ti、Td与这个误差值结合得出PID输出（PID_pitch、PID_roll或者PID_yaw），再将这三个PID输出与油门结合在一起算出送到每一个电调的PWM数值，从而控制每一个电机的转动。

那么我们就需要考虑三个问题，这三个问题都是围绕《让自己的四轴怎样转动》：

1. 飞行时是使用“+”模式还是使用“x”模式

2. 四个电机分别该如何旋转，是逆时针还是顺时针

3. 每个电机该怎样旋转才能让我们的四轴上升、下降、左翻、右翻、上仰、俯冲、左转头或者右转头

对于第一个问题来说：

一般“+”模式比较好操作，也比较好调节PID的那三个参数，而“x”模式飞行起来比较灵活，可以做很多复杂动作，但是比较难操作，这里我用的是“x”模式，对于新手来说，包括我，虽然“x”模式难一些，不过最多也就是需要时间来学习和熟练罢了，其实也没什么难的。

对于第二个问题：

首先要确定飞控板的xyz轴正方向如何放置，然后确定4个电机的旋转方向分别是什么，我定义的方式如下图，Y正方向为四轴前进方向：

M1、M2、M3、M4分别为4个电机，其中M1和M4顺时针旋转，M2和M3逆时针旋转。

对于第三个问题：

还是看上图，我定义三点：

一．xyz轴的正方向为：y指向正北，x指向东，z指向天；

二．绕y旋转是roll，绕x旋转是pitch，绕z旋转是yaw；

三．Yaw北偏西为正，pitch往上为正，roll“右翼”下沉为正。

令：“+”表示增加该电机的旋转速度，“-”表示减小该电机的旋转速度，那么“x”模式下的控制规律如下：

最后得出每个电机需要的PWM输出计算公式：

其中Limit_PWMOUT函数是限制PWM的输出，根据电调的PWM占空比来确定。

还有一点需要注意的，就是桨叶的安装，桨叶是分正反浆的，通俗的说，安装时要保证4个桨叶转动时都是往下吹风的。

四轴零件之间的接线与简单说明：
4个电调的正负极需要并联（红色连一起，黑色连1一起），并接到电池的正负极上；
电调3根黑色的电机控制线，连接电机，交换任意两根黑色的连线，可改变电机转动方向，以此来改变桨的旋转方向；
电调有个BEC输出，共3根线，红、黑线用于输出5v的电压，给飞行控制板供电，另外一根线用于接收飞行控制板的PWMout信号；
遥控接收器连接在飞行控制器的PWMin上，输出遥控信号给飞控板，并同时从飞行控制板上得到5v供电；

关于PWMin和PWMout：

对于我的天地飞7通遥控+WFT07接收机来说，遥控发送给接收机的信号是PPM，而接收机输出的信号是标准的PWM。

对于接收机来说，我们只需要关心它输出的高电平时间，也就是PWMin，不用管占空比，因此TIM_Period设定为 0xFFFF。我的WFT07接收机的PWM周期是20ms，高电平是1ms~2ms。

对于给电调的输入来说，也就是PWMout，要确定其占空比，一般根据自己的电调来确定这个占空比，我用的是好盈天行者20A，频率为50~432hz，所以我的PWM周期设定为2.5ms 频率400hz，高电平持续时间限制在0.875ms~2.0ms，高电平一般在周期的20%~80%。

如何将遥控的PWMin转化为期望的角度expect：

先看一个公式：e(t)=expect – measured，这是PID中的误差e(t)，等于遥控期望值减去传感器的测量值。

而将遥控的PWMin转化为期望的角度expect所达到的目的就是将PWMin转化为expect，使expect的范围大小(不是单位，PWMin的单位可以看成已经是角度单位了)与measured的对应起来，并限制expect的范围，对新手来说好操控。

至于转化，就是将PWMin通过一些加减乘除限定在一定范围内，比如我将PWMin（Motor_Ail、Motor_Ele、Motor_Rud）限定在-25度到+25度内，而Motor_Thr保持原来的范围，暂不做限定，可根据具体操作手感来做限定，如下图，其中AilMiddle、EleMiddle、RudMiddle均为1500：

关于对PWMin的滤波：

TIM2_ICInitStructure.TIM_Channel =TIM_Channel_4;

TIM2_ICInitStructure.TIM_ICPolarity= TIM_ICPolarity_Rising; //先上升沿捕获

TIM2_ICInitStructure.TIM_ICSelection= TIM_ICSelection_DirectTI; //映射到TI1上

TIM2_ICInitStructure.TIM_ICPrescaler= TIM_ICPSC_DIV1; //配置输入分频,不分频

TIM2_ICInitStructure.TIM_ICFilter = 0x0B;//IC1F=1010滤掉1us以下脉冲宽度的干扰，避免意外进入中断6/(108Mhz/16)=0.9us

TIM_ICInit(TIM2,&TIM2_ICInitStructure);

将TIM_ICFilter设置为0x0B，滤掉1us以下脉冲干扰，防止误进PWMin中断，避免将干扰信号当作PWMin来接收。

下面上代码：

GD32F103VC-AHRS.rar (640.55 KB)

代码是针对Gigadevice公司的GD32F103VCT6这款芯片的，系统时钟108M，修改完系统时钟后可直接移植到STM32F103VCT6上，代码是在《[小应用]四轴飞行器第一步》那个帖子的基础上修改了很多东西，也添加了很多东西，并且代码都是经过我自己验证过的，而且已经能飞了的。
在测试过程中如果发现代码有问题或纰漏，请指出。