Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add get_euler_(roll|pitch|yaw) functions to quaternion

This commit is contained in:
Jonathan Challinger 2015-04-23 12:44:49 -07:00 committed by Andrew Tridgell
parent 020d9ea78a
commit fb8da1b2d8
2 changed files with 30 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
libraries/AP_Math/quaternion.cpp
View File

@ -226,12 +226,30 @@ void Quaternion::rotate_fast(const Vector3f &v) {
q4 = w1*z2 + x1*y2 - y1*x2 + z1*w2;
}
// get euler roll angle
float Quaternion::get_euler_roll() const
{
return (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 2.0f*(q2*q2 + q3*q3)));
}
// get euler pitch angle
float Quaternion::get_euler_pitch() const
{
return safe_asin(2.0f*(q1*q3 - q4*q2));
}
// get euler yaw angle
float Quaternion::get_euler_yaw() const
{
return atan2f(2.0f*(q1*q4 + q2*q3), 1 - 2.0f*(q3*q3 + q4*q4));
}
// create eulers from a quaternion
void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const
{
roll = (atan2f(2.0f*(q1*q2 + q3*q4), 1 - 2.0f*(q2*q2 + q3*q3)));
pitch = safe_asin(2.0f*(q1*q3 - q4*q2));
yaw = atan2f(2.0f*(q1*q4 + q2*q3), 1 - 2.0f*(q3*q3 + q4*q4));
roll = get_euler_roll();
pitch = get_euler_pitch();
yaw = get_euler_yaw();
}
// create eulers from a quaternion

9
libraries/AP_Math/quaternion.h
View File

@ -80,10 +80,19 @@ public:
void rotate_fast(const Vector3f &v);
// get euler roll angle
float get_euler_roll() const;
// get euler pitch angle
float get_euler_pitch() const;
// get euler yaw angle
float get_euler_yaw() const;
// create eulers from a quaternion
void to_euler(float &roll, float &pitch, float &yaw) const;
// create eulers from a quaternion
void to_vector312(float &roll, float &pitch, float &yaw) const;
float length(void) const;