AP_Math: float versions of wrap_360 and wrap_180

libraries/AP_Math/AP_Math.h

@@ -105,6 +105,8 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
  */
 int32_t wrap_360_cd(int32_t error);
 int32_t wrap_180_cd(int32_t error);
+float wrap_360_cd_float(float angle);
+float wrap_180_cd_float(float angle);
 
 /*
   wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)

libraries/AP_Math/location.cpp

@@ -152,7 +152,7 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
 }
 
 /*
-  wrap an angle in centi-degrees to 0..36000
+  wrap an angle in centi-degrees to 0..35999
  */
 int32_t wrap_360_cd(int32_t error)
 {
@@ -179,6 +179,34 @@ int32_t wrap_180_cd(int32_t error)
     return error;
 }
 
+/*
+  wrap an angle in centi-degrees to 0..35999
+ */
+float wrap_360_cd_float(float angle)
+{
+    if (angle >= 72000.0f || angle < -36000.0f) {
+        // for larger number use fmodulus
+        angle = fmod(angle, 36000.0f);
+    }
+    if (angle >= 36000.0f) angle -= 36000.0f;
+    if (angle < 0.0f) angle += 36000.0f;
+    return angle;
+}
+
+/*
+  wrap an angle in centi-degrees to -18000..18000
+ */
+float wrap_180_cd_float(float angle)
+{
+    if (angle > 54000.0f || angle < -54000.0f) {
+        // for large numbers use modulus
+        angle = fmod(angle,36000.0f);
+    }
+    if (angle > 18000.0f) { angle -= 36000.0f; }
+    if (angle < -18000.0f) { angle += 36000.0f; }
+    return angle;
+}
+
 /*
   wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
  */