四元数转欧拉角

根据无人机上获取的四元数转为欧拉角（单位弧度）：

C++：

float q0 = flight->getQuaternion().q0;
float q1 = flight->getQuaternion().q1;
float q2 = flight->getQuaternion().q2;
float q3 = flight->getQuaternion().q3;


const double Epsilon = 0.0009765625f;
const double Threshold = 0.5f - Epsilon;
const double PI = 3.1415926;

double TEST = q3*q1 - q0*q2;

if (TEST < -Threshold || TEST > Threshold) // 奇异姿态,俯仰角为±90°
{
int sign = Sign(TEST);

yaw = -2 * sign * (double)atan2(q0, q3); // yaw

pitch = sign * (PI / 2.0); // pitch

roll = 0; // roll

}
else
{
roll = atan2(2 * (q1*q2 + q3*q1), q3*q3 - q0*q0 - q1*q1 + q2*q2) * 180 / PI;
pitch = asin(-2 * (q0*q2 - q3*q1)) * 180 / PI;
yaw = atan2(2 * (q0*q1 + q3*q2), q3*q3 + q0*q0 - q1*q1 - q2*q2) * 180 / PI;
}

ROS：

geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat)
{
  geometry_msgs::Vector3 ans;

  tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
  R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
  return ans;
}