From 540ca25b840ea44f15cd94f60bffe0f2d55230d3 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 3465617015..8078732418 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 146f634cbd..4343f2ce30 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;