frame_conversion.hpp

/****************************************************************************
 * Copyright (c) 2024 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <px4_ros2/utils/geometry.hpp>
 
namespace px4_ros2 {
template <typename Type>
Eigen::Quaternion<Type> attitudeNedToEnu(const Eigen::Quaternion<Type>& q_ned)
{
  static constexpr Type kHalfSqrt2 = static_cast<Type>(0.7071067811865476);
  const Eigen::Quaternion<Type> q_ned_to_enu{0.0, kHalfSqrt2, kHalfSqrt2,
                                             0.0};  // 180 deg along X, -90 deg along Z (intrinsic)
  const Eigen::Quaternion<Type> q_flu_to_frd{0.0, 1.0, 0.0, 0.0};  // 180 deg along X
 
  return q_ned_to_enu * q_ned * q_flu_to_frd;
}
 
template <typename Type>
Eigen::Quaternion<Type> attitudeEnuToNed(const Eigen::Quaternion<Type>& q_enu)
{
  static constexpr Type kHalfSqrt2 = static_cast<Type>(0.7071067811865476);
  const Eigen::Quaternion<Type> q_enu_to_ned{0.0, kHalfSqrt2, kHalfSqrt2,
                                             0.0};  // 180 deg along X, -90 deg along Z (intrinsic)
  const Eigen::Quaternion<Type> q_frd_to_flu{0.0, 1.0, 0.0, 0.0};  // 180 deg along X
 
  return q_enu_to_ned * q_enu * q_frd_to_flu;
}
 
template <typename Type>
Eigen::Matrix<Type, 3, 1> yawBodyToWorld(Type yaw, const Eigen::Matrix<Type, 3, 1>& point_body)
{
  const Eigen::Quaternion<Type> q_z(
      Eigen::AngleAxis<Type>(yaw, Eigen::Matrix<Type, 3, 1>::UnitZ()));
  return q_z * point_body;
}
 
template <typename T>
static inline T yawNedToEnu(const T yaw_ned_rad)
{
  return wrapPi(static_cast<T>(M_PI / 2.0) - yaw_ned_rad);
}
 
template <typename T>
static inline T yawEnuToNed(const T yaw_enu_rad)
{
  return wrapPi(static_cast<T>(M_PI / 2.0) - yaw_enu_rad);
}
 
template <typename T>
static inline T yawRateNedToEnu(const T yaw_rate_ned)
{
  return -yaw_rate_ned;
}
 
template <typename T>
static inline T yawRateEnuToNed(const T yaw_rate_enu)
{
  return -yaw_rate_enu;
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> positionNedToEnu(const Eigen::Matrix<T, 3, 1>& ned)
{
  return {ned.y(), ned.x(), -ned.z()};
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> positionEnuToNed(const Eigen::Matrix<T, 3, 1>& enu)
{
  return {enu.y(), enu.x(), -enu.z()};
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> frdToFlu(const Eigen::Matrix<T, 3, 1>& frd)
{
  return {frd.x(), -frd.y(), -frd.z()};
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> fluToFrd(const Eigen::Matrix<T, 3, 1>& flu)
{
  return {flu.x(), -flu.y(), -flu.z()};
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> varianceNedToEnu(const Eigen::Matrix<T, 3, 1>& v_ned)
{
  return {v_ned.y(), v_ned.x(), v_ned.z()};
}
 
template <typename T>
static inline Eigen::Matrix<T, 3, 1> varianceEnuToNed(const Eigen::Matrix<T, 3, 1>& v_enu)
{
  return {v_enu.y(), v_enu.x(), v_enu.z()};
}
 
}  // namespace px4_ros2