5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年）

标题: mwc mega 超声波定高 [打印本页]
作者: abccbef    时间: 2012-12-11 12:48
标题: mwc mega 超声波定高
超声波模块支持GPIO和串口两种模式，一开始用GPIO模式
  1.   digitalWrite(TrigPin,HIGH);
  2.   delayMicroseconds(50);
  3.   digitalWrite(TrigPin,LOW);

  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;

  5. static uint32_t currentTime = 0;
  6. static uint32_t rcTime  = 0;
  7. static uint16_t previousTime = 0;
  8. static uint16_t cycleTime = 0;

  10. void setup(){
  11.   Serial.begin(9600);
  12.   Serial3.begin(9600);
  13. }

  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.   }


  37.   currentTime = micros();
  38.   cycleTime = currentTime - previousTime;
  39.   previousTime = currentTime;
  40. }

复制代码
这段代码也是按mwc的传感器测量思路，测试频率50HZ
初步测试感觉比较成功，测量速度很快，下一步准备添加到MWC中
作者: abccbef    时间: 2012-12-11 12:49
本帖最后由 abccbef 于 2012-12-12 08:50 编辑

已经搞定，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


  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    时间: 2012-12-11 12:59
占位先～小机子超声波有点用～大机子就算了……直接气压
作者: 孤独夜凄凉    时间: 2012-12-11 13:03
看不懂~~~~
作者: zhiyiyg    时间: 2012-12-11 14:24
楼主学电子器件
作者: wcj3445    时间: 2012-12-11 14:38
收藏等楼主跟新
作者: abccbef    时间: 2012-12-11 14:53
selith 发表于 2012-12-11 12:59
占位先～小机子超声波有点用～大机子就算了……直接气压

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

本身的气压焊掉，扩展高精的就行了……
作者: 圈圈911911    时间: 2012-12-11 17:16
一样啊，帮忙搞下呗
作者: kissghost    时间: 2012-12-11 19:47
哥们给你提个醒
不要用直接IO口操作方式搞超声波
因为超声波是声速，一个来回2米距离差不多需要11.8毫秒，越远越久
直接操作IO口要等待超声波返回，会严重拖慢程序
不知道串口方式是不是超声波模块独立测量之后把数据放在寄存器等待调用
如果是这样就没问题了
作者: y920419    时间: 2012-12-11 20:47
觉得自带的气压计就够了。。。。毕竟声波的距离有限制。怎么说，也是各有各的好处吧。
作者: abccbef    时间: 2012-12-12 08:59
目前已完工作：
1、0号串口通过蓝牙从机模块连接蓝牙手机
2、2号串口通过蓝牙主机模块连接蓝牙GPS
3、3号串口连接超声波
作者: abccbef    时间: 2012-12-12 09:00
zhiyiyg 发表于 2012-12-11 14:24
楼主学电子器件

我是学食品工程的，目前在电脑公司做点小软件开发
作者: ninninwpn    时间: 2012-12-12 17:44
顶 这个不错
作者: Smartype    时间: 2012-12-12 20:00
kissghost 发表于 2012-12-11 19:47
哥们给你提个醒
不要用直接IO口操作方式搞超声波
因为超声波是声速，一个来回2米距离差不多需要11.8毫秒， ...

恩，应该用port寄存器和中断。
作者: 跳入死亡湖    时间: 2012-12-12 20:09
顶，mwc也能用上超声波了
作者: 小哥哥    时间: 2012-12-14 09:02
路過幫頂上
作者: 蔚蓝天空1975    时间: 2012-12-14 09:43
没看懂，路過幫頂上
作者: hammer11    时间: 2012-12-14 11:28
楼主 有没有较全的代码传一下吧
我手里有个MEGA 2560的海盗飞控 一直没动
想改刷MWC的程序了 求个程序和代码
作者: 竞技小子    时间: 2013-1-15 16:24
标记下 学习了
作者: 小哥哥    时间: 2013-1-15 19:38
留著
留意樓主的發展
作者: XIEYUNFENG    时间: 2013-1-15 21:06
帮顶
作者: kissghost    时间: 2013-1-23 16:17
前几天在APM上用串口完成了超声波
不错
作者: bys_1123    时间: 2013-2-22 01:52
这个代码是LZ自己写的还是官方的啊？没大看懂
作者: 爵爷    时间: 2013-2-22 12:43
楼主能不能改下代码让mwc别支持那么偏门的超声波模块啊...和海盗一样的多好
作者: 刘博    时间: 2013-2-22 14:03
我的是328的crius se板子！能用这个吗？能详细一点点吗？
作者: fryefryefrye    时间: 2013-3-5 23:39
爵爷 发表于 2013-2-22 12:43
楼主能不能改下代码让mwc别支持那么偏门的超声波模块啊...和海盗一样的多好

看楼主用的9600，发送0x55命令，应该是淘宝销量最大的那个15元的超声波了。我也买了两个，准备接到mega上面。

作者: mealie    时间: 2013-3-6 11:03
感谢楼主的努力，非常好的共享精神。不过我个人观点认为用声纳意义不大，现在APM、MPNG新版都已经不再让声纳参与定高了。只要算法足够好，气压计就已经很好用啦。
作者: racoco    时间: 2013-3-6 13:41
MPNG有用超聲波定高呀！
https://code.google.com/p/megapirateng/wiki/dypme007
作者: siasdanny    时间: 2013-3-6 19:05
学习了，期待也加上超声波
作者: y920419    时间: 2013-3-12 12:51
本帖最后由 y920419 于 2013-3-12 18:06 编辑

LZ求助，我按你的步骤加进去之后，一直显示错误。。。
能把改过之后的发给我一下吗？  253063845@qq.com
作者: blackbirds    时间: 2013-5-19 14:15
我也想在MWC上添加超声波，楼主有改好的固件吗？可以发给我一份吗？谢谢！我的邮箱  shuizdaoedl@sina.cn
作者: lee3327    时间: 2013-5-20 00:47
我也想在MWC上添加超声波，楼主有改好的固件吗？可以发给我一份吗？谢谢！我的邮箱 82377355@qq.com
作者: ancky    时间: 2013-6-15 18:34
啥东西呢？？？太难了
作者: gxhifi    时间: 2013-7-8 23:20
嗯~先顶~留着~谢楼主
作者: gxhifi    时间: 2013-7-19 02:24
abccbef 发表于 2012-12-11 12:49
已经搞定，0到2.5米内高度采用超声波，超过范围的使用气压计，在multiwii_dev_r1240上修改完成首先在config ...

楼主!不得不问细些.
像在IMU那段里面的代码是否完全.我会点点JAVA

看这段的代码觉得有点不完整.是这样么!求完整代码了!不会单片机不会C头大啊

作者: 宇浩轩杰    时间: 2013-7-19 06:29
好厉害，这是神马语言？
作者: aweixinga    时间: 2013-7-19 09:11
不懂，学习中1！
作者: gxhifi    时间: 2013-7-19 12:00
selith 发表于 2012-12-11 14:55
本身的气压焊掉，扩展高精的就行了……

神......

真的可以这么干嘛!我的MWC飞控板带的低精气压.之前也这么想过.但是有懂单片机的兄弟说.直接焊掉换上的话.XXX硬件地址不一样不能用?有这回事!

求真相....如果可以焊,我马上丢掉这Sonar

作者: selith    时间: 2013-7-19 20:36
gxhifi 发表于 2013-7-19 12:00
神......

真的可以这么干嘛!我的MWC飞控板带的低精气压.之前也这么想过.但是有懂单片机的兄弟说.直接焊 ...

不能直接焊上去……用I2C接

作者: ywywxb    时间: 2013-8-30 20:53

学习了
作者: gxhifi    时间: 2013-9-3 17:02



楼主你的这个代码

是在什么版本代码基础上修该添加的?

作者: gxhifi    时间: 2013-9-4 18:06
abccbef 发表于 2012-12-11 12:49
已经搞定，0到2.5米内高度采用超声波，超过范围的使用气压计，在multiwii_dev_r1240上修改完成首先在config ...

楼主.我的代码添加到 这一步 sensor

时.添加你的代码后.我通过校验代码功能发现.报错.

我不知道是因为没有所有代码都加外出的错.还是我没加对地方.

我会些JAVA的AS3的语法.但是对于这个代码还是不大清础.

楼主可以给一定帮助么!我也想给我的MWC加上超声

我是2560的板子.

作者: cnsqmy    时间: 2013-9-4 18:08
回复赚积分喽！
作者: gxhifi    时间: 2013-9-4 23:37



楼主.经查.你这代码应该是2.1的版本的.求问可不可以写个2.2后版本的.

2.2的自稳给力呀....

作者: a872529868    时间: 2013-9-5 01:29
abccbef 发表于 2012-12-11 12:49
已经搞定，0到2.5米内高度采用超声波，超过范围的使用气压计，在multiwii_dev_r1240上修改完成首先在config ...

建议外加一个湿度传感器这样精度大概能去到±1CM。

作者: q1q360370q1q    时间: 2013-12-21 13:36
提示: 作者被禁止或删除 内容自动屏蔽
作者: 57185490    时间: 2013-12-21 17:11
这个帖子有点nb呀
作者: SamitTech    时间: 2014-2-21 16:15
最好是用IO中断触发来接收超声波的数据吧。这样效率高，还有，楼主现在做得怎样了
作者: 狐狸精    时间: 2014-6-11 21:23
求指导啊！最近买了一个MWC2.5的飞控，感觉定高不是很够用，想用超声波！
作者: dragontan    时间: 2014-9-2 17:34
abccbef 发表于 2012-12-11 12:49
已经搞定，0到2.5米内高度采用超声波，超过范围的使用气压计，在multiwii_dev_r1240上修改完成首先在config ...

lz使用超声波定高，需要调整PID吗？

作者: wesker小威    时间: 2014-9-2 19:15
很赞 支持一个
作者: dragontan    时间: 2014-9-20 10:35
lee3327 发表于 2013-5-20 00:47
我也想在MWC上添加超声波，楼主有改好的固件吗？可以发给我一份吗？谢谢！我的邮箱

你好请问你在MWC中加入超声波定高成功了吗





欢迎光临 5iMX.com 我爱模型 玩家论坛 ——专业遥控模型和无人机玩家论坛（玩模型就上我爱模型，创始于2003年） (http://wz.5imx.com/) Powered by Discuz! X3.3