aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: added euler312 functions (for gimbal)

Browse Source
This commit is contained in:
Andrew Tridgell 2015-05-25 09:02:42 +10:00
parent 2bb5f677c7
commit 7aa7e03169
2 changed files with 51 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
libraries/AP_Math/matrix3.cpp
View File

@ -58,6 +58,43 @@ void Matrix3<T>::to_euler(float *roll, float *pitch, float *yaw) const
}
}
/*
calculate Euler angles (312 convention) for the matrix.
See http://www.atacolorado.com/eulersequences.doc
vector is returned in r, p, y order
*/
template <typename T>
Vector3<T> Matrix3<T>::to_euler312() const
{
return Vector3<T>(asinf(c.y),
atan2f(-c.x, c.z),
atan2f(-a.y, b.y));
}
/*
fill the matrix from Euler angles in radians in 312 convention
*/
template <typename T>
void Matrix3<T>::from_euler312(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);
a.x = c1 * c3 - s1 * s2 * s3;
b.y = c1 * c2;
c.z = c3 * c2;
a.y = -c2*s1;
a.z = s3*c1 + c3*s2*s1;
b.x = c3*s1 + s3*s2*c1;
b.z = s1*s3 - s2*c1*c3;
c.x = -s3*c2;
c.y = s2;
}
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
template <typename T>
@ -197,6 +234,8 @@ template void Matrix3<float>::rotateXYinv(const Vector3<float> &g);
template void Matrix3<float>::normalize(void);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw) const;
template void Matrix3<float>::from_euler312(float roll, float pitch, float yaw);
template Vector3<float> Matrix3<float>::to_euler312(void) const;
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;
template Vector3<float> Matrix3<float>::mul_transpose(const Vector3<float> &v) const;
template Matrix3<float> Matrix3<float>::operator *(const Matrix3<float> &m) const;

12
libraries/AP_Math/matrix3.h
View File

@ -208,6 +208,18 @@ public:
// create eulers from a rotation matrix
void to_euler(float *roll, float *pitch, float *yaw) const;
/*
calculate Euler angles (312 convention) for the matrix.
See http://www.atacolorado.com/eulersequences.doc
vector is returned in r, p, y order
*/
Vector3<T> to_euler312() const;
/*
fill the matrix from Euler angles in radians in 312 convention
*/
void from_euler312(float roll, float pitch, float yaw);
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
void rotate(const Vector3<T> &g);