Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-31 04:58:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: Move simple math function implementations to header for better

Browse Source 
compile time optimization

Functions like sq() are better moved to the header file as inline.
Compiler can then optimize these out when used in code, this saves cpu
cycles with stack push, pop during function calls.
This commit is contained in:
Valmantas Palikša 2015-03-21 10:48:46 +02:00 committed by Andrew Tridgell
parent fc2c94f78f
commit d4daf19151
2 changed files with 49 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AP_Math/AP_Math.cpp
View File

@ -70,59 +70,3 @@ enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *fou
}
#endif
// constrain a value
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 int16_t value
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
// constrain a int32_t value
int32_t constrain_int32(int32_t amt, int32_t low, int32_t high) {
return ((amt)<(low)?(low):((amt)>(high)?(high):(amt)));
}
// degrees -> radians
float radians(float deg) {
return deg * DEG_TO_RAD;
}
// radians -> degrees
float degrees(float rad) {
return rad * RAD_TO_DEG;
}
// square
float sq(float v) {
return v*v;
}
// 2D vector length
float pythagorous2(float a, float b) {
return sqrtf(sq(a)+sq(b));
}
// 3D vector length
float pythagorous3(float a, float b, float c) {
return sqrtf(sq(a)+sq(b)+sq(c));
}
float maxf(float a, float b)
{
return (a>b?a:b);
}
float minf(float a, float b)
{
return (a<b?a:b);
}

60
libraries/AP_Math/AP_Math.h
View File

@ -173,25 +173,55 @@ void wgsecef2llh(const Vector3d &ecef, Vector3d &llh);
#endif
// constrain a value
float constrain_float(float amt, float low, float high);
int16_t constrain_int16(int16_t amt, int16_t low, int16_t high);
int32_t constrain_int32(int32_t amt, int32_t low, int32_t high);
// 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 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)));
}
// 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);
// degrees -> radians
float radians(float deg);
static inline float radians(float deg) {
return deg * DEG_TO_RAD;
}
// radians -> degrees
float degrees(float rad);
static inline float degrees(float rad) {
return rad * RAD_TO_DEG;
}
// square
float sq(float v);
static inline float sq(float v) {
return v*v;
}
// sqrt of sum of squares
float pythagorous2(float a, float b);
float pythagorous3(float a, float b, float c);
// 2D vector length
static inline float pythagorous2(float a, float b) {
return sqrtf(sq(a)+sq(b));
}
// 3D vector length
static inline float pythagorous3(float a, float b, float c) {
return sqrtf(sq(a)+sq(b)+sq(c));
}
#ifdef radians
#error "Build is including Arduino base headers"
@ -201,8 +231,16 @@ float pythagorous3(float a, float b, float c);
#define max(a,b) ((a)>(b)?(a):(b))
#define min(a,b) ((a)<(b)?(a):(b))
float maxf(float a, float b);
float minf(float a, float b);
static inline float maxf(float a, float b)
{
return (a>b?a:b);
}
static inline float minf(float a, float b)
{
return (a<b?a:b);
}
#undef INLINE
#endif // AP_MATH_H