MWC飞控增加声纳定高的方法

MWC飞控增加声纳定高的方法

现状

MWC开源飞控已经很有点年头了,现在好多新的穿越机改用CC3D、航拍机改用APM,商业化的飞控也很多了。但是MWC作为基于Arduino的开源飞控,可以说非常成熟而且代码简单易懂,便宜,效果也不错,所以我的四轴平台依然采用了MWC飞控。MWC2.4相对前代的改进,主要是加入了对串口GPS的支持,修复了声纳崩溃问题,以及改进了计算实时性。但是声纳定高依然没有得到官方支持(包括I2C声纳)。新加入的GPS支持,由于占用内存过高,实际上在Mega之外的Arduino上无法使用。一个没有GPS定点和声纳定高的飞控,就相当于一个简单的增稳飞控而已,没有一定的技术的话生存能力不是太高。个人觉得MWC有点走偏了。

所以我的思路是在MWC2.4的基础上进一步实现声纳定高和GPS定点,至于GPS RTB、WP等功能等生存能力提高之后再考虑。声纳定高和GPS定点应该都不会对内存造成过大的压力。这次首先介绍声纳定高的方法。

传统的MWC为了支持GPS和声纳,一般是使用I2C_GPS_Nav这个项目,外置一片Arduino作为I2C GPS和声纳板。另外网上有些高手自行给MWC打了补丁,可以在低空用声纳数据覆盖气压计数据,从而利用气压定高代码。

思路

我的声纳定高基本思路也是利用气压定高代码。

但是气压计的高度读数与飞机本身姿态无关,而超声波必须考虑机身倾斜程度,将测得的距离向Z轴投影,才是真实的高度。

网上对于MWC Baro模式的描述往往就是一句气压定高,具体的使用方法并没有讲得很清楚。阅读代码发现,Baro模式的具体行为是,油门不变,MWC会维持在进入Baro时的高度上;以进入Baro模式时为基准,增减油门飞机会升降,其速率为油门每增减100点,速度相应的增减30cm/s左右;油门回到基准则维持当前高度。

用Baro模式辅助降落应该是比较有用的,飞机降到声纳有效范围之后,开启Baro模式,稍微收油,飞机缓缓降落。

实现

首先是需要把I2C_GPS_Nav中对于Pingpong型声纳的代码移植到MWC2.4的Sensor.cpp中。

我的MWC上A1和A2是空出来的,所以就用这两个作为声纳的信号引脚。

// ************************************************************************************************************
// Pingpong Sonar
// ************************************************************************************************************
// 2015.11.29 by XD : ported from I2C_GPS_NAV_v2_2
#if defined(PINGPONG_SONAR)
volatile static uint32_t Sonar_starTime = 0;
volatile static uint32_t Sonar_echoTime = 0;
volatile static uint16_t Sonar_waiting_echo = 0;

void Sonar_init()
{
  // Pin change interrupt control register - enables interrupt vectors
  PCICR  |= (1<<PCIE1); // Port C

  // Pin change mask registers decide which pins are enabled as triggers
  PCMSK1 |= (1<<PCINT10); // echo, arduino pin A2, PC2

  DDRC |= 0x02; // trigger, arduino pin A1, triggerpin PC1 as output
  Sonar_update();
}

ISR(PCINT1_vect) {
    //uint8_t pin = PINC;
    if (PINC & 1<<PCINT10) {     //indicates if the bit 0 of the arduino port [B0-B7] is at a high state
      Sonar_starTime = micros();
    }
    else {
      Sonar_echoTime = micros() - Sonar_starTime; // Echo time in microseconds

      if (Sonar_echoTime <= 700*58) {     // valid distance
        sonarAlt = Sonar_echoTime / 58;
      }
      else
      {
        // No valid data
        sonarAlt = -1;
      }
      Sonar_waiting_echo = 0;
    }
}

void Sonar_update()
{
  if (Sonar_waiting_echo == 0)
  {
    // Send 2ms LOW pulse to ensure we get a nice clean pulse
    // PORTC &= ~(0x08);//PC3 low
    PORTC &= ~(0x02);//PC1 low
    delayMicroseconds(2);

    // send 10 microsecond pulse
    // PORTC |= (0x08);//PC3 high
    PORTC |= (0x02);//PC1 high
    // wait 10 microseconds before turning off
    delayMicroseconds(10);
    // stop sending the pulse
    // PORTC &= ~(0x08);//PC3 low
    PORTC &= ~(0x02);//PC1 low
    Sonar_waiting_echo = 1;
  }
}
#endif // #if defined(PINGPONG_SONAR)

我用的声纳就是X宝上买的10多块号称比较准的超声波模块,拔掉背后的跳线帽就是Pingpong模式(不然是串口模式),这种模块还可以输出温度值用于距离补偿,但是读取温度需要额外占用10多us,而且绝对距离在飞控上意义不大,所以暂时不考虑。这种Pingpong声纳在读取距离的时候会占用10多us,相当于100多个时钟周期,软件开销还是有点的。不过好在便宜,I2C的声纳要100多软妹币,这个只要10多块。

官方I2C声纳的代码最后,如果所有的I2C声纳都没有定义,则会把Sonar_init和Sonar_update定义为空函数。这里(2.4版本Sensor.cpp:1576)需要进行一点修改,避免重复定义Sonar_init和Sonar_update:

#if !defined(PINGPONG_SONAR) // 2015.11.29 by XD : check for pingpong
inline void Sonar_init() {}
void Sonar_update() {}
#endif

在Config.h中开启Pingpong声纳:

#define PINGPONG_SONAR 

要注意在def.h中添加对Pingpong声纳的支持:

#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(I2C_GPS_SONAR) || defined(PINGPONG_SONAR) // 2015.11.29 by XD : ported from I2C_GPS_NAV_v2_2
  #define SONAR 1
#else
  #define SONAR 0
#endif

编译通过并且正确连接声纳(Trigger接A1,Echo接A2)的话,可以看到MWC GUI上Sonar的标签变绿了。

但是现在声纳数据还没有被使用,需要进一步添加代码使之与气压计数据结合。修改IMU.cpp中getEstimatedAltitude()函数,在BaroEstAlt LPF之后进行融合:

  BaroEstAlt = (BaroEstAlt * 6 + BaroAlt ) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)

  // 2015.11.29 by XD, if sonar reads less than 4.2m (sensor limit 4.5m - safety margin 0.3m), use sonar
  // this maybe unsafe...
  if ((sonarAlt > 0 && sonarAlt < 420) ||
    ((att.angle[ROLL] > -90 && att.angle[ROLL] < 90) && (att.angle[PITCH] > -90 && att.angle[PITCH] < 90)))
  {
    // actual alt = sonarAlt * cos(att.angle[ROLL]) * cos(att.angle[PITCH])
    int32_t actualAlt = abs((int32_t)sonarAlt * (_cos10(att.angle[ROLL]) * _cos10(att.angle[PITCH]) >> 8));
    alt.EstAlt = actualAlt >> 12;
  }
  else
    alt.EstAlt = BaroEstAlt;

上面这段代码中,声纳的测量值乘以Roll和Pitch的余弦就是向Z轴的投影,最终高度结果是以cm为单位。_cos10()是用于计算余弦的函数。

其实这个算法个人感觉有可能不安全,当高度超过声纳阈值的时候会突然切换到气压计高度,中间的误差可能导致飞机猛烈晃动。可以考虑设置一个过渡区,根据飞机的高度对声纳/气压数据进行权重加和,权重根据高度自动调节。

Arduino库中的cos函数开销太大,经过测试,使用原装cos函数,MWC较难在2.8ms内跑完一个周期(计算周期可以在GUI的Cycle Time查看,2.4版本大部分时候都在2800us,略有跳动)。

所以需要自己编写快速余弦函数:

// 2015.11.30 by XD, x is in 0.1 deg, returns cos * (1 << 10)
// when x approaches zero, cos(x) = 1 - x ^ 2 / 2, x is in radians
// https://en.wikipedia.org/wiki/Small-angle_approximation
// rad = deg / 180 * PI
int32_t _cos10(int16_t x)
{
  // x within [-1800, 1800]
  int32_t radTemp = (int32_t)x * 114; // rad = x * ((PI / 1800) << 16), rad within [-205200, 205200]
  int32_t rad = radTemp >> 6; // rad ^ 2 within [-657922500, 657922500]

  int32_t cos20 = ((uint32_t)1 << 20) - ((rad * rad) >> 1);
  int32_t result = cos20 >> 10;

  return result;
}

使用的算法和MWC IMU差不多,在小角度的条件下计算三角函数的级数展开式前两项。对于cos来说:cos(x) = 1 - x ^ 2 / 2,x是弧度单位。

由于Arduino没有浮点单元和硬件除法器,需要尽量避免这两种运算,这里直接手工计算PI / 1800并扩大65536倍(1 << 16)。

在求平方的时候,为了保证计算不越界,需要预先进行移位操作。

计算完成后将结果右移,最终保持10位2进制有效数字,差不多相当于4位10进制有效数字。最终结果相当于余弦乘以1024。

在前面的数据融合代码中,我们也可以看到移位操作,就是用来避免计算越界,以及把余弦多乘的1024除回来。

最后在四轴上实际测试的效果不错,Cycle Time没有明显变化,算出来的高度还是很准的,准备周末去实地飞行了。

时间: 2024-10-06 23:18:06

MWC飞控增加声纳定高的方法的相关文章

基于超声波的四轴定高控制简析

笔者是来自武汉理工的小青同学,接下来为大家讲一下基于超声波的定高问题,鉴于笔者能力有限,所以如有错误请多指教,且很多仅仅是工程上的近似化应用,没有做过仿真模拟. 我讲的东西更加偏重于实践,可以帮助你切实的实现这个应用! 基于超声波的定高主要有如下几个问题: 硬件和安装技巧? 超声波测距的算法(如何编程,如何处理结果)? 是用什么样子的PID算法? 如何调试更加快捷? 实际开发将会遇到哪些棘手问题? 定高控制如何实现较为稳定为起降? 笔者做定高用了一个星期,而且参考了别的算法,实现比较简单,但是真

Pixhawk 添加超声波定高

记录一下自己在Pixhawk上添加超声波模块进行定高的过程: 硬件:Pixhawk2.4.8(APM3.5固件).STM32开发板.几块钱一个的超声波模块 软件:Mission Planner.Keil(STM32编程用) 过程: 1.分析官方给出的各种RangFinder的添加方案和源码,确定使用STM32单片机读取超声波数据,然后以串口方式发送给Pixhawk飞控. 相关源码链接:https://github.com/ArduPilot/ardupilot/tree/master/libra

离场定高转弯DF与CF的对比

也许是刚学会CAD的缘故,配合风螺旋插件,画图的感觉真是蛮爽的,忍不住画了一张又一张. 接着昨天的离场保护区,我们来聊一下PBN指定高度转弯保护区的画法.指定高度转弯的计算本身没有太多复杂的地方,真正复杂的是DF(直飞定位点)与CF(沿航迹飞至定位点)飞行方式上的差别.规范里有这样两张图例: 图III-2-2-9 定高转弯接DF航段 图III-2-2-8 定高转弯接CF航段 先画起来,再来分析. 一.基础参数计算: 使用昨天的转弯最晚点距DER(起飞跑道末端) 13km左右的图为底图,离场转弯高

一个定高,一个高度自适应的布局

Problem:父元素内包含两个子元素,一个定高,另一个高度自适应 Ans: <div class="box">    <div class="ele1"></div>    <div class="ele2"></div></div> (1)  .box {      width:200px;      height:300px;      background:red; 

谈谈jQuery和js里有关位置和宽高的方法

总结一下jQuery.原生js设置和获取位置.宽高的方法. 一.jQuery关于元素位置的方法 position() 方法:只能获取,不能设置. 可获取元素相对于其第一个position值不为static的祖先元素的偏移量,如果没有,则是相对于文档的偏移量:返回一个对象,包括两个属性:left,top; 属性值无单位,number类型.具体用法: $(element).position().left; //获取元素相对于其第一个position值不为static的祖先元素的偏移量 offset(

改变QTreeView项高的方法(改变Delegate行高,或者::data取数据的时候,根据Qt::SizeHintRole进行判断)

很久之前写过一篇关于QTreeView快速显示超过千万条数据项的方法,如果说那篇文章讲的是QTreeView的内功的话,今天这篇是讲QTreeView的外功,有时我们想改变视图的数据项的行高,那怎么办呢?今天在这里介绍两种改变行高的方法: 1.通过更改QTreeView的默认delegate的方法 [cpp] view plain copy class CDelegate : public QStyledItemDelegate { Q_OBJECT public: CDelegate(QObj

div表格定高垂直居中,水平居中

近日做项目,遇到一个难题:就是用div ul li 做表格时,要不就是水平居中不了,要不是垂直居中不来 垂直居中用vertical-align: middle;,但是浮动把它给无效化了 因为定高,所以用不了行高 如何解决垂直居中呢? 就是在li里面加一个标签 <!DOCTYPE html> <html> <head> <meta charset="UTF-8"> <meta name="viewport" con

mwc飞控使用debug调试

0.说明 使用mwc飞控提供的debug数组来完成调试 1.使用 在MultiWii.cpp的loop中,任意位置对debug[0-3]进行赋值. 那么在连接了电脑的MultiWiiConf之后,就会在窗口下边显示出设置的值.这对于调试非常有用. 如图:

mwc飞控配置功能速查

0.说明 记录mwc飞控各个功能的配置使用情况. 所有代码基于mwc2.3 飞行模式选择 1 //#define GIMBAL // 云台 2 //#define BI // 阿凡达模式 3 //#define TRI // 3轴模式 4 //#define QUADP // +四轴 5 #define QUADX // x四轴 6 //#define Y4 // Y4轴模式 7 //#define Y6 // Y6轴模式 8 //#define HEX6 // 平面6轴+模式 9 //#def