diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h
index ad4b8feede..58c1596d92 100644
--- a/libraries/AP_Math/AP_Math.h
+++ b/libraries/AP_Math/AP_Math.h
@@ -58,6 +58,7 @@ 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);
+float wrap_360(float angle);
 
 /*
   wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp
index 789942640f..c6ba3c195a 100644
--- a/libraries/AP_Math/location.cpp
+++ b/libraries/AP_Math/location.cpp
@@ -205,6 +205,20 @@ float wrap_180_cd_float(float angle)
     return angle;
 }
 
+/*
+  wrap an angle in degrees to 0..360
+ */
+float wrap_360(float angle)
+{
+    if (angle >= 720.0f || angle < -360.0f) {
+        // for larger number use fmodulus
+        angle = fmod(angle, 360.0f);
+    }
+    if (angle >= 360.0f) angle -= 360.0f;
+    if (angle < 0.0f) angle += 360.0f;
+    return angle;
+}
+
 /*
   wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
  */