Prev: zero padding
Next: Call For Manuscripts: International Journal of Signal Processing (IJSP)
From: dewagter on 25 Feb 2010 07:29
Here is a possible fixed point version of the above code. The result is
accurate within +/-1 degree. If +/-5 degree errors are acceptable then the
r^3 term can be left out with coeff_1b becoming -45. This function returns
degrees and not radians.

-------------------------------------------------------------------

#define MULTIPLY_FP_RESOLUTION_BITS 15

int16_t atan2_fp(int16_t y_fp, int16_t x_fp)
{
int32_t coeff_1 = 45;
int32_t coeff_1b = -56; // 56.24;
int32_t coeff_1c = 11; // 11.25
int16_t coeff_2 = 135;

int16_t angle = 0;

int32_t r;
int32_t r3;

int16_t y_abs_fp = y_fp;
if (y_abs_fp < 0)
y_abs_fp = -y_abs_fp;

if (y_fp == 0)
{
if (x_fp >= 0)
{
angle = 0;
}
else
{
angle = 180;
}
}
else if (x_fp >= 0)
{
r = (((int32_t)(x_fp - y_abs_fp)) << MULTIPLY_FP_RESOLUTION_BITS) /
((int32_t)(x_fp + y_abs_fp));

r3 = r * r;
r3 = r3 >> MULTIPLY_FP_RESOLUTION_BITS;
r3 *= r;
r3 = r3 >> MULTIPLY_FP_RESOLUTION_BITS;
r3 *= coeff_1c;
angle = (int16_t) ( coeff_1 + ((coeff_1b * r + r3) >>
MULTIPLY_FP_RESOLUTION_BITS) );
}
else
{
r = (((int32_t)(x_fp + y_abs_fp)) << MULTIPLY_FP_RESOLUTION_BITS) /
((int32_t)(y_abs_fp - x_fp));
r3 = r * r;
r3 = r3 >> MULTIPLY_FP_RESOLUTION_BITS;
r3 *= r;
r3 = r3 >> MULTIPLY_FP_RESOLUTION_BITS;
r3 *= coeff_1c;
angle = coeff_2 + ((int16_t) (((coeff_1b * r + r3) >>
MULTIPLY_FP_RESOLUTION_BITS)) );
}

if (y_fp < 0)
return (-angle); // negate if in quad III or IV
else
return (angle);
}



From: Vladimir Vassilevsky on 25 Feb 2010 10:38


dewagter wrote:
> Here is a possible fixed point version of the above code. The result is
> accurate within +/-1 degree.

:))))))) +/- 1 km

> If +/-5 degree errors are acceptable then the
> r^3 term can be left out with coeff_1b becoming -45. This function returns
> degrees and not radians.

The best way to approximate atan2() is LUT with trivial linear
interpolation. It takes LUT as small as 32 elements to get the 16-bit
accurate result.


Vladimir Vassilevsky
DSP and Mixed Signal Design Consultant
http://www.abvolt.com



 | 
Pages: 1
Prev: zero padding
Next: Call For Manuscripts: International Journal of Signal Processing (IJSP)