AP_Math: added quaternion from_angular_velocity

libraries/AP_Math/quaternion.cpp

@@ -534,6 +534,19 @@ void QuaternionT<T>::from_axis_angle_fast(const Vector3<T> &axis, T theta)
     q4 = axis.z * st2;
 }
 
+template <typename T>
+void QuaternionT<T>::from_angular_velocity(const Vector3<T>& angular_velocity, float time_delta)
+{
+    const float half_time_delta = 0.5f*time_delta;
+
+    q1 = 1.0;
+    q2 = half_time_delta*angular_velocity.x;
+    q3 = half_time_delta*angular_velocity.y;
+    q4 = half_time_delta*angular_velocity.z;
+    normalize();
+}
+
+
 // rotate by the provided axis angle
 // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
 template <typename T>

libraries/AP_Math/quaternion.h

@@ -112,6 +112,8 @@ public:
     // the axis vector must be length 1, theta should less than 0.17 radians (i.e. 10 degrees)
     void        from_axis_angle_fast(const Vector3<T> &axis, T theta);
 
+    void        from_angular_velocity(const Vector3<T>& angular_velocity, float time_delta);
+
     // rotate by the provided rotation vector
     // only use with small angles.  I.e. length of v should less than 0.17 radians (i.e. 10 degrees)
     void        rotate_fast(const Vector3<T> &v);