Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add rotateXY

Browse Source
This commit is contained in:
Randy Mackay 2013-07-20 17:59:06 +09:00
parent 11e7e9eaf1
commit 540ca25b84
2 changed files with 24 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Math/matrix3.cpp
View File

@ -193,6 +193,24 @@ void Matrix3<T>::rotate(const Vector3<T> &g)
(*this) += temp_matrix;
}
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
template <typename T>
void Matrix3<T>::rotateXY(const Vector3<T> &g)
{
Matrix3f temp_matrix;
temp_matrix.a.x = -a.z * g.y;
temp_matrix.a.y = a.z * g.x;
temp_matrix.a.z = a.x * g.y - a.y * g.x;
temp_matrix.b.x = -b.z * g.y;
temp_matrix.b.y = b.z * g.x;
temp_matrix.b.z = b.x * g.y - b.y * g.x;
temp_matrix.c.x = -c.z * g.y;
temp_matrix.c.y = c.z * g.x;
temp_matrix.c.z = c.x * g.y - c.y * g.x;
(*this) += temp_matrix;
}
// multiplication by a vector
template <typename T>
@ -257,6 +275,7 @@ void Matrix3<T>::zero(void)
template void Matrix3<float>::rotation(enum Rotation);
template void Matrix3<float>::zero(void);
template void Matrix3<float>::rotate(const Vector3<float> &g);
template void Matrix3<float>::rotateXY(const Vector3<float> &g);
template void Matrix3<float>::from_euler(float roll, float pitch, float yaw);
template void Matrix3<float>::to_euler(float *roll, float *pitch, float *yaw);
template Vector3<float> Matrix3<float>::operator *(const Vector3<float> &v) const;

6
libraries/AP_Math/matrix3.h
View File

@ -187,7 +187,11 @@ public:
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix.
void rotate(const Vector3<T> &g);
void rotate(const Vector3<T> &g);
// apply an additional rotation from a body frame gyro vector
// to a rotation matrix but only use X, Y elements from gyro vector
void rotateXY(const Vector3<T> &g);
};
typedef Matrix3<int16_t> Matrix3i;