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

AP_Math: change fast_atan2 to use atan2f on fast CPUs

This commit is contained in:
Jonathan Challinger 2015-02-02 15:02:39 -08:00 committed by Randy Mackay
parent b64077ac2e
commit 5f7480b740
1 changed files with 41 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
libraries/AP_Math/AP_Math.cpp
View File

@ -38,40 +38,47 @@ float fast_atan(float v)
return (v*(1.6867629106f + v2*0.4378497304f)/(1.6867633134f + v2)); return (v*(1.6867629106f + v2*0.4378497304f)/(1.6867633134f + v2));
} }
#define FAST_ATAN2_PIBY2_FLOAT 1.5707963f #if HAL_CPU_CLASS < HAL_CPU_CLASS_75 || CONFIG_HAL_BOARD == HAL_BOARD_AVR_SITL
// fast_atan2 - faster version of atan2 #define FAST_ATAN2_PIBY2_FLOAT 1.5707963f
// 126 us on AVR cpu vs 199 for regular atan2 // fast_atan2 - faster version of atan2
// absolute error is < 0.005 radians or 0.28 degrees // 126 us on AVR cpu vs 199 for regular atan2
// origin source: https://gist.github.com/volkansalma/2972237/raw/ // absolute error is < 0.005 radians or 0.28 degrees
float fast_atan2(float y, float x) // origin source: https://gist.github.com/volkansalma/2972237/raw/
{ float fast_atan2(float y, float x)
if (x == 0.0f) { {
if (y > 0.0f) { if (x == 0.0f) {
return FAST_ATAN2_PIBY2_FLOAT; if (y > 0.0f) {
} return FAST_ATAN2_PIBY2_FLOAT;
if (y == 0.0f) { }
return 0.0f; if (y == 0.0f) {
} return 0.0f;
return -FAST_ATAN2_PIBY2_FLOAT; }
} return -FAST_ATAN2_PIBY2_FLOAT;
float atan; }
float z = y/x; float atan;
if (fabs( z ) < 1.0f) { float z = y/x;
atan = z / (1.0f + 0.28f * z * z); if (fabs( z ) < 1.0f) {
if (x < 0.0f) { atan = z / (1.0f + 0.28f * z * z);
if (y < 0.0f) { if (x < 0.0f) {
return atan - PI; if (y < 0.0f) {
} return atan - PI;
return atan + PI; }
} return atan + PI;
} else { }
atan = FAST_ATAN2_PIBY2_FLOAT - (z / (z * z + 0.28f)); } else {
if (y < 0.0f) { atan = FAST_ATAN2_PIBY2_FLOAT - (z / (z * z + 0.28f));
return atan - PI; if (y < 0.0f) {
} return atan - PI;
} }
return atan; }
} return atan;
}
#else
float fast_atan2(float y, float x)
{
return atan2f(y,x);
}
#endif
#if ROTATION_COMBINATION_SUPPORT #if ROTATION_COMBINATION_SUPPORT
// find a rotation that is the combination of two other // find a rotation that is the combination of two other