MAVROS坐标转换

飞控在OFFBOARD模式下通过MAVLINK的接口接收MAVROS上的期望，这些期望可以是期望位置、期望速度和期望姿态，而同时TX2也会从MAVROS上获取需要的飞机状态信息，一般包括飞机的控制模式、解锁状态、姿态、速度、位置信息等。 TX2获取的主要信息都来自MAVROS的/mavros/local_position/pose这个话题，但所有的位置和姿态信息都要根据坐标系来定义，本来以为它们都是使用的NED和Aircraft系，结果在使用它们运算的时候出现了很多错误，通过echo此topic的值，很容易就发现在位置上使用的是EDU坐标系，但是姿态由于是四元数的表示方法，很难明确使用的是哪两个坐标系之间的转换关系，因此，只有到MAVROS的源码中寻找了。 在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库中的旋转公式来完成的：

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

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