// Heading == Yaw
double computeHeading(double qw, double qx, double qy, double qz, bool isSouthPole)
{
// For North Pole
if(isSouthPole == false && qx*qy + qz*qw == 0.5)
{
return 2 * atan2(qx,qw);
}
// For South Pole
else if(isSouthPole == true && qx*qy + qz*qw == -0.5)
{
return 2 * atan2(qx,qw);
}
else
{
return atan2(2*qy*qw-2*qx*qz , 1 - 2*qy*qy - 2*qz*qz);
}
}
// Pitch == Attitude
double computePicth(double qw, double qx, double qy, double qz)
{
return asin(2*qx*qy + 2*qz*qw);
}
// Bank == Roll
double computeBank(double qw, double qx, double qy, double qz)
{
return atan2(2*qx*qw-2*qy*qz , 1 - 2*qx*qx - 2*qz*qz);
}
Referred these:
http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/
http://en.wikipedia.org/wiki/Flight_dynamics_(fixed-wing_aircraft)
No comments:
Post a Comment