【干货】聊聊飞控的起飞落地检测(pixhawk落地检测代码分析)

frag86@126.com

文 / across

【作者简介】

不懂算法、不懂编程的飞控算法工程师
略懂飞行控制
略懂开源px4飞控

对于开发一款飞控而言，起飞和落地的检测是必不可少的，甚至占有举足轻重的地位。比如，飞控内部起飞前控制器的一些初始化设置，落地及时自动加锁等功能都必须要基于这个功能，如果出现判定不准确或者误判的情况，会导致炸鸡的风险。

那如何检测起飞与落地呢？

答案自然是根据飞控内部的传感器数据进行判定，一般而言，大多使用的传感器包含陀螺仪、加速度计、GPS等信息，那如何融合这部分数据，就是算法工程师需要解决的问题了。

检测算法基本上每家飞控都不一样，不过目的则是一致的。

以pixhawk的原生PX4固件为例，代码位置在src\modules\land_detector\MulticopterLandDetector.cpp

PX4代码的落地检测主要包含4个条件，分别是位置估计的水平速度、垂直速度、角速度和油门方向的控制量，不包含起飞检测，起飞就是在land状态进行取反，其中还根据油门杆，解锁等进行顺序判定。

大致的判定顺序如下：

1、 判定是否已经解锁，加锁状态下，不进行后续条件的判定，认为是land状态；

2、 姿态模式下，看油门杆量，如有打杆操作，则认为起飞，不是land，如果在空中，油门杆满足条件，也不影响；

3、 如果GPS数据丢失时间过长，或者位置估计没有有效值，就判定油门控制量是否已经长时间满足，满足的话，也认为是land；

4、 4个条件进行综合判定，另外该函数也有一个触发时间，默认2s，能消除数据突变造成的误判影响；

程序注解：

1、记录当前时间；

2、设置最小油门控制量参数，并进行油门控制量的判定；



3、记录最小油门满足的时间，后续会用到；



4、判定是否已经解锁，如果还没解锁，则直接返回true，不进行后面的判定，同时，记录解锁的时间，后续会用到；



5、如果是手动模式下，则只判定油门杆，大于一定阈值，就认为起飞了，返回false；



6、判定GPS是否丢包或者位置估计数据是否有效，如果不正常，就只判定油门控制量条件，前面记录的油门控制量满足的时间，如果超过8s，也认为已经落地；



7、设置阈值因子，解锁2s内偏向认为未起飞，解锁后由于电机转动，可能会使数据进行突变造成误判；



8、根据位置估计的三个轴向的速度进行垂直和水平的移动判定；



9、根据角速度进行旋转运动判定；



10、4个条件的综合判定，有一个满足，则认为是起飞，需要都满足，才能认为是落地状态；



关于pixhawk的落地检测算法的思考

优点：个人认为好处就是不会误判，这极大的保证了安全性；

为什么不会误判？

笔者认为有两点：

1、 油门控制量条件相对比较苛刻，阈值是0.15，一般满足该条件下，飞机已经不足以支撑悬停飞行了；

2、 有触发时间保障，短时间的数据突变造成误判会被消除；

缺点：

这套算法的缺点就有很多了，归纳起来有以下：

1、 将起飞与落地检测揉和在一起，起飞检测太笼统，没有分步进行，而对于飞控不同模式下的控制器初始化，这个是很有必要的。比如px4固件不支持定点模式起飞也与这个有关系；

2、 落地检测机制太保守，比如油门控制量需要小于0.15，虽然增加了安全性，但导致落地自动加锁太慢，容易造成地面的翻机；

3、 用到了位置估计的数据，因此位置估计的精度就很大程度影响了这个判定的准确性，而事实上，px4固件的落地检测算法检测准确性确实不高。

掌握了该套算法的优缺点，基本上各位可以根据自己的需求进行适当的改进，比如起飞如何改动，追求落地秒加的又如何去改，总结起来，就是用好各个数据，包括陀螺仪、加速度计、GPS、飞控的控制量、油门杆量、解锁状态以及时间，这里笔者就不细致描述了。提供两个思路：

1、 将起飞与落地检测进行分离；

2、 加入加速度数据进行撞击检测，实现落地秒加。



每周更新

把握产业链脉络

追踪高价值的产品

坚持深度有价值内容
定位无人机频道
微信号：auscor

对无人机从业者有用的号

一定有一篇文章能给你带来不可估量的价值

历史爆文




【可靠消息】国家质检总局将对一些无人机产品进行风险监测，你准备好了吗？


什么样的飞手才算是好飞手？AOPA有个卵用


这批无人机创业者真他Ma不行！


究竟是什么让植保无人机不赚钱？几乎都是一锤子买卖


无人机玩家要有规则意识！