AP_Math: add wrap_2PI

libraries/AP_Math/AP_Math.h

@@ -157,6 +157,11 @@ float wrap_180_cd_float(float angle);
  */
 float wrap_PI(float angle_in_radians);
 
+/*
+  wrap an angle defined in radians to the interval [0,2*PI)
+ */
+float wrap_2PI(float angle);
+
 /*
  * check if lat and lng match. Ignore altitude and options
  */

libraries/AP_Math/location.cpp

@@ -218,6 +218,20 @@ float wrap_PI(float angle_in_radians)
     return angle_in_radians;
 }
 
+/*
+ * wrap an angle in radians to 0..2PI
+ */
+float wrap_2PI(float angle)
+{
+    if (angle > 10*PI || angle < -10*PI) {
+        // for very large numbers use modulus
+        angle = fmodf(angle, 2*PI);
+    }
+    while (angle > 2*PI) angle -= 2*PI;
+    while (angle < 0) angle += 2*PI;
+    return angle;
+}
+
 /*
   return true if lat and lng match. Ignores altitude and options
  */