Magnetometer V: Fixed-Point Math

This is another article in my series about developing a magnetometer-based digital compass. Last time, I talked about estimating code size, and what I might do to fit the application in the roughly 5.25K program space available on an Adafruit Trinket.

In this article, I replace the floating-point math with fixed-point, and make various space-saving improvements to the calibration and rotation code. Read on for more.

Update: This article is part of a larger series on building an AVR-based compass. You may be interested in the other articles in the series.

Fixed-Point Math

When I measured what was consuming space in my naïve implementation, by far the single biggest offender was the use of floating-point math.

There are several good reasons the standard math library takes up a lot of space (and to suspect a custom library for this application could be a lot smaller):

• The standard library has to be fully general; we need only a few operations (and by being a little clever about the code, we can reduce the number of operations needed ever further).
• The standard library has to produce results which are bit-for-bit conformant with the relevant standards. We need only “good enough” precision.
• The standard library has to deal with edge cases, overflow, undefined values (NaN) and infinities. We need none of these.
• The standard library has to be able to represent a huge range of different magnitudes. We can arrange the code so that all values fall within a relatively small range.

For my custom math library implementation, I chose to use a fixed-point representation with the following properties:

• 16-bit data width; this is a compromise between speed and size on one hand versus precision and range on the other.
• Signed values — a natural choice given the process I wanted to implement.
• Values represented in binary (as opposed to decimal / BCD); this makes nearly every operation easier, faster and smaller. (We can’t exactly represent decimal powers of ten, but it doesn’t matter for this application.)
• Signed values represented in two’s-complement form. This gives us addition and subtraction “for free” (i.e., using the native operations for integers), makes multiplication and division hugely easier, and doesn’t have any significant drawbacks I can see.
• 11 bits to the right of the radix point. This gives us a range of ±15 and approximately three decimal places of precision. (This is the most arbitrary choice in the list. Other values could work just as well.)

Once I had decided on a representation, the next task was to look at the operations I needed to implement. After a fair number of revisions, I arrived at working code which used only the following:

• Addition, subtraction and arithmetic shift (i.e., multiplication or division by even integer powers of two). These could all be done by simply using the corresponding operators (+, -, << and >>) built in to the C language.
• Multiplication and division. These were implemented by sign-extending the 16-bit values to 32 bits, using the native C multiplication and division operators, then truncating the result. (For division, the numerator was left-shifted 16 bits.)
• Absolute value. Trivial (and, come to think of it, should probably be a macro rather than a function).
• Distance from the origin to a point in three-space. (This is also used to find the hypotenuse of a right triangle given two sides, simply by passing zero as one of the arguments.) This is done by finding a 32-bit sum-of-squares, then taking the square root. (Aside: the binary square root algorithm is cool.)
• The two-argument arctangent. Here, I rotated the system into the first octant, then approximated using CORDIC. Jasper Vijn’s online explanation was hugely helpful in figuring this out. Note that the output is in “binary radians” (where a semicircle is represented by a small integer value rather than π).

Unit testing (by performing analogous operations in fixed- and floating-point, and comparing the results) was crucial. My early efforts were riddled with bugs, and I never would have gotten anywhere had I just tried to run the whole calibration process at the start.

Other Space-Saving Measures

In addition to changing over to fixed-point math, I made a number of other space-saving changes. Mostly these fell into the general categories of:

1. Eliminate unnecessary stuff.
2. Combine redundant stuff.
3. Find space-efficient alternatives for space-expensive things.

Specific noteworthy changes include:

• Eliminate the jitter calculation. By experiment, I concluded that the jitter changed relatively little in environments that would be found during practical use. (If you put any compass on top of your subwoofer and crank the dubtrot, you probably won’t get accurate headings.)
• Combine the three functions for finding elemental rotations into a single generic function. (This also made it much simpler to pick an alternative order of operations and get better accuracy in the case where the starting point was very near the Z axis.)
• Rather than storing three elemental rotation matrices, store a single matrix and compose rotations as we go.
• Use plain distance (instead of square-of-distance) in a bunch of places in calibration. The square of the distance can overflow 16-bit values. Using 32-bit values would cost code space, RAM and complexity. Using plain distance avoids this, and we can use a single distance function both here and for rotation. Finding the 16-bit square root of a 32-bit value takes time, but space is at a much greater premium.

Integration Testing

As before, I used a Raspberry Pi with WiFi and battery power as a test platform. You can download my test code here: compass-tst8.tar.gz (15KB gzipped tar archive).

This let me use real interactive debugging tools, intelligible diagnostic output and a short compile-test-debug cycle while I was struggling to get the math right.

The linked code above seems to calibrate reliably and produce reasonable headings, though I haven’t measured the accuracy against a traceable standard, nor have I experimented with more than a few sensor orientations and interferers.

What about size? A quick (untested) port to AVR with diagnostic output removed comes in at 3850B (as compared to 8796B for my previous effort). Less than half the size — not too bad. At this point, I’m extremely optimistic that a working application will fit in the available space (even with the extra bits I have yet to add). I’m also reasonably confident that further size optimization, while possible, is a game of diminishing returns.

Next Steps

Now I have code that can do calibration and convert a reading to a heading, that fits in the available space and that works (at least on the Pi). That’s good, but it’s a long way from being a usable gadget.

Specific things I still need to do:

• Use the headings to drive the display. (The size above includes the bitmaps I plan to use and all the display hardware drivers; I just need to add chose a bitmap based on heading.)
• Store the calibration data in NVRAM when calibration is completed successfully. (Bonus: Do load-leveling; don’t always write to the same place.)
• Read the calibration data from NVRAM on power-up.
• Give the user a way to trigger a new calibration via an input button.
• Let the user cancel calibration in progress (via same input button) and return to previously stored calibration values.
• Give user visible feedback during calibration process.
• Display an error indication if any sensor axis is saturated.
• Display an error indication if rotated reading is too far from XY plane. (This would mean either transient interference or severe vehicle tilt.)
• Perform the HMC5883L self-test on power-up and periodically to achieve temperature compensation.
• Consider other display options; an 8×8 LED matrix isn’t necessarily the best choice.
• Come up with a power supply solution that can deal with a dirty 10-14VDC input.
• Make a nice enclosure and/or panel-mount solution.
• Lots more testing.

Looks like lots of work, but those are mostly things I know how to do already; I just need to do them. I at least know they’re all possible; I wasn’t so sure about making the code fit on my chosen platform.

2015 September 08 DGH: Arithmetic shift gives you multiplication or division by any integer power of two, not just any even power. Sometimes I have a dumb.