From 091b474a1ddc653ef020ec1ceadb6b54614a29cc Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Fri, 29 Mar 2013 13:13:37 +1100
Subject: [PATCH] AP_Math: added wrap_360_cd() and wrap_180_cd()

moved from per-vehicle code
---
 libraries/AP_Math/AP_Math.h    |  6 ++++++
 libraries/AP_Math/location.cpp | 21 +++++++++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 151b750e3e..2d9e0d3e12 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/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);
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index 0cfe68c326..f42a491d14 100644
--- a/libraries/AP_Math/location.cpp
+++ b/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;
+}
+