mavros中实现东北天坐标系到北东地坐标系转换的代码

/mavros/mavros/include/mavros/frame_tf.hpp

/**
 * @brief Transform from attitude represented WRT NED frame to attitude
 *		  represented WRT ENU frame
 */
template<class T>
inline T transform_orientation_ned_enu(const T & in)
{
  return detail::transform_orientation(in, StaticTF::NED_TO_ENU);
}

/mavros/mavros/src/lib/ftf_frame_conversions.cpp

/**
 * @brief Static quaternion needed for rotating between ENU and NED frames
 * NED to ENU: +PI/2 rotation about Z (Down) followed by a +PI rotation around X (old North/new East)
 * ENU to NED: +PI/2 rotation about Z (Up) followed by a +PI rotation about X (old East/new North)
 */
static const auto NED_ENU_Q = quaternion_from_rpy(M_PI, 0.0, M_PI_2);

/mavros/mavros/src/lib/ftf_frame_conversions.cpp

Eigen::Quaterniond transform_orientation(const Eigen::Quaterniond & q, const StaticTF transform)
{
  // Transform the attitude representation from frame to frame.
  // The proof for this transform can be seen
  // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/
  switch (transform) {
    case StaticTF::NED_TO_ENU:
    case StaticTF::ENU_TO_NED:
      return NED_ENU_Q * q;

    case StaticTF::AIRCRAFT_TO_BASELINK:
    case StaticTF::BASELINK_TO_AIRCRAFT:
      return q * AIRCRAFT_BASELINK_Q;

    case StaticTF::ABSOLUTE_FRAME_AIRCRAFT_TO_BASELINK:
    case StaticTF::ABSOLUTE_FRAME_BASELINK_TO_AIRCRAFT:
      return AIRCRAFT_BASELINK_Q * q;

    default:
      rcpputils::require_true(false, "unsupported transform arg");
      return q;
  }
}

这篇博客可以参考

mavros坐标系转换与方向_mavros baselink坐标系向edu坐标系转换-CSDN博客