From 76362caee02b70361a6988cd0d70ac1c4366c68b Mon Sep 17 00:00:00 2001 From: dgrat Date: Wed, 27 Apr 2016 11:35:18 +0200 Subject: [PATCH] AP_Math: Replace wrap_* functions with template versions --- APMrover2/test.cpp | 2 +- AntennaTracker/Log.cpp | 3 +- AntennaTracker/servos.cpp | 2 +- ArduCopter/Log.cpp | 2 +- ArduCopter/control_autotune.cpp | 2 +- ArduCopter/control_guided.cpp | 4 +- ArduPlane/test.cpp | 2 +- Tools/Replay/Replay.cpp | 2 +- libraries/AP_Math/AP_Math.cpp | 88 ++++++++++++++++++++++++ libraries/AP_Math/AP_Math.h | 43 +++++++++--- libraries/AP_Math/location.cpp | 98 --------------------------- libraries/AP_Math/tests/test_math.cpp | 64 ++++++++--------- 12 files changed, 161 insertions(+), 151 deletions(-) diff --git a/APMrover2/test.cpp b/APMrover2/test.cpp index 6f97b0167c..d50174c582 100644 --- a/APMrover2/test.cpp +++ b/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); diff --git a/AntennaTracker/Log.cpp b/AntennaTracker/Log.cpp index 54f4c8a531..d0f7483323 100644 --- a/AntennaTracker/Log.cpp +++ b/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 diff --git a/AntennaTracker/servos.cpp b/AntennaTracker/servos.cpp index cf8bfedc55..78a614abee 100644 --- a/AntennaTracker/servos.cpp +++ b/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)); diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index e63d1cbc4f..5790586f47 100644 --- a/ArduCopter/Log.cpp +++ b/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 diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index dae9e0d1e4..4e2b7aa0d9 100644 --- a/ArduCopter/control_autotune.cpp +++ b/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 { diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index 4389396293..f988ab4df8 100644 --- a/ArduCopter/control_guided.cpp +++ b/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()); diff --git a/ArduPlane/test.cpp b/ArduPlane/test.cpp index 9facd33af1..af69ad37df 100644 --- a/ArduPlane/test.cpp +++ b/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); diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 891d61be45..148ef1fe90 100644 --- a/Tools/Replay/Replay.cpp +++ b/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); diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index a767d08f3a..49dc3b8f02 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/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 +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(angle) + ang_180, ang_360); + if (res < 0 || is_zero(res)) { + res += ang_180; + } else { + res -= ang_180; + } + return res; +} + +template float wrap_180(const int angle, float unit_mod); +template float wrap_180(const short angle, float unit_mod); +template float wrap_180(const float angle, float unit_mod); +template float wrap_180(const double angle, float unit_mod); + +template +auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f)) +{ + return wrap_180(angle, 100.f); +} + +template auto wrap_180_cd(const float angle) -> decltype(wrap_180(angle, 100.f)); +template auto wrap_180_cd(const int angle) -> decltype(wrap_180(angle, 100.f)); +template auto wrap_180_cd(const short angle) -> decltype(wrap_180(angle, 100.f)); +template auto wrap_180_cd(const double angle) -> decltype(wrap_360(angle, 100.f)); + +template +float wrap_360(const T angle, float unit_mod) +{ + const float ang_360 = 360.f * unit_mod; + float res = fmodf(static_cast(angle), ang_360); + if (res < 0) { + res += ang_360; + } + return res; +} + +template float wrap_360(const int angle, float unit_mod); +template float wrap_360(const short angle, float unit_mod); +template float wrap_360(const float angle, float unit_mod); +template float wrap_360(const double angle, float unit_mod); + +template +auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f)) +{ + return wrap_360(angle, 100.f); +} + +template auto wrap_360_cd(const float angle) -> decltype(wrap_360(angle, 100.f)); +template auto wrap_360_cd(const int angle) -> decltype(wrap_360(angle, 100.f)); +template auto wrap_360_cd(const short angle) -> decltype(wrap_360(angle, 100.f)); +template auto wrap_360_cd(const double angle) -> decltype(wrap_360(angle, 100.f)); + +template +float wrap_PI(const T radian) +{ + float res = fmod(static_cast(radian) + M_PI, M_2PI); + if (res < 0 || is_zero(res)) { + res += M_PI; + } else { + res -= M_PI; + } + return res; +} + +template float wrap_PI(const int radian); +template float wrap_PI(const short radian); +template float wrap_PI(const float radian); +template float wrap_PI(const double radian); + +template +float wrap_2PI(const T radian) +{ + float res = fmodf(static_cast(radian), M_2PI); + if (res < 0) { + res += M_2PI; + } + return res; +} + +template float wrap_2PI(const int radian); +template float wrap_2PI(const short radian); +template float wrap_2PI(const float radian); +template float wrap_2PI(const double radian); diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 58c1596d92..96dce69340 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/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 +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 +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 +float wrap_360(const T angle, float unit_mod = 1); + +/* + * Wrap an angle in centi-degrees. See wrap_360(). + */ +template +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 +float wrap_PI(const T radian); + +/* + * wrap an angle in radians to 0..2PI + */ +template +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))); diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index c6ba3c195a..e15e2c1f76 100644 --- a/libraries/AP_Math/location.cpp +++ b/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 */ diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp index e37689c52c..d38a2a6810 100644 --- a/libraries/AP_Math/tests/test_math.cpp +++ b/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(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_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)); } 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(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_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(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()