Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Math: stop returning float for integer wrap_180/wrap_360 etc

This commit is contained in:
Peter Barker 2019-09-17 19:50:00 +10:00 committed by Peter Barker
parent 84d999142f
commit 5692f3d79e
3 changed files with 52 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduSub/control_surface.cpp
View File

@ -49,7 +49,7 @@ void Sub::surface_run()
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate);
// set target climb rate
float cmb_rate = constrain_float(abs(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up());
float cmb_rate = constrain_float(fabsf(wp_nav.get_default_speed_up()), 1, pos_control.get_max_speed_up());
// record desired climb rate for logging
desired_climb_rate = cmb_rate;

70
libraries/AP_Math/AP_Math.cpp
View File

@ -32,7 +32,12 @@ is_equal(const Arithmetic1 v_1, const Arithmetic2 v_2)
return fabs(v_1 - v_2) < std::numeric_limits<double>::epsilon();
}
#endif
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wabsolute-value"
// clang doesn't realise we catch the double case above and warns
// about loss of precision here.
return fabsf(v_1 - v_2) < std::numeric_limits<float>::epsilon();
#pragma clang diagnostic pop
}
template bool is_equal<int>(const int v_1, const int v_2);
@ -124,60 +129,53 @@ float throttle_curve(float thr_mid, float alpha, float thr_in)
}
template <typename T>
float wrap_180(const T angle, float unit_mod)
T wrap_180(const T angle, T unit_mod)
{
auto res = wrap_360(angle, unit_mod);
if (res > 180.f * unit_mod) {
res -= 360.f * unit_mod;
if (res > T(180) * unit_mod) {
res -= T(360) * unit_mod;
}
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 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 float wrap_180<double>(const double angle, float unit_mod);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
template double wrap_180<double>(const double angle, double unit_mod);
#endif
template <typename T>
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
float wrap_360(const float angle, float unit_mod)
{
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<long>(const long 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 <typename 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);
const auto ang_360 = float(360) * unit_mod;
auto res = fmodf(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<long>(const long 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 <typename T>
auto wrap_360_cd(const T angle) -> decltype(wrap_360(angle, 100.f))
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
double wrap_360(const double angle, double unit_mod)
{
return wrap_360(angle, 100.f);
const auto ang_360 = double(360) * unit_mod;
auto res = fmod(angle, ang_360);
if (res < 0) {
res += ang_360;
}
return res;
}
#endif
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<long>(const long 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));
int wrap_360(const int angle, int unit_mod)
{
const int ang_360 = 360 * unit_mod;
int res = angle % ang_360;
if (res < 0) {
res += ang_360;
}
return res;
}
template <typename T>
float wrap_PI(const T radian)

27
libraries/AP_Math/AP_Math.h
View File

@ -99,27 +99,34 @@ bool inverse(float x[], float y[], uint16_t dim) WARN_IF_UNUSED;
* 100 == centi.
*/
template <typename T>
float wrap_180(const T angle, float unit_mod = 1);
T wrap_180(const T angle, T unit_mod = T(1));
/*
* Wrap an angle in centi-degrees. See wrap_180().
*/
template <typename T>
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f));
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
/*
* 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.
*/
template <typename T>
float wrap_360(const T angle, float unit_mod = 1);
float wrap_360(const float angle, float unit_mod = 1);
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
double wrap_360(const double angle, double unit_mod = 1);
#endif
int wrap_360(const int angle, int unit_mod = 1);
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)); }
#ifdef ALLOW_DOUBLE_MATH_FUNCTIONS
inline double wrap_360_cd(const double angle) { return wrap_360(angle, double(100)); }
#endif
/*
* Wrap an angle in centi-degrees. See wrap_360().
*/
template <typename 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)