mwc mega 超声波定高

abccbef

超声波模块支持GPIO和串口两种模式，一开始用GPIO模式

1.   digitalWrite(TrigPin,HIGH);
   
2.   delayMicroseconds(50);
   
3.   digitalWrite(TrigPin,LOW);
   
4. 
   
5.   Time_Echo_us = pulseIn(EchoPin,HIGH,40000);

复制代码

结果发现在没有收到超声波信号的时候puseIn方法延时非常厉害，严重影响了其他传感器的测量，后来改用串口模式

1. unsigned int hl = 0;
   
2. unsigned int ll = 0;
   
3. unsigned int Len_mm = 0;
   
4. 
   
5. static uint32_t currentTime = 0;
   
6. static uint32_t rcTime  = 0;
   
7. static uint16_t previousTime = 0;
   
8. static uint16_t cycleTime = 0;
   
9. 
   
10. void setup(){
    
11.   Serial.begin(9600);
    
12.   Serial3.begin(9600);
    
13. }
    
14. 
    
15. void loop(){
    
16.   if(currentTime>rcTime){//50Hz
    
17.     rcTime = currentTime + 20000;
    
18.     Serial3.write(0X55);
    
19.   }
    
20.   else{
    
21.     if(Serial3.available()>=2){
    
22.       hl=Serial3.read();
    
23.       ll=Serial3.read();
    
24.       Len_mm = (hl*256+ll)/10;
    
25.       if((Len_mm >1)&&(Len_mm<4000) ){
    
26.         Serial.print("OK:");
    
27.         Serial.print(Len_mm,DEC);
    
28.         Serial.println("cm");
    
29.       }else{
    
30.         Len_mm = 0;
    
31.       }
    
32.       Serial3.flush();
    
33.     }
    
34.   }
    
35. 
    
36. 
    
37.   currentTime = micros();
    
38.   cycleTime = currentTime - previousTime;
    
39.   previousTime = currentTime;
    
40. }
    
41. 
    

复制代码

这段代码也是按mwc的传感器测量思路，测试频率50HZ
初步测试感觉比较成功，测量速度很快，下一步准备添加到MWC中

abccbef

已经搞定，0到2.5米内高度采用超声波，超过范围的使用气压计，在multiwii_dev_r1240上修改完成首先在config中定义一个自己的超声波，因为mwc自带的是i2c的超声波，SRF08就是

1.       /* Sonar */ // for visualization purpose currently - no control code behind
   
2.       //#define SRF02 // use the Devantech SRF i2c sensors
   
3.       //#define SRF08
   
4.       //#define SRF10
   
5.       //#define SRF235
   
6.       #define US100  //add by abccbef.  The US100 use Serial3 with 9600.

复制代码

sensor传感器代码里面增加

1. #elif defined(US100)  //add by abccbef
   
2. inline void Sonar_init() {}
   
3. inline void Sonar_trig() {//这个是触发测量
   
4.   SerialWrite(3,0X55);
   
5.   //Serial3.write(0X55);
   
6. }
   
7. inline void Sonar_update() {//检测测量数据
   
8.   while(SerialAvailable(3)>2){
   
9.     SerialRead(3);
   
10.   }
    
11.     if(SerialAvailable(3)==2){
    
12.       //hl=Serial3.read();
    
13.       //SerialRead(3);
    
14.       hl=SerialRead(3);
    
15.       //debug[0]=hl;
    
16.       ll=SerialRead(3);
    
17.       //debug[1]=SerialAvailable(3);
    
18.       //debug[1]=ll;
    
19.       sonarAlt = (hl*256+ll)/10;//这个就是声纳高度了
    
20.       //debug[2] = SerialRead(3);
    
21.       //debug[3] = SerialRead(3);
    
22.       if((sonarAlt >1)&&(sonarAlt<250) ){
    
23.         //sonarAlt
    
24.       }else{
    
25.         sonarAlt = 0;
    
26.       }
    
27.       //Serial3.flush();
    
28.     }  
    
29.    
    
30. }// add end

复制代码

IMU里面重算高度，本来还准备用俯仰及翻滚角度来修正声纳高度的，好像还有点问题

1.   EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 μs)
   
2.   //add by abccbef
   
3. 
   
4. 
   
5.     // Attitude of the estimated vector
   
6.     //angle[ROLL]  =  _atan2(EstG.V.X , EstG.V.Z) ;
   
7.     //angle[PITCH] =  _atan2(EstG.V.Y , EstG.V.Z) ;
   
8.     //debug[1] = 123;
   
9.     //debug[3] = 456;
   
10.    
    
11.     if(/*abs(angle[ROLL]*10)<90 && abs(angle[PITCH]*10)<90 &&*/ sonarAlt >1  && sonarAlt <250){

复制代码

MultiWii中loop主循环中增加

1.   if (currentTime > rcTime ) { // 50Hz
   
2.     rcTime = currentTime + 20000;
   
3.     computeRC();
   
4. //////////////////////////////////add by abccbef
   
5.   #if defined(US100)
   
6.     Sonar_trig();
   
7.   #endif
   
8. //////////////////////////////

复制代码

def中还得修改一下

1. #if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(TINY_GPS_SONAR) || defined(US100)
   
2.   #define SONAR 1
   
3. #else
   
4.   #define SONAR 0
   
5. #endif

复制代码

selith

占位先～小机子超声波有点用～大机子就算了……直接气压

孤独夜凄凉

看不懂~~~~

zhiyiyg

楼主学电子器件

wcj3445

收藏等楼主跟新

abccbef

selith 发表于 2012-12-11 12:59
占位先～小机子超声波有点用～大机子就算了……直接气压

本身带气压计，但是气压计误差较大，在2米一下高度时使用超声波，超过两米用气压

selith

abccbef 发表于 2012-12-11 14:53
本身带气压计，但是气压计误差较大，在2米一下高度时使用超声波，超过两米用气压

本身的气压焊掉，扩展高精的就行了……

圈圈911911

一样啊，帮忙搞下呗

kissghost

哥们给你提个醒
不要用直接IO口操作方式搞超声波
因为超声波是声速，一个来回2米距离差不多需要11.8毫秒，越远越久
直接操作IO口要等待超声波返回，会严重拖慢程序
不知道串口方式是不是超声波模块独立测量之后把数据放在寄存器等待调用
如果是这样就没问题了

y920419

觉得自带的气压计就够了。。。。毕竟声波的距离有限制。怎么说，也是各有各的好处吧。

abccbef

目前已完工作：
1、0号串口通过蓝牙从机模块连接蓝牙手机
2、2号串口通过蓝牙主机模块连接蓝牙GPS
3、3号串口连接超声波

abccbef

zhiyiyg 发表于 2012-12-11 14:24
楼主学电子器件

我是学食品工程的，目前在电脑公司做点小软件开发

ninninwpn

顶 这个不错

Smartype

kissghost 发表于 2012-12-11 19:47
哥们给你提个醒
不要用直接IO口操作方式搞超声波
因为超声波是声速，一个来回2米距离差不多需要11.8毫秒， ...

恩，应该用port寄存器和中断。

跳入死亡湖

顶，mwc也能用上超声波了

小哥哥

路過幫頂上

蔚蓝天空1975

没看懂，路過幫頂上

hammer11

楼主 有没有较全的代码传一下吧
我手里有个MEGA 2560的海盗飞控 一直没动
想改刷MWC的程序了 求个程序和代码

竞技小子

标记下 学习了