AP_Math: added wrap_360_cd() and wrap_180_cd()

moved from per-vehicle code

libraries/AP_Math/AP_Math.h

@@ -80,6 +80,12 @@ void        location_update(struct Location *loc, float bearing, float distance)
 // extrapolate latitude/longitude given distances north and east
 void        location_offset(struct Location *loc, float ofs_north, float ofs_east);
 
+/*
+  wrap an angle in centi-degrees
+ */
+int32_t wrap_360_cd(int32_t error);
+int32_t wrap_180_cd(int32_t error);
+
 // constrain a value
 float   constrain(float amt, float low, float high);
 int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);

libraries/AP_Math/location.cpp

@@ -140,3 +140,24 @@ void location_offset(struct Location *loc, float ofs_north, float ofs_east)
         loc->lng += dlng;
     }
 }
+
+/*
+  wrap an angle in centi-degrees to 0..36000
+ */
+int32_t wrap_360_cd(int32_t error)
+{
+    while (error > 36000) error -= 36000;
+    while (error < 0) error += 36000;
+    return error;
+}
+
+/*
+  wrap an angle in centi-degrees to -18000..18000
+ */
+int32_t wrap_180_cd(int32_t error)
+{
+    while (error > 18000) error -= 36000;
+    while (error < -18000) error += 36000;
+    return error;
+}
+