From 975804fa357b3edb829de47c9a1e57e08c28fc00 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 18 Sep 2019 13:13:51 +1000 Subject: [PATCH] AP_Math: remove unit_mod concept from wrap functions devcall decided it would be clearer to have non-shared implementation for the _cd variants --- libraries/AP_Math/AP_Math.cpp | 92 +++++++++++++++++++++++++++-------- libraries/AP_Math/AP_Math.h | 22 ++++----- 2 files changed, 82 insertions(+), 32 deletions(-) diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index f0baef8a4b..6d2def0779 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -129,54 +129,106 @@ float throttle_curve(float thr_mid, float alpha, float thr_in) } template -T wrap_180(const T angle, T unit_mod) +T wrap_180(const T angle) { - auto res = wrap_360(angle, unit_mod); - if (res > T(180) * unit_mod) { - res -= T(360) * unit_mod; + auto res = wrap_360(angle); + if (res > T(180)) { + res -= T(360); + } + return res; +} + +template +T wrap_180_cd(const T angle) +{ + auto res = wrap_360_cd(angle); + if (res > T(18000)) { + res -= T(36000); + } + return res; +} + +template int wrap_180(const int angle); +template short wrap_180(const short angle); +template float wrap_180(const float angle); +#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS +template double wrap_180(const double angle); +#endif + +template int wrap_180_cd(const int angle); +template long wrap_180_cd(const long angle); +template short wrap_180_cd(const short angle); +template float wrap_180_cd(const float angle); +#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS +template double wrap_180_cd(const double angle); +#endif + +float wrap_360(const float angle) +{ + float res = fmodf(angle, 360.0f); + if (res < 0) { + res += 360.0f; } return res; } -template int wrap_180(const int angle, int unit_mod); -template short wrap_180(const short angle, short unit_mod); -template float wrap_180(const float angle, float unit_mod); #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS -template double wrap_180(const double angle, double unit_mod); +double wrap_360(const double angle) +{ + double res = fmod(angle, 360.0); + if (res < 0) { + res += 360.0; + } + return res; +} #endif -float wrap_360(const float angle, float unit_mod) +int wrap_360(const int angle) { - const auto ang_360 = float(360) * unit_mod; - auto res = fmodf(angle, ang_360); + int res = angle % 360; if (res < 0) { - res += ang_360; + res += 360; + } + return res; +} + +float wrap_360_cd(const float angle) +{ + float res = fmodf(angle, 36000.0f); + if (res < 0) { + res += 36000.0f; } return res; } #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS -double wrap_360(const double angle, double unit_mod) +double wrap_360_cd(const double angle) { - const auto ang_360 = double(360) * unit_mod; - auto res = fmod(angle, ang_360); + double res = fmod(angle, 36000.0); if (res < 0) { - res += ang_360; + res += 36000.0; } return res; } #endif -int wrap_360(const int angle, int unit_mod) +int wrap_360_cd(const int angle) { - const int ang_360 = 360 * unit_mod; - int res = angle % ang_360; + int res = angle % 36000; if (res < 0) { - res += ang_360; + res += 36000; } return res; } +long wrap_360_cd(const long angle) +{ + long res = angle % 36000; + if (res < 0) { + res += 36000; + } + return res; +} template float wrap_PI(const T radian) { diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 40d38a0f9c..7f480b295c 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -99,32 +99,30 @@ bool inverse(float x[], float y[], uint16_t dim) WARN_IF_UNUSED; * 100 == centi. */ template -T wrap_180(const T angle, T unit_mod = T(1)); +T wrap_180(const T angle); /* * Wrap an angle in centi-degrees. See wrap_180(). */ -inline float wrap_180_cd(const float angle) { return wrap_180(angle, float(100)); } -inline int32_t wrap_180_cd(const int32_t angle) { return wrap_180(int(angle), int(100)); } -#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS -inline double wrap_180_cd(const double angle) { return wrap_180(angle, double(100)); } -#endif +template +T wrap_180_cd(const T angle); /* * 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_360(const float angle, float unit_mod = 1); +float wrap_360(const float angle); #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS -double wrap_360(const double angle, double unit_mod = 1); +double wrap_360(const double angle); #endif -int wrap_360(const int angle, int unit_mod = 1); +int wrap_360(const int angle); -inline int32_t wrap_360_cd(const int32_t angle) { return wrap_360(int(angle), int(100)); } -inline float wrap_360_cd(const float angle) { return wrap_360(angle, float(100)); } +int wrap_360_cd(const int angle); +long wrap_360_cd(const long angle); +float wrap_360_cd(const float angle); #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS -inline double wrap_360_cd(const double angle) { return wrap_360(angle, double(100)); } +double wrap_360_cd(const double angle); #endif