quick and dirty approximation for arctan function.

edited January 2008 in Technique
Outside this forum I was discussing some of my autokap system's software and I offered to share my code for approximating the atan2 function. This is a trig function I use to convert X and Y magnetometer readings into a heading angle.

I had to write my own function for this because I didn't have enough program memory to use a full-blown atan2 function, I needed to work in a fixed point number representation and I didn't need very high accuracy. (This code actually has a maximum error of about 5 degrees, if I recall correctly.)

Anyway, I'm sure this won't be of interest to everyone but I figured this would be a good place to put it out in the public domain:

// these macros treat 16 bit signed integers as 16-bit fixed point
// quantities in 1.15 format.

#define FPMUL(x, y) ((int16_t)(((int32_t)x * (int32_t)y) >> 15))
#define FPDIV(x, y) ((int16_t)(((int32_t)x << 15) / (int32_t) y))

#define FP_ONE (0x8000)
#define FP_HALF (0x4000)

// computes a coarse approximation to the arctan function.
// inputs: x, y 16-bit integer or fixed point quantities.
// output: fixed point quantity in 1.15 format in range -1.0 to 0.99997 pirads.
// (alternatively this can be interpreted as int in range
// -32768 to 32767.)
// angles are measured relative to the positive x axis.
//
// A pirad is a unit of angle measurement equivalent to Pi Radians.
// 360 degrees = 2*pi radians = 2 pirads.
//
// This function uses a very coarse approximation to the atan function.
// It has a maximum error of about 5 degrees.
//
int16_t my_atan2(int16_t y, int16_t x)
{
uint8_t flags = 0;
int16_t tmp;
int16_t atan;

// printf("myatan2 input: y=0x%04x, x=0x%04x\n", y, x);

// fold input values into first 45 degree sector
if (y < 0) {
flags |= NEG_Y;
y = -y;
}

if (x < 0) {
flags |= NEG_X;
x = -x;
}

if (y > x)
{
flags |= SWAP_XY;
tmp = x;
x = y;
y = tmp;
}

// printf("myatan2 normalized input: y=0x%04x, x=0x%04x\n", y, x);

// compute ratio y/x in 0.15 format.
if (x == 0) {
atan = 0;
} else {
atan = FPDIV(y, x) >> 2;
}

// printf("0x%04x\n", atan);
// unfold result
if (flags & SWAP_XY)
{
atan = FP_HALF - atan;
// printf("unswapped to 0x%04x\n", atan);
}

if (flags & NEG_X)
{
atan = FP_ONE - atan;
}

if (flags & NEG_Y)
{
atan = -atan;
}

return atan;
}

Comments

  • That's really nicely done, David!

    When I saw this pop up, it reminded me of some weird bit of code I had written a couple years ago. I went and dug it up-- turned out to be an autolisp (Autocad Lisp) implementation of the arcsin function involved in laying out seating in auditoriums. Autolisp has an arctan, but not an arcsin function for some odd historical reason.
  • Thanks, Ben. It's based on material in Jack Crenshaw's book, "Math Toolkit for Real-Time Programming." (at amazon)
Sign In or Register to comment.