ned-to-emu

ned-to-emu

#include <cmath>
#include <cstdio>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>

void print_quat(double x, double y, double z, double w)
{
  tf2::Quaternion quat(x, y, z, w);

  fprintf(stderr, "Quaternion:\n");
  fprintf(stderr, "x: %f\ny: %f\nz: %f\nw: %f\n\n", quat.x(), quat.y(), quat.z(), quat.w());

  double roll, pitch, yaw;

  tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw);
  fprintf(stderr, "RPY:\n");
  fprintf(stderr, "roll: %f\npitch: %f\nyaw: %f\n\n", roll, pitch, yaw);
}

tf2::Quaternion rpy_to_quat(double roll, double pitch, double yaw)
{
  tf2::Quaternion quat;
  quat.setRPY(roll, pitch, yaw);
  return quat;
}

tf2::Quaternion ned_to_enu(const tf2::Quaternion & ned_quat)
{
  // First try: https://github.com/mavlink/mavros/blob/c3e01e7cc8ceb6550e7e3f1ed4ad4e87ac4e2902/mavros/src/lib/ftf_frame_conversions.cpp#L29
  //tf2::Quaternion ned_to_enu_quat = rpy_to_quat(M_PI, 0.0, M_PI / 2.0);
  //return ned_to_enu_quat * ned_quat;

  // Second try: https://github.com/FroboLab/frobomind/blob/389f7e6b14434811e7836bf260295c8bdcf522ca/fmSensors/global_sensing/imu/vectornav_vn100/src/vectornav_vn100_node.cpp#L117
  //return tf2::Quaternion(ned_quat.y(), ned_quat.x(), -ned_quat.z(), ned_quat.w());

  // Third try: https://github.com/hidmic/imu_vn_100/blob/6fc57b44942d455e23885e4847007e0378bae6ce/src/imu_vn_100.cpp#L789)
  tf2::Quaternion ned_to_enu_quat(std::sqrt(0.5), std::sqrt(0.5), 0.0, 0.0);
  return ned_to_enu_quat * ned_quat;
}

int main()
{
  tf2::Quaternion quat;

  fprintf(stderr, "-------------- NED NEGATIVE PITCH ----------------\n");
  quat = rpy_to_quat(0.0, -1.570795, 0.0);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- SHOULD BE AN ENU POSITIVE ROLL ----------------\n");
  quat = ned_to_enu(quat);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

#if 0
  fprintf(stderr, "-------------- NED POSITIVE ROLL ----------------\n");
  quat = rpy_to_quat(1.570795, 0.0, 0.0);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- SHOULD BE AN ENU POSITIVE PITCH ----------------\n");
  quat = ned_to_enu(quat);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- NED POSITIVE PITCH ----------------\n");
  quat = rpy_to_quat(0.0, 1.570795, 0.0);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- SHOULD BE AN ENU POSITIVE ROLL ----------------\n");
  quat = ned_to_enu(quat);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- NED POSITIVE YAW ----------------\n");
  quat = rpy_to_quat(0.0, 0.0, 1.570795);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());

  fprintf(stderr, "-------------- SHOULD BE AN ENU NEGATIVE YAW ----------------\n");
  quat = ned_to_enu(quat);
  print_quat(quat.x(), quat.y(), quat.z(), quat.w());
#endif

  return 0;
}