MAVROS PX4

最近在做视觉识别的项目，视觉开发的工作在NVIDIA的JETSON TX2板卡上完成，也可以用ODROID，这个视情况而定，而视觉识别后的结果要传输给飞控，因为在TX2上面跑的是ROS系统，所以TX2与PIXHAWK数据之间的交互使用到了MAVROS，飞控在OFFBOARD模式下通过MAVLINK的接口接收MAVROS上的期望，这些期望可以是期望位置、期望速度和期望姿态，而同时TX2也会从MAVROS上获取需要的飞机状态信息，一般包括飞机的控制模式、解锁状态、姿态、速度、位置信息等。
TX2获取的主要信息都来自与MAVROS中的/mavros/local_position/pose这个话题，但所有的位置和姿态信息都要根据坐标系来定义，本来以为它们都是使用的NED和机体坐标系，结果在使用它们运算的时候出现了很多错误，通过echo此topic的值，很容易就发现在位置上使用的是EDU坐标系，但是姿态由于是四元数的表示方法，很难明确使用的是哪两个坐标系之间的转换关系，因此，只有到MAVROS的源码中寻找了。
先到github上下载mavros的源码：
git clone https://github.com/mavlink/mavros.git
然后在plugins文件夹下找到local_position.cpp文件：

advertise函数返回一个 Publisher对象。 通过调用对象的publish( )函数，我们可以在这个topic上发布 message。

这就是MAVROS发布消息的过程。接下来我们看到的是位置信息是东北天（EDU）坐标系下的，而姿态信息也是EDU坐标系向Baselink坐标系的旋转关系，这些代码在imu.cpp文件中可以找到：

姿态信息来源代码：

ned_aircraft_orientation是来自飞控计算得到的NED-机体坐标系的四元数姿态坐标，而enu_baselink_orientation是将坐标系经过两次转换变成了ENU-Baselink坐标系的四元数姿态坐标。
机体坐标系是这样的（X在参考平面内沿机头方向向前，Y轴垂直机身参考平面向右，Z轴在参考平面内垂直XOY平面向下）：

Baselink坐标系是这样的（X在参考平面内沿机头方向向前，Y轴垂直机身参考平面向左，Z轴在参考平面内垂直XOY平面向上）：

而上述这些坐标转换都使用了Eigen库中的旋转公式来完成的：

上述的公式其实就是著名的罗德里格斯旋转公式，其源码是这样的，因为是绕轴逆时针旋转，所以其绕坐标轴旋转得到的结果和我们以前推导的旋转公式刚好是转置的关系：

罗德里格斯旋转公式是这样的：

关于Eigen库中欧拉角、四元数、旋转矩阵的转换关系，我们后面有时间可以再来写一写，其实最重要的还是要理清楚他们的旋转方向，在什么坐标系下面进行的旋转以及旋转的顺序是怎么样的。

本期科学小知识：

蒸馒头的时候哪层的馒头先熟？

答案是最上面一层，因为蒸馒头的时候主要靠蒸气来蒸熟，蒸气比空气轻会往上走，所以最上面那层的馒头接触蒸气最多最久，会先熟。

                                ----------THE END----------

往期精彩文章：

无人机理论（一）：坐标转换与姿态描述（基础篇）

无人机理论（二）：欧拉角与万向锁

无人机理论（三）：简单搞定四元数

PX4算法解析（一）：L1制导律

PX4算法解析（二）：L1制导律进阶

感兴趣的读者可长按下方二维码识别关注公众号：无人机系统技术，也可在公众号底部点击加群交流进入微信群技术讨论和交流：