在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的数据流图，可以看到，每一个灰色的小方块就是一个搬运数据的员工，而每一个彩色的小方块则代表了一块黑板，他们之间的虚线连接表示员工会在某块黑板上写字，而他们之间的实线则表示员工从某个黑板上读取数据。

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

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

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

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