Math: add wrap_PI

libraries/AP_Math/AP_Math.h

@@ -94,6 +94,11 @@ void        location_offset(struct Location *loc, float ofs_north, float ofs_eas
 int32_t wrap_360_cd(int32_t error);
 int32_t wrap_180_cd(int32_t error);
 
+/*
+  wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
+ */
+float wrap_PI(float angle_in_radians);
+
 /*
   print a int32_t lat/long in decimal degrees
  */

libraries/AP_Math/location.cpp

@@ -161,6 +161,15 @@ int32_t wrap_180_cd(int32_t error)
     return error;
 }
 
+/*
+  wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
+ */
+float wrap_PI(float angle_in_radians)
+{
+    while (angle_in_radians > PI) angle_in_radians -= 2.0f*PI;
+    while (angle_in_radians < -PI) angle_in_radians += 2.0f*PI;
+    return angle_in_radians;
+}
 
 /*
   print a int32_t lat/long in decimal degrees