/** * @brief Store Quaternion to MAVLink float[4] format * * MAVLink uses wxyz order, wile Eigen::Quaterniond uses xyzw internal order, * so it can't be stored to array using Eigen::Map. */ template <typename _Scalar, typename std::enable_if<std::is_floating_point<_Scalar>::value, bool>::type = true> inline void quaternion_to_mavlink(const Eigen::Quaternion<_Scalar> &q, std::array<float, 4> &qmsg) { qmsg[0] = q.w(); qmsg[1] = q.x(); qmsg[2] = q.y(); qmsg[3] = q.z(); }

这是一个C++模板函数，用于将Eigen库中的四元数转换为MAVLink协议中的四元数数组格式，其中MAVLink协议使用wxyz顺序，而Eigen库中的Quaterniond类使用xyzw内部顺序。该函数使用std::array作为MAVLink四元数数组的容器，并将四元数的四个分量依次存储到容器中。函数的模板参数_Scalar表示四元数分量的类型，通常为float或double类型。需要注意的是，该函数只适用于实数类型的四元数。