AP_Math: fix loss of precision on float addition

When using wrap_180_cd() we are adding a small float (180 * 100) to a
possibly big number. This may lose float precision as illustrated by the
unit test failing:

    OUT: ../../libraries/AP_Math/tests/test_math.cpp:195: Failure
    OUT: Value of: wrap_180_cd(-3600000000.f)
    OUT:   Actual: -80
    OUT: Expected: 0.f
    OUT: Which is: 0

libraries/AP_Math/AP_Math.cpp

@@ -52,13 +52,9 @@ float linear_interpolate(float low_output, float high_output,
 template <class T>
 float wrap_180(const T angle, float unit_mod)
 {
-    const float ang_180 = 180.f * unit_mod;
-    const float ang_360 = 360.f * unit_mod;
-    float res = fmod(static_cast<float>(angle) + ang_180, ang_360);
-    if (res < 0 || is_zero(res)) {
-        res += ang_180;
-    } else {
-        res -= ang_180;
+    auto res = wrap_360(angle, unit_mod);
+    if (res > 180.f * unit_mod) {
+        res -= 360.f * unit_mod;
     }
     return res;
 }
@@ -109,11 +105,9 @@ template auto wrap_360_cd<double>(const double angle) -> decltype(wrap_360(angle
 template <class T>
 float wrap_PI(const T radian)
 {
-    float res = fmod(static_cast<float>(radian) + M_PI, M_2PI);
-    if (res < 0 || is_zero(res)) {
-        res += M_PI;
-    } else {
-        res -= M_PI;
+    auto res = wrap_2PI(radian);
+    if (res > M_PI) {
+        res -= M_2PI;
     }
     return res;
 }

libraries/AP_Math/tests/test_math.cpp

@@ -240,4 +240,25 @@ TEST(MathWrapTest, Angle360)
     EXPECT_EQ(0.f,     wrap_360_cd(-72000.f));
 }
 
+TEST(MathWrapTest, AnglePI)
+{
+    const float accuracy = 1.0e-5;
+
+    EXPECT_NEAR(M_PI,    wrap_PI(M_PI),      accuracy);
+    EXPECT_NEAR(0.f,     wrap_PI(M_2PI),     accuracy);
+    EXPECT_NEAR(0,       wrap_PI(M_PI * 10), accuracy);
+}
+
+TEST(MathWrapTest, Angle2PI)
+{
+    const float accuracy = 1.0e-5;
+
+    EXPECT_NEAR(M_PI, wrap_2PI(M_PI), accuracy);
+    EXPECT_NEAR(0.f,  wrap_2PI(M_2PI), accuracy);
+    EXPECT_NEAR(0.f,  wrap_2PI(M_PI * 10), accuracy);
+    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);
+}
+
 AP_GTEST_MAIN()