AP_Math: change wrap_PI to ftype

prevent loss of precision

libraries/AP_Math/AP_Math.cpp

@@ -244,36 +244,25 @@ long wrap_360_cd(const long angle)
     }
     return res;
 }
-template <typename T>
-float wrap_PI(const T radian)
+
+ftype wrap_PI(const ftype radian)
 {
-    auto res = wrap_2PI(radian);
+    ftype res = wrap_2PI(radian);
     if (res > M_PI) {
         res -= M_2PI;
     }
     return res;
 }
 
-template float wrap_PI<int>(const int radian);
-template float wrap_PI<short>(const short radian);
-template float wrap_PI<float>(const float radian);
-template float wrap_PI<double>(const double radian);
-
-template <typename T>
-float wrap_2PI(const T radian)
+ftype wrap_2PI(const ftype radian)
 {
-    float res = fmodf(static_cast<float>(radian), M_2PI);
+    ftype res = fmodF(radian, M_2PI);
     if (res < 0) {
         res += M_2PI;
     }
     return res;
 }
 
-template float wrap_2PI<int>(const int radian);
-template float wrap_2PI<short>(const short radian);
-template float wrap_2PI<float>(const float radian);
-template float wrap_2PI<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

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

libraries/AP_Math/ftype.h

@@ -26,6 +26,7 @@ typedef double ftype;
 #define fabsF(x) fabs(x)
 #define ceilF(x) ceil(x)
 #define fminF(x,y) fmin(x,y)
+#define fmodF(x,y) fmod(x,y)
 #define toftype todouble
 #else
 typedef float ftype;
@@ -43,6 +44,7 @@ typedef float ftype;
 #define fabsF(x) fabsf(x)
 #define ceilF(x) ceilf(x)
 #define fminF(x,y) fminf(x,y)
+#define fmodF(x,y) fmodf(x,y)
 #define toftype tofloat
 #endif