add separate double and float implementations for wrap_PI and wrap_2PI

libraries/AP_Follow/AP_Follow.cpp

@@ -289,7 +289,7 @@ void AP_Follow::update_estimates()
         // update heading angle separately to maintain proper wrapping [-PI, PI]
         postype_t estimate_heading_rad = _estimate_heading_rad;
         update_pos_vel_accel(estimate_heading_rad, _estimate_heading_rate_rads, _estimate_heading_accel_radss, e_dt, 0.0, 0.0, 0.0);
-        _estimate_heading_rad = wrap_PI(estimate_heading_rad);
+        _estimate_heading_rad = wrap_PI(float(estimate_heading_rad));
     } else {
         // no valid estimate yet: initialise from latest target position
         _estimate_pos_ned_m = _target_pos_ned_m + delta_pos_m.topostype();

libraries/AP_Math/AP_Math.cpp

@@ -248,24 +248,37 @@ long wrap_360_cd(const long angle)
     return res;
 }
 
-ftype wrap_PI(const ftype radian)
+float wrap_2PI(const float radian)
 {
-    ftype res = wrap_2PI(radian);
-    if (res > M_PI) {
-        res -= M_2PI;
+    float res = fmodf(radian, float(M_2PI));
+    if (res < 0) {
+        res += float(M_2PI);
     }
     return res;
 }
 
-ftype wrap_2PI(const ftype radian)
+double wrap_2PI(const double radian)
 {
-    ftype res = fmodF(radian, M_2PI);
+    double res = fmod(radian, M_2PI);
     if (res < 0) {
         res += M_2PI;
     }
     return res;
 }
 
+template <typename T>
+T wrap_PI(const T radian)
+{
+    T res = wrap_2PI(radian);
+    if (res > T(M_PI)) {
+        res -= T(M_2PI);
+    }
+    return res;
+}
+
+template float wrap_PI<float>(const float radian);
+template double wrap_PI<double>(const double radian);
+
 template <typename T>
 T constrain_value_line(const T amt, const T low, const T high, uint32_t line)
 {

libraries/AP_Math/AP_Math.h

@@ -155,14 +155,16 @@ double wrap_360_cd(const double angle);
 
 
 /*
-  wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
+  wrap an angle in radians to 0..2PI
  */
-ftype wrap_PI(const ftype radian);
+float wrap_2PI(const float radian);
+double wrap_2PI(const double radian);
 
 /*
- * wrap an angle in radians to 0..2PI
+  wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
  */
-ftype wrap_2PI(const ftype radian);
+template <typename T>
+T wrap_PI(const T radian);
 
 /*
  * Constrain a value to be within the range: low and high

libraries/AP_Math/tests/test_math.cpp

@@ -533,8 +533,7 @@ TEST(MathWrapTest, AnglePI)
     EXPECT_NEAR(0.f,     wrap_PI(M_2PI),     accuracy);
     EXPECT_NEAR(0,       wrap_PI(M_PI * 10), accuracy);
     EXPECT_NEAR(-2.1415925025939941f,    wrap_PI(M_PI+1),      accuracy);
-    EXPECT_NEAR(1.f,     wrap_PI(1),     accuracy);
-    EXPECT_NEAR(1.f,     wrap_PI((short)1),     accuracy);
+    EXPECT_NEAR(1.f,     wrap_PI(1.0),     accuracy);
 }
 
 TEST(MathWrapTest, Angle2PI)
@@ -547,8 +546,7 @@ TEST(MathWrapTest, Angle2PI)
     EXPECT_NEAR(0.f,  wrap_2PI(0.f), accuracy);
     EXPECT_NEAR(M_PI, wrap_2PI(-M_PI), accuracy);
     EXPECT_NEAR(0,    wrap_2PI(-M_2PI), accuracy);
-    EXPECT_NEAR(1,    wrap_2PI(1), accuracy);
-    EXPECT_NEAR(1,    wrap_2PI((short)1), accuracy);
+    EXPECT_NEAR(1,    wrap_2PI(1.0), accuracy);
 }
 
 TEST(MathTest, ASin)