From fff275fd99dbf2a87e11b6c824320d74bd6ab3b7 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Tue, 24 Nov 2015 13:11:34 -0800
Subject: [PATCH] AP_Math: add wrap_2PI

---
 libraries/AP_Math/AP_Math.h    |  5 +++++
 libraries/AP_Math/location.cpp | 14 ++++++++++++++
 2 files changed, 19 insertions(+)

diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index 7cae64d702..8ca7f3b4c8 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/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
  */
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index db8e0c0248..fb82db2a41 100644
--- a/libraries/AP_Math/location.cpp
+++ b/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
  */