I am calculating angles from a 3-axis accelerometer, but my compiler doesn\'t have a atan or atan2 function. It has a reserved memory slot, but it calls a function i can\'t
The actual implementations of the math functions (or stubs to the HWFPU if one exists) should be in libm. With GCC this is indicated by passing -lm
to the compiler, but I don't know how it is done with your specific tools.
The following code uses a rational approximation to get the arctangent normalized to the [0 1) interval (you can multiply the result by Pi/2 to get the real arctangent)
normalized_atan(x) ~ (b x + x^2) / (1 + 2 b x + x^2)
where b = 0.596227
The maximum error is 0.1620º
#include <stdint.h>
#include <math.h>
// Approximates atan(x) normalized to the [-1,1] range
// with a maximum error of 0.1620 degrees.
float normalized_atan( float x )
{
static const uint32_t sign_mask = 0x80000000;
static const float b = 0.596227f;
// Extract the sign bit
uint32_t ux_s = sign_mask & (uint32_t &)x;
// Calculate the arctangent in the first quadrant
float bx_a = ::fabs( b * x );
float num = bx_a + x * x;
float atan_1q = num / ( 1.f + bx_a + num );
// Restore the sign bit
uint32_t atan_2q = ux_s | (uint32_t &)atan_1q;
return (float &)atan_2q;
}
// Approximates atan2(y, x) normalized to the [0,4) range
// with a maximum error of 0.1620 degrees
float normalized_atan2( float y, float x )
{
static const uint32_t sign_mask = 0x80000000;
static const float b = 0.596227f;
// Extract the sign bits
uint32_t ux_s = sign_mask & (uint32_t &)x;
uint32_t uy_s = sign_mask & (uint32_t &)y;
// Determine the quadrant offset
float q = (float)( ( ~ux_s & uy_s ) >> 29 | ux_s >> 30 );
// Calculate the arctangent in the first quadrant
float bxy_a = ::fabs( b * x * y );
float num = bxy_a + y * y;
float atan_1q = num / ( x * x + bxy_a + num );
// Translate it to the proper quadrant
uint32_t uatan_2q = (ux_s ^ uy_s) | (uint32_t &)atan_1q;
return q + (float &)uatan_2q;
}
In case you need more precision, there is a 3rd order rational function:
normalized_atan(x) ~ ( c x + x^2 + x^3) / ( 1 + (c + 1) x + (c + 1) x^2 + x^3)
where c = (1 + sqrt(17)) / 8
which has a maximum approximation error of 0.00811º
Its not very difficult to implement your own arctan2
. Convert arctan2
to arctan
using this formula. And you can then calculate arctan
using this infinite series. If you sum sufficient number of terms of this infinite series, you will get very close to what the library function arctan2
does.
Here is one similar implementation for exp() that you could use as a reference.
There is an open source atan implementation here.