多旋翼通用避障开源项目2015-10-21号更新
飞控开发近期有人转载本文以及附带程序，在此本人发表一下声明：
1：程序每周都在进行优化修改，本人不承担任何使用该程序造成的任何损失！所以请随时关注社区以及QQ群。
2：请转载者以负责的态度引用相关链接，保障作者权益，修改代码后鼓励开源代码。
看了http://playuav.com/article/234的帖子后，觉得可实现多旋翼的自主避障肯定是将来发展的方向，寻思还不如大家一起开发一个。playUAV作为一个开源社区，希望大家能一起开发这个项目。
多旋翼避障首先要确定避障系统的基本思路。
现如今市场充斥着各种飞控，原本想在开源飞控项目上进行二次开发，但感觉能力有限，又违背了通用的原则所以决定在接收机与飞控中间做一个中间层。
添加一个中间层后，所有飞控都能通用，从QQ飞控，到PX4，APM，都可以通吃。
这样就达到的通用的目的。
为什么可以达到通用的目的呢？
参考：http://www.arduino.cn/thread-11353-1-1.html的帖子，航模中输出信号大多数为PWM信号，其1000为最低点，1500为中立点，2000为最高点。
开发过程中选用arduino进行开发，其语法简单，各种库丰富，是进行开发的不二之选。
#include<Servo.h>;
#define AIL1 22 //AIL fuyi
#define ELE2 24 //ELE shengjiang
#define THR3 26 //THR youmen
#define RUD4 28 //RUD fangxiang
unsigned long INAIL;
unsigned long INELE;
unsigned long INTHR;
unsigned long INRUD;
int OUTAIL;
int OUTELE;
int OUTTHR;
int OUTRUD;
Servo AIL;
Servo ELE;
Servo THR;
Servo RUD;
void setup()
{
pinMode(AIL1,0);
pinMode(ELE2,0);
pinMode(THR3,0);
pinMode(RUD4,0);
AIL.attach(4);
ELE.attach(5);
THR.attach(6);
RUD.attach(7);
Serial.begin(9600);
}
void loop()
{
INAIL = pulseIn(AIL1, 1);
INELE = pulseIn(ELE2, 1);
INTHR = pulseIn(THR3, 1);
INRUD = pulseIn(RUD4, 1);
OUTAIL = map(INAIL,1010,2007,47,144);
OUTELE = map(INELE,1010,2007,47,144);
OUTTHR = map(INTHR,1010,2007,47,144);
OUTRUD = map(INRUD,1010,2007,47,144);
AIL.write(OUTAIL);
ELE.write(OUTELE);
THR.write(OUTTHR);
RUD.write(OUTRUD);
Serial.print("AIL=");
Serial.print(INAIL);
Serial.print(" ELE=");
Serial.print(INELE);
Serial.print(" THR=");
Serial.print(INTHR);
Serial.print(" RUD=");
Serial.print(INRUD);
delay(5);
}
到目前为止，中间层已经初步的建立，开始选用相关避障所需要的传感器。
这段代码主要用来读取接收机的PWM值，pluseIn（）函数可以读取指定端口的电平脉冲时间。其还调用了arduino 的舵机库的舵机函数来控制，有人看出了，OUTTHR = map(INTHR,1010,2007,47,144); 这是个缩放函数，让1010-2007缩放为47-144，这里有人会问，为什么是1010-2007，为什么是47-144，我再来和同学们讲解，刚刚在取样的时候我们发现arduino读取的PWM宽度在1000-2000之间，但是有些值是大于2000的，那么我取了一个更大的区间，来“装”PWM的区间，所以是1000-2007，但是为什么要让舵机输出47-144度呢。这里要用点小技巧，羊毛出在羊身上，没错我们让arduino自己给自己测脉冲时间，我们可以写一个很简单的代码，还记得刚刚那个pluseIn（）函数吗，我们让任意一个端口输出一个90度的舵机角度值，也就是xxxx.write(90); 然后把这个端口接在arduino其他任意一个数字端口上，用pluseIn（）去测这个90度的脉宽是多少，比如90度测出来脉冲时间是1500，大了，我们要找的是1000-2000范围的那个舵机角度，我们再来一个30度，或者170度，就这样把范围越缩越小，最后，我找出了他们之间的关系，47度的舵机输出，用pluseIn（）去测量刚好在1000左右，那么地、低位就找到了，同样高位在144度上 脉冲宽度时间是2000左右，那么接下来代码大家都看懂了吧？，这里的arduino就原封不动的把接收机的信号吃进去了，又吐出来给飞控，至此，arduino已经完全潜入飞机控制系统。
（如有疑问，请查阅连接帖子）
出于实用和成本方面的原因，通过本人的实际购买测试，最终选用了HC-SR04超声波，价格低廉，且测量范围与精度值复合要求。
接线方式，VCC、trig（控制端）、 echo（接收端）、 GND地线
1.使用电压：DC5V
2.静态电流：小于2mA
3：电平输出：高5V
4：电平输出：低0V
5：感应角度：不大于15度
6：探测距离：2cm-450cm
7:高精度：可达3mm
本产品使用方法：一个控制口发一个10US以上的高电平,就可以在接收口等待高电平输出.一有
输出就可以开定时器计时,当此口变为低电平时就可以读定时器的值,此时就为此次测距的时间,
方可算出距离.如此不断的周期测,就可以达到你移动测量的值了~~
模块工作原理：
(1)采用IO触发测距，给至少10us的高电平信号;
(2)模块自动发送8个40khz的方波，自动检测是否有信号返回；
(3)有信号返回，通过IO输出一高电平，高电平持续的时间就是
超声波从发射到返回的时间．测试距离=(高电平时间*声速(340M/S))/2
避障相关思路：
超声波测距程序：
一般我写的会发布到群里，本人写的实在不堪入目，哈哈。
2015-6-16
这周比较忙，周末会进行一次机上测试，想测试自己程序的可以联系我。
2015-6-20
今天进行了若干测试，排除了一些错误，可是发现没什么反应，有些抖动，但并没有避障趋势，下午仔细分析了一下，决定采用一种新的避障方式。
2015-6-24
2015-10-21
放出源码：
#include<Servo.h>//The Arduino Mega has an additional four: numbers 2 (pin 21), 3 (pin 20), 4 (pin 19), and 5 (pin 18).}
int ppm1 = 2;
int ppm2 = 3;
unsigned long rc1_PulseStartTicks, rc2_PulseStartTicks;
volatile int rc1_val, rc2_val;
int OUTAIL, OUTELE; //输出
////////////////////////////超声波值///////////////////
int distance1 = 300, distance2 = 300, distance3 = 300, distance4 = 300;
//////////////////////////超声波针脚定义///////////////
const int input1 = 22, input2 = 24, input3 = 30, input4 = 32;
const int output1 = 23, output2 = 25, output3 = 31, output4 = 33;
Servo AIL;
Servo ELE;
void setup()
{
// 电平变化即触发中断
attachInterrupt(0, rc1, CHANGE);
attachInterrupt(1, rc2, CHANGE);
////////////////////////////////
AIL.attach(8);
ELE.attach(9);
pinMode(ppm1, INPUT);
pinMode(ppm2, INPUT);
pinMode(input1, INPUT);
pinMode(output1, OUTPUT);
pinMode(input2, INPUT);
pinMode(output2, OUTPUT);
pinMode(input3, INPUT);
pinMode(output3, OUTPUT);
pinMode(input4, INPUT);
pinMode(output4, OUTPUT);
Serial.begin(9600);
}
void rc1()
{ // did the pin change to high or low?
if (digitalRead( ppm1 ) == HIGH)
rc1_PulseStartTicks = micros(); // store the current micros() value
else
rc1_val = micros() - rc1_PulseStartTicks;
}
void rc2()
{
// did the pin change to high or low?
if (digitalRead( ppm2 ) == HIGH)
rc2_PulseStartTicks = micros();
else
rc2_val = micros() - rc2_PulseStartTicks;
}
void loop() {
Sonar();//超声波函数
OUTAIL = map(rc1_val, 986, 1964, 1000, 2000);
OUTELE = map(rc2_val, 976, 1948, 1000, 2000);
AIL.writeMicroseconds(OUTAIL);
ELE.writeMicroseconds(OUTELE);
Print();//串口输出
}
void Sonar ()
{
//第一个超声波/前
digitalWrite(output1, LOW);
delayMicroseconds(2);
digitalWrite(output1, HIGH);
delayMicroseconds(10);
distance1 = pulseIn(input1, HIGH, 8000) / 58;
if (distance1 >= 140 || distance1 == 0)
{
distance1 = 140;
}
void Avoidance_B ();
//第二个超声波/后
digitalWrite(output2, LOW);
delayMicroseconds(2);
digitalWrite(output2, HIGH);
delayMicroseconds(10);
distance2 = pulseIn(input2, HIGH, 8000) / 58;
if (distance2 >= 140 || distance2 == 0)
{
distance2 = 140;
}
void Avoidance_F ();
//第三个超声波/左
digitalWrite(output3, LOW);
delayMicroseconds(2);
digitalWrite(output3, HIGH);
delayMicroseconds(10);
distance3 = pulseIn(input3, HIGH, 8000) / 58;
if (distance3 >= 140 || distance3 == 0)
{
distance3 = 140;
}
void Avoidance_R ();
//第四个超声波/右
digitalWrite(output4, LOW);
delayMicroseconds(2);
digitalWrite(output4, HIGH);
delayMicroseconds(10);
distance4 = pulseIn(input4, HIGH, 8000) / 58;
if (distance4 >= 140 || distance4 == 0)
{
distance4 = 140;
}
void Avoidance_L ();
}
////////////////////////函数封装//////////////////////////////////
void Avoidance_F ()
{
if (distance2 <= 130)
{ //前进,后有障碍物
ELE.writeMicroseconds(1600);
}
}
void Avoidance_B ()
{
if (distance1 <= 130)
{ //后退,前有障碍物
ELE.writeMicroseconds(1400);
}
}
void Avoidance_L ()
{
if (distance4 <= 130)
{ //左飞,右有障碍物
AIL.writeMicroseconds(1600);
}
}
void Avoidance_R ()
{
if (distance3 <= 130)
{ //右飞,左有障碍物
AIL.writeMicroseconds(1400);
}
}
void Print()
{
Serial.print("INAIL=");
Serial.print(rc1_val);
Serial.print(" INELE=");
Serial.print(rc2_val);
Serial.print(" OUTAIL=");
Serial.print(OUTAIL);
Serial.print(" OUTELE=");
Serial.print(OUTELE);
Serial.print(" distance1=");
Serial.print(distance1);
Serial.print(" distance2=");
Serial.print(distance2);
Serial.print(" distance3=");
Serial.print(distance3);
Serial.print(" distance4=");
Serial.print(distance4);
Serial.println();
delay(2);
串口调试的相关数据，加上滤波后数据可能更加平滑，这周试试，Time表示已经运行的时间（S）。
（待续，有兴趣的加456441242群，刚建立）
22 个评论
想法不错。不明白为什么和接收机有关。避障系统直接和飞控通信。
楼主思路很清晰~
我觉得最好是把超声波测距信号通过arduino直接转换成I2C串行信号。利用飞控（APM或PIX）强大的处理器来处理和控制飞行器，这样可以让飞行器更智能，更具备开发性。
去年10月份我也写过原理相同的方案，可惜不会编程，老师又嫌时间来不及，是为了去比赛的
只关油门不踩刹车有什么用？你开车不踩刹车？
lihuachunray 回复 Apis
这就需要通过飞控进行修正。
很不错。。我也想到过这点，挺好的。。中间层的加入可以拓展多方位的传感器（超声等），接收机--中间层---飞控的做法很高端啊。。。极大的拉近了普通飞控和高端飞控的距离
很不错 支持下~
51单片机行吗？我还有4个SR04，一个51最小系统，有避障小车程序，怎么让我的PIXHAWK4轴实现避障
不错，很给力，直接开发成模块给大家吧。
本人需要APM输出口读取解析出以下参数：飞行模式状态（自稳、gps定点定高、自驾）、三轴坐标、三轴姿态、水平速度、垂直速度、垂直向下加速度（加电）起飞点坐标或与起飞点之间的距离；读取频率5-10赫兹； (说明:飞行模式状态（自稳、gps定点定高、自驾）也可从CH5 或CH8读取)，哪位大侠能帮助解决？可以谈费用。急需解决！有意请联系！电话：0562- 5820742 18956206693 QQ: 426544944
我也在做，方案差不多
pulseIn()这个函数读取的时候会把程序暂停在哪个位置的，所以最好判断下这个引脚有没有接接收机，没有的话直接跳过读取脉冲，我之前做伞降器控制器遇到这个问题了。
