科普 | 从在 PX4 环境下更换 pixhawk 的 IMU 聊起

写在前面

出于一些原因,这篇文章不给出具体的源码,因此博主试着将这篇写成了一篇科普性质的文章,如果你认真读的话,应该会有收获的。

关注我的个人公众号「我不是药丸」,支持博主写更多高质量原创文章。

为什么要更换pixhawk的传感器?大多数的玩家拿到 pixhawk 之后,是不会在意这个问题的,一个pixhawk六七百对玩票来说已经挺贵的了,自然不会想着再去花大价钱买IMU来换。

但如果你的飞行器本身价值很高的话,就很有必要更换一个传感器了。这不仅仅是精度的问题,更事关飞行器的生死存亡。GPS出了问题,飞机只是迷路而已,可能还能在天上撑几分钟,而一旦IMU出了问题,好像老司机被封闭了五感,那是当时就要翻车的。

正经点研发无人机,都是要在各种严酷的环境下做实验的,pixhawk的IMU在高温下还好,温度稍微一低,它的电子加热根本扛不住,温漂很剧烈,数据也飘得厉害。

有人说直接买别人集成好的飞控不好吗?那些飞控稳定也挺稳定的,可你根本没法修改里面的东西。有些功能,自己去搞加一句话就够了,买别人的飞控还得找人家客服给你改,想做点什么修改还得看人家的售后有没有时间,乐不乐意给你改,科研的迭代效率大大降低。

IMU替换的思路

说了这么多,再聊一聊IMU替换的思路,IMU的替换主要要做两件事:

  • 从新的IMU中读到数据。
  • 将读到的数据融入到原有的程序流中,而不影响其他进程的工作。

数据读取

数据的读取实际上是一个信息的传播过程,凡是通信过程,就绕不过通信的几个概念——信源、信道和接受者,信源将信息编码后通过信道将信息传给接受者,接受者再通过解码获得信息。在这个例子中,IMU是信源,串口是信道,而接受者是飞控,这个过程具体来讲就是IMU将姿态信息编码,再通过串口将信息传给飞控,我们在飞控中编写的程序再通过解码来获得姿态的数据。

听起来很复杂,但这实际上和几万年前原始人之间的交流其实并没有什么区别。

编码

不同的IMU编码的方式千差万别,就好像世上有法语,有英语也有中文,只不过是人为规定的而已。通信科学家们不把这些规定叫做语言,而把他们叫作协议,其实本质都是一样的。

解码

我们通过PX4源码自带的read函数来读取数据,读到数据后再写一个有限状态机,来对数据进行解码和校验,校验的方法千差万别,一般在IMU的说明书中都有写。

数据替换

现在我们已经收到了数据,但PX4的程序对我们来说是一个很复杂的系统,如何以最小的代价把这些IMU数据去替换到系统里呢?我不知道其他的开发者是怎么做的,但我觉得我所用的方法非常地「偷懒」,要解释我所用的放法,你首先要对PX4内部的程序运行机制有一定的理解。

PX4内部的数据传输机制UORB

PX4的内部有一套这样的信息传输系统,他叫做UORB,这套信息传输机制本质是一种异步通信。可以这样理解,PX4的整个程序就像一个公司,这个公司比较奇怪,每个人之间相互是不交流的,公司里的员工只干三件事:一、从公司的一些黑板上读取数据。二、对这些数据进行处理。三、将其中一些黑板上的数据擦掉,写上自己的数据。

这个公司的黑板,实际上就是UORB,每个黑板只能写特定格式的数据,并且一旦新数据写入了,旧数据也就被擦除了。

偷梁换柱

理解了这套信息传输机制,你有没有一点思路了呢?药丸是这样做的:第一步,阻止原本传输IMU信息的员工,但不阻止它传输其他的数据。第二步,雇佣一个新员工,让他去定时地把新IMU的信息写在黑板上。

这样我们就完成了一次信息的「偷梁换柱」,成功地将老IMU的数据换成了新IMU的数据,公司里的其他员工完全没有发现有什么不同。

如果你想换其他的传感器,其实也可以这样做,PX4这种高度解耦的程序设计为二次开发提供了极大的方便。但是一定要注意你有没有把一些不该拦截的数据拦截了,或者不该保留的数据保留了。

PX4数据流

让我们来看一眼PX4的数据流图,可以看到,每一个灰色的小方块就是一个搬运数据的员工,而每一个彩色的小方块则代表了一块黑板,他们之间的虚线连接表示员工会在某块黑板上写字,而他们之间的实线则表示员工从某个黑板上读取数据。

file

这张图这么乱,如何找到我们想找的黑板和员工呢?药丸帮你总结了一点小技巧:

1.从控制你飞行器的那个员工开始找,比如负责多旋翼控制的员工是mc_pos_control,先找到这个员工从哪些黑板上读取了数据,再根据各个黑板的名字和注释大致确定,哪个黑板是IMU的数据。最后再找哪个员工在这块黑板上写了字。

2.通过第一步,我们可以找到两个员工:ekf2和attitude_estimator_q,那么究竟是哪个员工在负责呢?这时我们就要实际看看到底是哪个员工在上班。我们实际运行飞控,在nsh中输入top来查看正在上班的员工的名单,发现ekf2在上班,attitude_estimator_q没在上班,那么结果很明显了,我们只要修改ekf2就可以了。

我的思路不一定对,但希望能给你启发,如果你有新想法,欢迎和我交流。