本帖最后由 szhcs 于 2015-2-9 10:31 编辑
本文的核心内容来自于一篇“mwc se v2加超声波_笔记20131007”（作者“飞天猪”）的文章，在此先致谢。
“飞天猪”的这篇文章在网络上百度不到，我好像也是在一个QQ群里找到的，为了方便大家使用，在这里结合自己的实践，转一下。
一、硬件安装
我用的导航板是CRIUS的I2C-GPS，GPS模块是CRIUS-NEO-6，(当时与飞控板一起买的），超声模块是网上5元钱一块的HC-SR04。
导航板自己加焊了针脚，方便接超声波模块和上载程序。硬件连接图
超声波模块用支架装在底板侧面
GPS模块装在飞控上面，I2C导航板装在上层板下面。
二、导航板软件 I2C_GPS_NAV_v2_2
config.h文件中修改下列代码，打开SONAR支持
#define SONAR // USE Sonar 因为这里我们使用的SR-04模块是乒乓脉冲方式，下面改为 #define PINGPONG //#define MAXBOTIX_PWM //PWM output mode sonar
上载到导航板。
飞天猪的原文中还对I2C_GPS_NAV_v2_2文件进行了修改，但我发现这样改后测距反而不准了，其实不用改。
三、飞控软件 MWC2.3
代码修改 1、config.h文件，打开I2C_GPS和I2C_GPS_SONAR的设置 #define I2C_GPS // If your I2C GPS board has Sonar support enabled #define I2C_GPS_SONAR
2、IMU.cpp 文件，增加逻辑：当超声波读数低于3米时，代替气压计读数
uint16_tcurrentT = micros(); uint16_tdTime; static int32_t tmpEstAlt = 0; //in cm 用于替换原气压高度的LPF运算，方便与超声波读数切换
dTime =currentT - previousT; if(dTime < UPDATE_INTERVAL) return 0; previousT = currentT;
if(calibratingB > 0) { logBaroGroundPressureSum = log(baroPressureSum); baroGroundTemperatureScale = (baroTemperature + 27315) * 29.271267f; calibratingB--; }
//baroGroundPressureSum is not supposed to be 0 here 气压地面压力和不应为0 BaroAlt= ( logBaroGroundPressureSum - log(baroPressureSum) ) *baroGroundTemperatureScale;
// 用tmpEstAlt变量替换原alt.EstAlt变量进行气压高度低通滤波 tmpEstAlt = (tmpEstAlt * 6 + BaroAlt * 2) >> 3; //additional LPF to reduce baro noise (faster by 30 μs)
//在这里给alt.EstAlt赋值 if(sonarAlt>0 && sonarAlt<300 ) { alt.EstAlt = sonarAlt; //来自声呐的值 } else { alt.EstAlt = tmpEstAlt; //来自气压计的值 }
#if(defined(VARIOMETER) && (VARIOMETER != 2)) ||!defined(SUPPRESS_BARO_ALTHOLD)
以上红字是修改部分或增加部分
上载到飞控板
四、GUI
用GUI查看，只要超声测出的高度小于3米，就使用超声波模块的高度，一条直线。
五、试飞
很遗憾，我试飞失败了。
拿手里，离地一米左右，高度显示准确，但只要一推油门，高度马上就乱了，一直在0.3米左右乱跳，个人感觉是电机和桨的声音干扰了超声测距。
没再继续研究，以失败告终，希望有兴趣者能找出真正的问题并成功。
原作者“飞天猪”试飞成功了，有视频，他使用的也是HC-SR04模块
http://video.baomihua.com/37784322/33935638
I2C导航板的使用可以参考这个帖子
http://bbs.5imx.com/bbs/forum.php?mod=viewthread&tid=782677
| 
/1 
发表于 2015-2-7 21:13
收藏
楼主
