AP_Math: add rotateXY

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;

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;