【DIY】 超声波定高功能，解放你的油门，让你飞得很无聊【41楼更新爬楼梯视频】

auuuux2011

最近飞四轴让我很是痛苦，因为我用飞固定翼的方式来控制油门，四轴就像兔子一样上窜上跳。等XAircraft 出超声波功能等了很久也没出来，还是自己动手写一个通用的超声波模块吧，估计不会很难。

想了一下需求大概是这样的 ：

1. 可以由单片机完全控制油门， 我只控制四轴的水平运动就行。
2. 可以用控的一个2段开关，来开启和关闭超声波定高， 开启后， 四轴将悬停在当前的高度。
3. 用另外一个开关，能自动起飞， 自动降落。
4. 能兼容大部分的四轴飞控(FS1212，KK，AQ50D之类)，因为接管了接收机的油门通道

在开始之前我设想了一些问题，后来在实际测试中进行了验证和修正。

1.超声波是否会被电机干扰？
实际测试后，超声波并不会被电机发出的声音影响。但很容易被紧贴在旁边的电池反射产生了干扰，所以超声波传感器得找一个周围至少有2-3cm间隔的地方放置。尽管如此，超声波在临界点时返回的数据一定会有噪点，我们得用kalman 过滤一次。

2.pid 如何实时查看output 。
方案一，用电脑上的 processing写一个波形显示程序。方案二，用anroid 写一个手机上的示波器，android sdk2以上可以使用蓝牙SDK， 再用一片20元的蓝牙芯片放在飞控上，就可以让飞控和手机通讯了。

3. PID 中比例，微分，积分的整定。
这三个参数需要在外场实际测试中调试，通讯链路是双向的，自订个协议设置参数没多大问题。遇到的困难是，在一些极端参数下，估计会产生巨大的不可控震荡，简单的说四轴会拔地而起，或者重重的高速撞击地球。暂时没想到好的办法防止这些情况，用绳子拴着？

4.pid output 用什么策略映射到 pwm
方案一是直接将输出映射到1000-2000 的脉宽，这样手动的油门将不参与控制
方案二，基于任何时候都不能相信机器人的理由，要将当前的油门值进行数据融合

5.开启pid 的策略
用遥控器的一个2档开关，开关拨到开启时，记录这瞬间的pwm和高度，作为setpoint。

6.超声波的异常处理，距离检测到 0 ，或很大的情况，距离突然变化的情况。
超声波必须要做卡尔曼滤波，在这些特殊情况下应该关闭定高功能，自动缓慢降落。

[ 本帖最后由 auuuux2011 于 2011-5-16 12:54 编辑 ]

auuuux2011

算法早已写完，连续两个星期都没有时间去外场实际验证效果， kp ，ki ，kd 在家基本没法整定。
今天虽然有些小雨，也去了华农体育场，带了手提电脑和无线串口，直接用无线来修正单片机上的参数，大概调整了10来组，就已经很好飞了。我的控是美国手，四轴是x450，最后我的左手完全解放了，只用右手控制四轴前后左右就行了。帮我一起调整参数的朋友cn，从来没有飞过航模，开启锁定航向+超声波定高，让他飞了十来分钟下来说：太没挑战了，太无聊了。话没说完就倾盆暴雨，只好收工回家，改日再战。





经过实际外场测试，整定出PID 参数如下， 震荡已经很小了。

#define IMU_SAMPLE_RATE 1.0/16 // 16k Hz
Kalman kalman_ym(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001  ); //油门卡尔曼
Kalman kalman_dt(IMU_SAMPLE_RATE, 0.002 , 0.003 , 0.001  ); //高度卡尔曼
int pinCSBin = 10;
int pinCSBout = 11;
//Define Variables we'll be connecting to
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint, 2  ,  0.5 ,  1 , DIRECT); // 最后整定出来的kp,ki,kd



这是运行在电脑上，用无线进行调整pid参数，这样就不用重新编译和上传程序到单片机了。
绿色线是目标高度
红色线是当前高度
蓝色是经过pid运算后修正的油门值

[ 本帖最后由 auuuux2011 于 2011-5-8 00:25 编辑 ]

auuuux2011

再上一个高清视频，几乎整个过程都是cpu在推油。



接下来，上制作教程： 程序也分享给大家，你可以随意下 载烧录。

一 。首先你得去taobao 以下器件：
1. arduino uno/nano/mini/pro 都可以。（我一开始用了一片stm32f103RB，但这开发板挺贵而且没有电子基础的人upload都很困难，觉得没必要了，换成最容易买到的avr）
2.超声波传感器 （一搜一大把，几十块钱吧，我用的是5米距离的）


二。然后将arduino 的pin 2  接到 接收机的 3通道（油门）， 将pin3 接到接收机的5通道 （2段开关） 。 将arduino 的pin4 （油门输出） 接到飞控的1通道 （油门输入）

三。下 载程序，用usb烧录到芯片里去。（程序到Q群74639945共享里下 载）


四。有必要的话调整一下参数，不调就用我设定好的。


然后，然后，打开care free+ 超声波定高，你会觉得飞四轴变得很无聊。

[ 本帖最后由 auuuux2011 于 2011-5-8 09:34 编辑 ]

dstdx72

:em04: 这个强，视频呢？

dstdx72

:em22: 神马CPU？卡尔曼，算得过来不？PID速率？

兔哥

KK上可以用吗?

ZDB5873

好!

auuuux2011

to: dstdx72
芯片是atmel avr 168 ，16khz 。 中断采样， 做了2次卡尔曼，1次 闭环pid  ， 从效果看肯定比人脑+眼睛算得快

to：兔哥
理论上兼容所有四轴飞控，包括KK ，因为cpu只是根据高度帮你控制油门，不涉及飞控本身。

zhen_sr

顶了——楼主！
好样的！
能不能出个详细的教程？我们照样学来？

兔哥

原帖由 auuuux2011 于 2011-5-8 09:26 发表

to：兔哥
理论上兼容所有四轴飞控，包括KK ，因为cpu只是根据高度帮你控制油门，不涉 ...

什么时候出成品或套材吗?

我搞一套

dpcom

能飞个航线来看看吗

Nokia82

:em26: :em15:

code_asf

这个应该加精。

mapgps

> "一个2段开关，来开启和关闭超声波定高"
这样是否更方便:
当油门杆回正后, 就自动触发超声波定高. 省了那个2段开关.
也就是说, 用油门杆位置来作为速度的大小: 这样就能控制: 匀速上升(正速度) / 定高悬停(零速度) / 匀速下降(负速度).

[ 本帖最后由 mapgps 于 2011-5-8 15:15 编辑 ]

yyqop01

楼主厉害，我是那天的美国手200，很敬佩DIYer，:em26: :em26:

531475118

:em15:

yyqop01

原帖由 dong3x 于 2011-5-8 15:07 发表
让他飞了十来分钟下来说：太没挑战了，太无聊了。话没说完就倾盆暴雨，只好收工回家，改日再战。

你的DIY，人神共愤啊，小心呀。

那天很大雨

longway7721

:em26: 强人，上详细流程吧，造福广大模友，感觉可以用于直机了？

咖啡猪

太强悍了！

auuuux2011

原帖由 mapgps 于 2011-5-8 15:09 发表
> "一个2段开关，来开启和关闭超声波定高"
这样是否更方便:
当油门杆回正后, 就自动触发超声波定高. 省了那个2段开关.
也就是说, 用油门杆位置来作为速度的大小: 这样就能控制: 匀速上升(正速度) / 定高悬停(零速 ...

我也在头疼自动起飞，自动降落，定高怎么做切换，用3段开关都不够。
油门要参与数据融合的，不能拿来做开关。