aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: remove unit_mod concept from wrap functions

Browse Source 
devcall decided it would be clearer to have non-shared implementation
for the _cd variants
This commit is contained in:
Peter Barker 2019-09-18 13:13:51 +10:00 committed by Peter Barker
parent 952d4d2cbc
commit 975804fa35
2 changed files with 82 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
libraries/AP_Math/AP_Math.cpp
View File

@ -129,54 +129,106 @@ float throttle_curve(float thr_mid, float alpha, float thr_in)
}
template <typename T>
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 int wrap_180<int>(const int angle, int unit_mod);
template short wrap_180<short>(const short angle, short unit_mod);
template float wrap_180<float>(const float angle, float unit_mod);
template <typename T>
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<int>(const int angle);
template short wrap_180<short>(const short angle);
template float wrap_180<float>(const float angle);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
template double wrap_180<double>(const double angle, double unit_mod);
template double wrap_180<double>(const double angle);
#endif
float wrap_360(const float angle, float unit_mod)
template int wrap_180_cd<int>(const int angle);
template long wrap_180_cd<long>(const long angle);
template short wrap_180_cd<short>(const short angle);
template float wrap_180_cd<float>(const float angle);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
template double wrap_180_cd<double>(const double angle);
#endif
float wrap_360(const float angle)
{
const auto ang_360 = float(360) * unit_mod;
auto res = fmodf(angle, ang_360);
float res = fmodf(angle, 360.0f);
if (res < 0) {
res += ang_360;
res += 360.0f;
}
return res;
}
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
double wrap_360(const double angle, double unit_mod)
double wrap_360(const double angle)
{
const auto ang_360 = double(360) * unit_mod;
auto res = fmod(angle, ang_360);
double res = fmod(angle, 360.0);
if (res < 0) {
res += ang_360;
res += 360.0;
}
return res;
}
#endif
int wrap_360(const int angle, int unit_mod)
int wrap_360(const int angle)
{
const int ang_360 = 360 * unit_mod;
int res = 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_cd(const double angle)
{
double res = fmod(angle, 36000.0);
if (res < 0) {
res += 36000.0;
}
return res;
}
#endif
int wrap_360_cd(const int angle)
{
int res = angle % 36000;
if (res < 0) {
res += 36000;
}
return res;
}
long wrap_360_cd(const long angle)
{
long res = angle % 36000;
if (res < 0) {
res += 36000;
}
return res;
}
template <typename T>
float wrap_PI(const T radian)
{

22
libraries/AP_Math/AP_Math.h
View File

@ -99,32 +99,30 @@ bool inverse(float x[], float y[], uint16_t dim) WARN_IF_UNUSED;
* 100 == centi.
*/
template <typename T>
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 <typename T>
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