AP_Math: Replace wrap_* functions with template versions

APMrover2/test.cpp

@@ -402,7 +402,7 @@ int8_t Rover::test_mag(uint8_t argc, const Menu::arg *argv)
             if (compass.healthy()) {
                 const Vector3f mag_ofs = compass.get_offsets();
                 const Vector3f mag = compass.get_field();
-                cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
+                cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
                                     (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                     (double)mag.x, (double)mag.y, (double)mag.z,
                                     (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);

AntennaTracker/Log.cpp

@@ -11,9 +11,8 @@ void Tracker::Log_Write_Attitude()
 {
     Vector3f targets;
     targets.y = nav_status.pitch * 100.0f;
-    targets.z = wrap_360_cd_float(nav_status.bearing * 100.0f);
+    targets.z = wrap_360_cd(nav_status.bearing * 100.0f);
     DataFlash.Log_Write_Attitude(ahrs, targets);
-
     DataFlash.Log_Write_EKF(ahrs,false);
     DataFlash.Log_Write_AHRS2(ahrs);
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL

AntennaTracker/servos.cpp

@@ -332,7 +332,7 @@ void Tracker::update_yaw_onoff_servo(float yaw)
 void Tracker::update_yaw_cr_servo(float yaw)
 {
     int32_t ahrs_yaw_cd = wrap_180_cd(ahrs.yaw_sensor);
-    float yaw_cd = wrap_180_cd_float(yaw*100.0f);
+    float yaw_cd = wrap_180_cd(yaw*100.0f);
     float err_cd = wrap_180_cd(yaw_cd - (float)ahrs_yaw_cd);
 
     channel_yaw.set_servo_out(g.pidYaw2Srv.get_pid(err_cd));

ArduCopter/Log.cpp

@@ -372,7 +372,7 @@ void Copter::Log_Write_Performance()
 void Copter::Log_Write_Attitude()
 {
     Vector3f targets = attitude_control.get_att_target_euler_cd();
-    targets.z = wrap_360_cd_float(targets.z);
+    targets.z = wrap_360_cd(targets.z);
     DataFlash.Log_Write_Attitude(ahrs, targets);
 
  #if OPTFLOW == ENABLED

ArduCopter/control_autotune.cpp

@@ -447,7 +447,7 @@ void Copter::autotune_attitude_control()
                 break;
             case AUTOTUNE_AXIS_YAW:
                 // request pitch to 20deg
-                attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd_float(direction_sign * autotune_target_angle + autotune_start_angle), false);
+                attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false);
                 break;
             }
         } else {

ArduCopter/control_guided.cpp

@@ -247,7 +247,7 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms)
     q.to_euler(guided_angle_state.roll_cd, guided_angle_state.pitch_cd, guided_angle_state.yaw_cd);
     guided_angle_state.roll_cd = ToDeg(guided_angle_state.roll_cd) * 100.0f;
     guided_angle_state.pitch_cd = ToDeg(guided_angle_state.pitch_cd) * 100.0f;
-    guided_angle_state.yaw_cd = wrap_180_cd_float(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
+    guided_angle_state.yaw_cd = wrap_180_cd(ToDeg(guided_angle_state.yaw_cd) * 100.0f);
 
     guided_angle_state.climb_rate_cms = climb_rate_cms;
     guided_angle_state.update_time_ms = millis();
@@ -535,7 +535,7 @@ void Copter::guided_angle_control_run()
     }
 
     // wrap yaw request
-    float yaw_in = wrap_180_cd_float(guided_angle_state.yaw_cd);
+    float yaw_in = wrap_180_cd(guided_angle_state.yaw_cd);
 
     // constrain climb rate
     float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabs(wp_nav.get_speed_down()), wp_nav.get_speed_up());

ArduPlane/test.cpp

@@ -442,7 +442,7 @@ int8_t Plane::test_mag(uint8_t argc, const Menu::arg *argv)
                 if (compass.healthy()) {
                     const Vector3f &mag_ofs = compass.get_offsets();
                     const Vector3f &mag = compass.get_field();
-                    cliSerial->printf("Heading: %d, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
+                    cliSerial->printf("Heading: %f, XYZ: %.0f, %.0f, %.0f,\tXYZoff: %6.2f, %6.2f, %6.2f\n",
                                         (wrap_360_cd(ToDeg(heading) * 100)) /100,
                                         (double)mag.x, (double)mag.y, (double)mag.z,
                                         (double)mag_ofs.x, (double)mag_ofs.y, (double)mag_ofs.z);

Tools/Replay/Replay.cpp

@@ -872,7 +872,7 @@ void Replay::log_check_solution(void)
 
     float roll_error  = degrees(fabsf(euler.x - check_state.euler.x));
     float pitch_error = degrees(fabsf(euler.y - check_state.euler.y));
-    float yaw_error = wrap_180_cd_float(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
+    float yaw_error = wrap_180_cd(100*degrees(fabsf(euler.z - check_state.euler.z)))*0.01f;
     float vel_error = (velocity - check_state.velocity).length();
     float pos_error = get_distance(check_state.pos, loc);
 

libraries/AP_Math/AP_Math.cpp

@@ -49,3 +49,91 @@ float linear_interpolate(float low_output, float high_output,
     return low_output + p * (high_output - low_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;
+    }
+    return res;
+}
+
+template float wrap_180<int>(const int angle, float unit_mod);
+template float wrap_180<short>(const short angle, float unit_mod);
+template float wrap_180<float>(const float angle, float unit_mod);
+template float wrap_180<double>(const double angle, float unit_mod);
+
+template <class T>
+auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
+{
+    return wrap_180(angle, 100.f);
+}
+
+template auto wrap_180_cd<float>(const float angle) -> decltype(wrap_180(angle, 100.f));
+template auto wrap_180_cd<int>(const int angle) -> decltype(wrap_180(angle, 100.f));
+template auto wrap_180_cd<short>(const short angle) -> decltype(wrap_180(angle, 100.f));
+template auto wrap_180_cd<double>(const double angle) -> decltype(wrap_360(angle, 100.f));
+
+template <class T>
+float wrap_360(const T angle, float unit_mod)
+{
+    const float ang_360 = 360.f * unit_mod;
+    float res = fmodf(static_cast<float>(angle), ang_360);
+    if (res < 0) {
+        res += ang_360;
+    }
+    return res;
+}
+
+template float wrap_360<int>(const int angle, float unit_mod);
+template float wrap_360<short>(const short angle, float unit_mod);
+template float wrap_360<float>(const float angle, float unit_mod);
+template float wrap_360<double>(const double angle, float unit_mod);
+
+template <class T>
+auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f))
+{
+    return wrap_360(angle, 100.f);
+}
+
+template auto wrap_360_cd<float>(const float angle) -> decltype(wrap_360(angle, 100.f));
+template auto wrap_360_cd<int>(const int angle) -> decltype(wrap_360(angle, 100.f));
+template auto wrap_360_cd<short>(const short angle) -> decltype(wrap_360(angle, 100.f));
+template auto wrap_360_cd<double>(const double angle) -> decltype(wrap_360(angle, 100.f));
+
+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;
+    }
+    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 <class T>
+float wrap_2PI(const T radian)
+{
+    float res = fmodf(static_cast<float>(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);

libraries/AP_Math/AP_Math.h

@@ -52,25 +52,45 @@ bool                    inverse4x4(float m[],float invOut[]);
 float* mat_mul(float *A, float *B, uint8_t n);
 
 /*
-  wrap an angle in centi-degrees
+ * Constrain an angle to be within the range: -180 to 180 degrees. The second
+ * parameter changes the units. Default: 1 == degrees, 10 == dezi,
+ * 100 == centi.
  */
-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);
+template <class T>
+float wrap_180(const T angle, float unit_mod = 1);
 
 /*
-  wrap an angle defined in radians to -PI ~ PI (equivalent to +- 180 degrees)
+ * Wrap an angle in centi-degrees. See wrap_180().
  */
-float wrap_PI(float angle_in_radians);
+template <class T>
+auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
 
 /*
-  wrap an angle defined in radians to the interval [0,2*PI)
+ * Constrain an euler angle to be within the range: 0 to 360 degrees. The
+ * second parameter changes the units. Default: 1 == degrees, 10 == dezi,
+ * 100 == centi.
  */
-float wrap_2PI(float angle);
+template <class T>
+float wrap_360(const T angle, float unit_mod = 1);
+
+/*
+ * Wrap an angle in centi-degrees. See wrap_360().
+ */
+template <class T>
+auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f));
+
+/*
+  wrap an angle in radians to -PI ~ PI (equivalent to +- 180 degrees)
+ */
+template <class T>
+float wrap_PI(const T radian);
+
+/*
+ * wrap an angle in radians to 0..2PI
+ */
+template <class T>
+float wrap_2PI(const T radian);
 
-// constrain a value
 // constrain a value
 static inline float constrain_float(float amt, float low, float high)
 {
@@ -83,6 +103,7 @@ static inline float constrain_float(float amt, float low, float high)
 	}
 	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
 }
+
 // constrain a int16_t value
 static inline int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
 	return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));

libraries/AP_Math/location.cpp

@@ -149,104 +149,6 @@ Vector2f location_diff(const struct Location &loc1, const struct Location &loc2)
                     (loc2.lng - loc1.lng) * LOCATION_SCALING_FACTOR * longitude_scale(loc1));
 }
 
-/*
-  wrap an angle in centi-degrees to 0..35999
- */
-int32_t wrap_360_cd(int32_t error)
-{
-    if (error > 360000 || error < -360000) {
-        // for very large numbers use modulus
-        error = error % 36000;
-    }
-    while (error >= 36000) error -= 36000;
-    while (error < 0) error += 36000;
-    return error;
-}
-
-/*
-  wrap an angle in centi-degrees to -18000..18000
- */
-int32_t wrap_180_cd(int32_t error)
-{
-    if (error > 360000 || error < -360000) {
-        // for very large numbers use modulus
-        error = error % 36000;
-    }
-    while (error > 18000) { error -= 36000; }
-    while (error < -18000) { error += 36000; }
-    return error;
-}
-
-/*
-  wrap an angle in centi-degrees to 0..35999
- */
-float wrap_360_cd_float(float angle)
-{
-    if (angle >= 72000.0f || angle < -36000.0f) {
-        // for larger number use fmodulus
-        angle = fmod(angle, 36000.0f);
-    }
-    if (angle >= 36000.0f) angle -= 36000.0f;
-    if (angle < 0.0f) angle += 36000.0f;
-    return angle;
-}
-
-/*
-  wrap an angle in centi-degrees to -18000..18000
- */
-float wrap_180_cd_float(float angle)
-{
-    if (angle > 54000.0f || angle < -54000.0f) {
-        // for large numbers use modulus
-        angle = fmod(angle,36000.0f);
-    }
-    if (angle > 18000.0f) { angle -= 36000.0f; }
-    if (angle < -18000.0f) { angle += 36000.0f; }
-    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)
- */
-float wrap_PI(float angle_in_radians)
-{
-    if (angle_in_radians > 10*M_PI || angle_in_radians < -10*M_PI) {
-        // for very large numbers use modulus
-        angle_in_radians = fmodf(angle_in_radians, 2*M_PI);
-    }
-    while (angle_in_radians > M_PI) angle_in_radians -= 2*M_PI;
-    while (angle_in_radians < -M_PI) angle_in_radians += 2*M_PI;
-    return angle_in_radians;
-}
-
-/*
- * wrap an angle in radians to 0..2PI
- */
-float wrap_2PI(float angle)
-{
-    if (angle > 10*M_PI || angle < -10*M_PI) {
-        // for very large numbers use modulus
-        angle = fmodf(angle, 2*M_PI);
-    }
-    while (angle > 2*M_PI) angle -= 2*M_PI;
-    while (angle < 0) angle += 2*M_PI;
-    return angle;
-}
-
 /*
   return true if lat and lng match. Ignores altitude and options
  */

libraries/AP_Math/tests/test_math.cpp

@@ -183,24 +183,24 @@ TEST(MathWrapTest, Angle180)
         }
     }
 
-    EXPECT_EQ(4500.f,   wrap_180_cd_float(4500.f));
-    EXPECT_EQ(9000.f,   wrap_180_cd_float(9000.f));
-    EXPECT_EQ(18000.f,  wrap_180_cd_float(18000.f));
-    EXPECT_EQ(-17990.f, wrap_180_cd_float(18010.f));
-    EXPECT_EQ(-9000.f,  wrap_180_cd_float(27000.f));
-    EXPECT_EQ(0.f,      wrap_180_cd_float(36000.f));
-    EXPECT_EQ(0.f,      wrap_180_cd_float(72000.f));
-    EXPECT_EQ(0.f,      wrap_180_cd_float(360000.f));
-    EXPECT_EQ(0.f,      wrap_180_cd_float(720000.f));
-    EXPECT_EQ(0.f,      wrap_180_cd_float(-3600000000.f));
-
-    EXPECT_EQ(-4500.f, wrap_180_cd_float(-4500.f));
-    EXPECT_EQ(-9000.f, wrap_180_cd_float(-9000.f));
-    EXPECT_EQ(18000.f, wrap_180_cd_float(-18000.f));
-    EXPECT_EQ(17990.f, wrap_180_cd_float(-18010.f));
-    EXPECT_EQ(9000.f,  wrap_180_cd_float(-27000.f));
-    EXPECT_EQ(0.f,     wrap_180_cd_float(-36000.f));
-    EXPECT_EQ(0.f,     wrap_180_cd_float(-72000.f));
+    EXPECT_EQ(4500.f,   wrap_180_cd(4500.f));
+    EXPECT_EQ(9000.f,   wrap_180_cd(9000.f));
+    EXPECT_EQ(18000.f,  wrap_180_cd(18000.f));
+    EXPECT_EQ(-17990.f, wrap_180_cd(18010.f));
+    EXPECT_EQ(-9000.f,  wrap_180_cd(27000.f));
+    EXPECT_EQ(0.f,      wrap_180_cd(36000.f));
+    EXPECT_EQ(0.f,      wrap_180_cd(72000.f));
+    EXPECT_EQ(0.f,      wrap_180_cd(360000.f));
+    EXPECT_EQ(0.f,      wrap_180_cd(720000.f));
+    EXPECT_EQ(0.f,      wrap_180_cd(-3600000000.f));
+
+    EXPECT_EQ(-4500.f, wrap_180_cd(-4500.f));
+    EXPECT_EQ(-9000.f, wrap_180_cd(-9000.f));
+    EXPECT_EQ(18000.f, wrap_180_cd(-18000.f));
+    EXPECT_EQ(17990.f, wrap_180_cd(-18010.f));
+    EXPECT_EQ(9000.f,  wrap_180_cd(-27000.f));
+    EXPECT_EQ(0.f,     wrap_180_cd(-36000.f));
+    EXPECT_EQ(0.f,     wrap_180_cd(-72000.f));
 }
 
 TEST(MathWrapTest, Angle360)
@@ -222,22 +222,22 @@ TEST(MathWrapTest, Angle360)
         }
     }
 
-    EXPECT_EQ(4500.f,  wrap_360_cd_float(4500.f));
-    EXPECT_EQ(9000.f,  wrap_360_cd_float(9000.f));
-    EXPECT_EQ(18000.f, wrap_360_cd_float(18000.f));
-    EXPECT_EQ(27000.f, wrap_360_cd_float(27000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(36000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(72000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(360000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(720000.f));
-    EXPECT_EQ( 0.f,    wrap_360_cd_float(-3600000000.f));
-
-    EXPECT_EQ(31500.f, wrap_360_cd_float(-4500.f));
-    EXPECT_EQ(27000.f, wrap_360_cd_float(-9000.f));
-    EXPECT_EQ(18000.f, wrap_360_cd_float(-18000.f));
-    EXPECT_EQ(9000.f,  wrap_360_cd_float(-27000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(-36000.f));
-    EXPECT_EQ(0.f,     wrap_360_cd_float(-72000.f));
+    EXPECT_EQ(4500.f,  wrap_360_cd(4500.f));
+    EXPECT_EQ(9000.f,  wrap_360_cd(9000.f));
+    EXPECT_EQ(18000.f, wrap_360_cd(18000.f));
+    EXPECT_EQ(27000.f, wrap_360_cd(27000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(36000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(72000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(360000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(720000.f));
+    EXPECT_EQ( 0.f,    wrap_360_cd(-3600000000.f));
+
+    EXPECT_EQ(31500.f, wrap_360_cd(-4500.f));
+    EXPECT_EQ(27000.f, wrap_360_cd(-9000.f));
+    EXPECT_EQ(18000.f, wrap_360_cd(-18000.f));
+    EXPECT_EQ(9000.f,  wrap_360_cd(-27000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(-36000.f));
+    EXPECT_EQ(0.f,     wrap_360_cd(-72000.f));
 }
 
 AP_GTEST_MAIN()