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_yaw_minus_pi_2{kHalfSqrt2, 0.0, 0.0, -kHalfSqrt2};  // -90 deg along Z
 
  return q_ned_to_enu * q_ned * q_yaw_minus_pi_2 * q_ned_to_enu.inverse();
}
 
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_yaw_minus_pi_2{kHalfSqrt2, 0.0, 0.0, -kHalfSqrt2};  // -90 deg along Z
 
  return q_enu_to_ned * q_enu * q_yaw_minus_pi_2 * q_enu_to_ned.inverse();
}
 
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