查看: 30219|回复: 117
收起左侧

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

[复制链接]
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-4-20 22:29 | 显示全部楼层 |返回版面|阅读模式
本帖最后由 zkcaptain 于 2014-6-18 10:31 编辑

第一步姿态融合见帖子:http://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次,这次是失败的例子,就放上来了,目前的情况是飞的还算稳定,不过基本都是它玩我,而我不敢玩它,桨的螺丝松了也是个大的教训,一定要用桨夹:


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

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

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

四轴PID控制的目的就是将接收到的遥控的控制信号(一般有油门THR、升降舵ELE(对应pitch)、副翼Ail(对应roll)、方向舵RUD(对应yaw))与飞控板本身计算得出pitchrollyaw做比较,分别得出它们之间的误差值,然后将KpTiTd与这个误差值结合得出PID输出(PID_pitchPID_roll或者PID_yaw),再将这三个PID输出与油门结合在一起算出送到每一个电调的PWM数值,从而控制每一个电机的转动。
那么我们就需要考虑三个问题,这三个问题都是围绕《让自己的四轴怎样转动》:
1.       飞行时是使用“+”模式还是使用“x”模式
2.       四个电机分别该如何旋转,是逆时针还是顺时针
3.       每个电机该怎样旋转才能让我们的四轴上升、下降、左翻、右翻、上仰、俯冲、左转头或者右转头
对于第一个问题来说:
一般“+”模式比较好操作,也比较好调节PID的那三个参数,而“x”模式飞行起来比较灵活,可以做很多复杂动作,但是比较难操作,这里我用的是“x”模式,对于新手来说,包括我,虽然“x”模式难一些,不过最多也就是需要时间来学习和熟练罢了,其实也没什么难的。
对于第二个问题:
首先要确定飞控板的xyz轴正方向如何放置,然后确定4个电机的旋转方向分别是什么,我定义的方式如下图,Y正方向为四轴前进方向:


M1M2M3M4分别为4个电机,其中M1M4顺时针旋转,M2M3逆时针旋转。

对于第三个问题:
还是看上图,我定义三点:
一.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供电;
关于PWMinPWMout
对于我的天地飞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通过一些加减乘除限定在一定范围内,比如我将PWMinMotor_AilMotor_EleMotor_Rud)限定在-25度到+25度内,而Motor_Thr保持原来的范围,暂不做限定,可根据具体操作手感来做限定,如下图,其中AilMiddleEleMiddleRudMiddle均为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来接收。

下面上代码:


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


本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册 手机登录

x

评分

参与人数 5威望 +23 收起 理由
一代掌门 + 3 很给力!
摩天轮1111 + 2 鼓励一下
猫小喵2010 + 1 很给力!
大秦正声 + 5 很给力!
21小跑堂 + 12 赞一个!

查看全部评分

     

47

主题

2547

帖子

7739

积分

高级工程师

发表于 2014-4-20 22:36 | 显示全部楼层 |返回版面
支持LZ....
我最讨厌两种人:一是有种族歧视的; 二是黑人;三是不识数的!

3

主题

51

帖子

169

积分

中级技术员

发表于 2014-4-21 08:51 | 显示全部楼层 |返回版面
作为失败的范例,楼猪实在是太成功了。

615

主题

1326

帖子

4286

积分

禁止访问

发表于 2014-4-21 08:57 | 显示全部楼层 |返回版面
不错不错,赞lz
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-4-21 09:29 | 显示全部楼层 |返回版面
cdshkf 发表于 2014-4-21 08:51
作为失败的范例,楼猪实在是太成功了。

哈哈,好评!
     

366

主题

3544

帖子

1万

积分

资深工程师

发表于 2014-4-21 12:23 | 显示全部楼层 |返回版面
这个需要技术和金钱!
大秦正声电子之家
http://yang96381.blog.163.com
提供蓝牙、zigbee芯片批量生产烧录服务:1带3脱机烧录工具,支持cc2540/cc2541,cc2530/cc2531,最新支持nrf51822,cc2650,cc2640,cc2630
     

20

主题

413

帖子

1255

积分

助理工程师

发表于 2014-4-25 11:16 | 显示全部楼层 |返回版面
不错严重支持
景观亮化、自动化控制方案。 有大型亮化工程多年现场经验

268

主题

4549

帖子

1万

积分

资深工程师

发表于 2014-4-25 23:40 | 显示全部楼层 |返回版面
好厉害呢

268

主题

4549

帖子

1万

积分

资深工程师

发表于 2014-4-25 23:41 | 显示全部楼层 |返回版面
楼主准备开源吗

268

主题

4549

帖子

1万

积分

资深工程师

发表于 2014-4-25 23:42 | 显示全部楼层 |返回版面
想学习一下

0

主题

3

帖子

9

积分

实习生

发表于 2014-5-7 11:10 | 显示全部楼层 |返回版面
好帖子,继续做
     

54

主题

722

帖子

2437

积分

初级工程师

发表于 2014-5-8 12:12 | 显示全部楼层 |返回版面
天宏科技 专业 专注温控工控自动化 QQ 496542965
     

64

主题

255

帖子

340

积分

禁止发言

 楼主| 发表于 2014-6-16 11:11 | 显示全部楼层 |返回版面
2014年6月16日楼主位更新代码和部分心得、说明等

1

主题

13

帖子

39

积分

实习生

发表于 2014-6-16 16:33 | 显示全部楼层 |返回版面
感觉做的不错呀。。请问楼主遥控器那一块怎么做的?

0

主题

5

帖子

15

积分

实习生

发表于 2014-6-16 23:18 | 显示全部楼层 |返回版面
敢于尝试就是成功,我也在做一个运动控制的东西,也失败了很多次
     

6

主题

1159

帖子

3540

积分

中级工程师

发表于 2014-6-17 00:31 | 显示全部楼层 |返回版面
感谢分享!

0

主题

9

帖子

27

积分

实习生

发表于 2014-6-18 09:30 | 显示全部楼层 |返回版面
收藏,楼主有心了
     

0

主题

1

帖子

13

积分

实习生

发表于 2014-6-18 10:19 | 显示全部楼层 |返回版面
楼主好强大

0

主题

3

帖子

9

积分

实习生

发表于 2014-6-18 10:48 | 显示全部楼层 |返回版面
高科技

1

主题

63

帖子

189

积分

中级技术员

发表于 2014-6-18 16:02 | 显示全部楼层 |返回版面
支持楼主
您需要登录后才可以回帖 登录 | 注册 手机登录

本版积分规则

分享 快速回复 返回顶部 返回列表