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

AP_Math: Replace the constrain_* functions by a single template 

Besides being simpler this reduces ~4k in the binary size for PX4.
This commit is contained in:
dgrat 2016-04-16 11:58:58 +02:00 committed by Lucas De Marchi
parent 348b07609c
commit 174f899a29
2 changed files with 25 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Math/AP_Math.cpp
View File

@ -131,3 +131,20 @@ 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);
template <class T>
T constrain_value(const T amt, const T low, const T high)
{
// the check for NaN as a float prevents propogation of floating point
// errors through any function that uses constrain_float(). The normal
// float semantics already handle -Inf and +Inf
if (isnan(amt)) {
return (low + high) * 0.5f;
}
return amt < low ? low : (amt > high ? high : amt);
}
template int constrain_value<int>(const int amt, const int low, const int high);
template short constrain_value<short>(const short amt, const short low, const short high);
template float constrain_value<float>(const float amt, const float low, const float high);
template double constrain_value<double>(const double amt, const double low, const double high);

28
libraries/AP_Math/AP_Math.h
View File

@ -91,28 +91,16 @@ float wrap_PI(const T radian);
template <class T>
float wrap_2PI(const T radian);
// constrain a value
static inline float constrain_float(float amt, float low, float high)
{
// the check for NaN as a float prevents propogation of
// floating point errors through any function that uses
// constrain_float(). The normal float semantics already handle -Inf
// and +Inf
if (isnan(amt)) {
return (low+high)*0.5f;
}
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
/*
* Constrain a value to be within the range: low and high
*/
template <class T>
T constrain_value(const T amt, const T low, const T high);
// 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)));
}
auto const constrain_float = &constrain_value<float>;
auto const constrain_int16 = &constrain_value<int16_t>;
auto const constrain_int32 = &constrain_value<int32_t>;
// constrain a int32_t value
static inline int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
//matrix algebra
bool inverse(float x[], float y[], uint16_t dim);