- Published:
- 12.27.2012 / 1am

- Category:
- Work

- Previous:
- It Was Never About the Mileage

- Next:
- Site Improvements

# Fixed-point atan2

*This was an unfinished draft meant to be published on February 6, 2012. Enjoy?*

`atan2`

is a freaking useful function: given some y and x^{1}^{2}, it computes their “four-quadrant” arctangent, which is basically that coordinate’s angle in polar form.

These two beautiful Wikipedia diagrams explain why it’s so much better than the naïve `atan(y/x)`

:

It also shows the two reasons why I suffered a stroke of unsanity and whipped up my own `atan2`

function in Q15 fixed point/1.0.15/1.15/whycan’twehaveonenameforthis format that’s so commonly used by DSP chips:

- You see how smooth that thing is? That’s because it was generated in some high-falutin’ floating point on a fancy machine with a sweet FPU. Well, I program itty-bitty embedded microcontrollers with nary a hardware integer divider, so I use fixed-point arithmetic for my math. One of the more popular fixed-point formats is Q15, which is taking a 16-bit signed two’s complement integer
^{3}and instead of having it represent , have it represent^{4}. I used it out of convenience since I knew a lot of the tips & tricks for making Q15 computations go fast. - The range on
`atan2`

output is . That’s nice and mathemagicatical, but not so useful an angle representation on a microcontroller. See, first, that doesn’t fit inside of a Q15 number. Then even if it did, it’d still be ridiculous to have your angle go from some arbitrary constant to some bigger arbitrary constant. You got only 16 bits! You want every possible 16-bit number to be a valid and useful angle, not just some subrange. So let’s map our 16-bit range to a full turn of the circle .

## Fixed point angular representation

By using all 16 bits for representing angle, we can do wicked fast things like `sin`

and `cos`

lookup tables:

const int16_t sin_lut[4096]; // a 2^12 table of precomputed sin values const uint16_t angle; // some angle in our 16-bit turn format const int16_t sin_of_angle = sin_lut[angle >> 4]; // rshift because there are 16x possible angles as table entries // we can round to the nearest entry: sin_lut[(angle + 8) >> 4]

We also avoid having to wrap our calculations to stay within ; instead, integer underflow and overflow does it automatically!

## Derivation for `atan2`

To implement `atan2`

, I whipped out Chapter 18 from Streamlining Digital Signal Processing, which had several approximations for `atan`

, including the following formula:

That’s good to a worst-case error of 0.22° for , which I’m cool with, but it only works for octants I and VIII. Also, the output ranges from , not like we want. Let’s fix the range first:

The last form is the one you’d actually put in your code, since it requires only two multiplies and one add. Because I’m using Q15, which represents and I want , I end up scaling the whole thing again by 2:

Now to handle Cartesian coordinate inputs not in octant I and VIII, I chose to manipulate the coordinates and exploit the symmetries of `atan2`

. For example, if our input is in octants II and III, then we can flip the x and y coordinates to perform a reflection about . If we do our range-restricted `atan(y/x)`

, the result will be in octants I/VIII. Now, we gotta negate the result to account for the “flip” caused by the earlier reflection, and then we need to rotate by 90° to get it back to octants II/III, which is as simple as adding 0.25 turns to the angle.

[diagram goes here]

Unfortunately, my derivations here diverged from the four-quadrant formula in the book. It turns out the book is just wrong. asdf

## Implementation details AKA `nabs`

rocks

So anyways, here’s the implementation. I should note that the code relies on a `q15_mul`

function that perform rounding of the product from the 32-bit intermediate to 16-bit result for maximum accuracy. Also, `s16_nabs`

is a function for *negative absolute value*. Why use `nabs`

? Well, `abs`

is undefined for the most negative number, and chances are if you plug the Q15 value for -1 into `abs`

, you’ll get -1 back out. *Negative* absolute value, on the other hand, is pretty easy to keep fully defined.

To test my approximate arctangent, I tested all possible inputs and computed their errors relative to the double-precision `atan2`

implementation in the GNU C library. I got a worst-case error of 0.221°, which means I’m not losing much precision or introducing quantization error by using fixed-point, and there’s an root mean square (RMS) error of about 0.0004 turns.

What I’m saying is this: relax, this code was tested by brute force. 😉

`s16_nabs`

, `q15_mul`

, `q15_div`

, and of course `fxpt_atan2`

are all included in this MIT-licensed source file: fxpt_atan2.c

A DSP library might have faster/better versions of some of those functions that may take advantage of your microcontroller’s DSP ALU.

## Codes!

/** * 16-bit fixed point four-quadrant arctangent. Given some Cartesian vector * (x, y), find the angle subtended by the vector and the positive x-axis. * * The value returned is in units of 1/65536ths of one turn. This allows the use * of the full 16-bit unsigned range to represent a turn. e.g. 0x0000 is 0 * radians, 0x8000 is pi radians, and 0xFFFF is (65535 / 32768) * pi radians. * * Because the magnitude of the input vector does not change the angle it * represents, the inputs can be in any signed 16-bit fixed-point format. * * @param y y-coordinate in signed 16-bit * @param x x-coordinate in signed 16-bit * @return angle in (val / 32768) * pi radian increments from 0x0000 to 0xFFFF */ uint16_t fxpt_atan2(const int16_t y, const int16_t x) { if (x == y) { // x/y or y/x would return -1 since 1 isn't representable if (y > 0) { // 1/8 return 8192; } else if (y < 0) { // 5/8 return 40960; } else { // x = y = 0 return 0; } } const int16_t nabs_y = s16_nabs(y), nabs_x = s16_nabs(x); if (nabs_x < nabs_y) { // octants 1, 4, 5, 8 const int16_t y_over_x = q15_div(y, x); const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(y_over_x)); const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, y_over_x); if (x > 0) { // octants 1, 8 return unrotated; } else { // octants 4, 5 return 32768 + unrotated; } } else { // octants 2, 3, 6, 7 const int16_t x_over_y = q15_div(x, y); const int16_t correction = q15_mul(q15_from_double(0.273 * M_1_PI), s16_nabs(x_over_y)); const int16_t unrotated = q15_mul(q15_from_double(0.25 + 0.273 * M_1_PI) + correction, x_over_y); if (y > 0) { // octants 2, 3 return 16384 - unrotated; } else { // octants 6, 7 return 49152 - unrotated; } } }