AP_Math: added rotate_xy to Vector3f

this makes it easy to rotate a vector3f in xy plane for earth/body
navigation

libraries/AP_Math/vector3.cpp

@@ -282,6 +282,18 @@ void Vector3<T>::rotate_inverse(enum Rotation rotation)
     (*this) = M.mul_transpose(*this);
 }
 
+// rotate vector by angle in radians in xy plane
+template <typename T>
+void Vector3<T>::rotate_xy(float angle_rad)
+{
+    const float cs = cosf(angle_rad);
+    const float sn = sinf(angle_rad);
+    float rx = x * cs - y * sn;
+    float ry = x * sn + y * cs;
+    x = rx;
+    y = ry;
+}
+
 // vector cross product
 template <typename T>
 Vector3<T> Vector3<T>::operator %(const Vector3<T> &v) const

libraries/AP_Math/vector3.h

@@ -186,6 +186,9 @@ public:
     void rotate(enum Rotation rotation);
     void rotate_inverse(enum Rotation rotation);
 
+    // rotate in xy plane
+    void rotate_xy(float rotation_rad);
+
     // gets the length of this vector squared
     T  length_squared() const
     {