Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 04:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: use matrix3 euler312 operations in quaternion library

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-25 09:08:31 +10:00
parent 7aa7e03169
commit 2a8f645d81
2 changed files with 4 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_Math/quaternion.cpp
View File

@ -118,23 +118,8 @@ void Quaternion::from_euler(float roll, float pitch, float yaw)
// create a quaternion from Euler angles
void Quaternion::from_vector312(float roll ,float pitch, float yaw)
{
float c3 = cosf(pitch);
float s3 = sinf(pitch);
float s2 = sinf(roll);
float c2 = cosf(roll);
float s1 = sinf(yaw);
float c1 = cosf(yaw);
Matrix3f m;
m.a.x = c1 * c3 - s1 * s2 * s3;
m.b.y = c1 * c2;
m.c.z = c3 * c2;
m.a.y = -c2*s1;
m.a.z = s3*c1 + c3*s2*s1;
m.b.x = c3*s1 + s3*s2*c1;
m.b.z = s1*s3 - s2*c1*c3;
m.c.x = -s3*c2;
m.c.y = s2;
m.from_euler312(roll, pitch, yaw);
from_rotation_matrix(m);
}
@ -253,18 +238,11 @@ void Quaternion::to_euler(float &roll, float &pitch, float &yaw) const
}
// create eulers from a quaternion
void Quaternion::to_vector312(float &roll, float &pitch, float &yaw) const
Vector3f Quaternion::to_vector312(void) const
{
Matrix3f m;
rotation_matrix(m);
float T21 = m.a.y;
float T22 = m.b.y;
float T23 = m.c.y;
float T13 = m.c.x;
float T33 = m.c.z;
yaw = atan2f(-T21, T22);
roll = safe_asin(T23);
pitch = atan2f(-T13, T33);
return m.to_euler312();
}
float Quaternion::length(void) const

2
libraries/AP_Math/quaternion.h
View File

@ -93,7 +93,7 @@ public:
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;
Vector3f to_vector312(void) const;
float length(void) const;
void normalize();