From 537bb04f5fea714e34627e037cc4fc88dc9ea098 Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 20 Jul 2013 17:59:06 +0900
Subject: [PATCH] AP_Math: add rotateXY
---
libraries/AP_Math/matrix3.cpp | 19 +++++++++++++++++++
libraries/AP_Math/matrix3.h | 6 +++++-
2 files changed, 24 insertions(+), 1 deletion(-)
diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp
index 346561701..807873241 100644
--- a/libraries/AP_Math/matrix3.cpp
+++ b/libraries/AP_Math/matrix3.cpp
@@ -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;
diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h
index 146f634cb..4343f2ce3 100644
--- a/libraries/AP_Math/matrix3.h
+++ b/libraries/AP_Math/matrix3.h
@@ -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;
--
GitLab