6DOF Arduino: Compass & Accelerometer
This Arduino library is a mixed bag containing a number of functions to facilitate rapid sensor integration between a three axis compass and a three axis accelerometer. Where speed is required, function math is 8.8 fixed point, while non-performance functions use float or some combination of both. The library contains detailed examples for each section.
There are three main parts making up this library, for introductory purposes, these parts are:
- Compass Hard Iron Offset Auto-Solver
- Accelerometer Yaw Pitch & Roll Calculator
- 360° Compass Tilt Compensation
The first item, the hard iron offset solver is an independent group of functions designed to capture a set of semi-arbitrary 3 axis magnetic data points around a sphere and then calculate the x, y & z hard iron offsets. It produces consistently repeatable results by way of outlier data rejection based on an established trust relationship with axial sensitivity. A closer look at the parts that make up this solver reveal four functions:
deviantSpread() – Picks eight positions around a sphere where each combination is a sampling of positive and negative x, y, z positions. To improve solving, it prefers to pick combinations where one of the x, y, z components is a low number.
calOffsets() – This function calls the solver for 6 of the 8 position datasets and creates an inverse trust associated with the result. After which, it bubbles the two remaining datasets through comparing the inverse trust to reject poor solutions. The accepted results get averaged to make up the x, y & z hard iron offsets.
calSense() – This bit is from David W. Schultz. It does the preparatory work of stuffing the arrays and calling the solver. After which it computes offsets and axis sensitivities. It is modified from original to accept integers and computes an inverse trust variable with respect to axial sensitivity.
linearEquationsSolving() – This is the actual solver which uses Gaussian elimination to deduce the axial limits of the datasets representing the sphere. This code is written by Henry Guennadi Levkin.
An example layout with the Honeywell HMC5883L compass, the Freescale MMA8453Q accelerometer, a 3v3 linear power supply and associated i2c level shifting:
The second and third parts of the library, the angle calculation and tilt compensation, are tightly integrated with each other, being they are inter-dependent. The basis of this code is the Freescale tilt compensation application note which has been modified to perform under the Arduino environment. Changes were also made to improve the efficiency on the 8bit AVR platform. These two parts break down into a plethora of handy functions including some very useful trigonometric fixed point math:
atan2Int() – This is a wrapper for the fixed point math function atanInt. It takes a ratio-metric input of x and y and returns degrees times 100 using a first, third and fifth order polynomial approximation.
sinInt() – Another trig function in fixed point integer math which aptly named, returns the sine of an angle. According to wikipedia, the word sine comes from a Latin mistranslation of the Arabic word jiba.
compCompass() – All the heavy lifting is done in this behemoth of an function. The device angles are computed and the tilt compensated magnetometer values are un-rolled, un-pitched and un-yawed.
divInt() – This helper is an accurate integer division function. The accuracy comes in part by maximizing both the denominator and numerator equally to reduce quantization error.
lowPassInt() – Finally we have a clever lowpass filter for the computed angles. What makes this function special is that it operates using modulo arithmetic to prevent rollover errors on dead North transitions where 0° starts and 360° ends.
And finally it’s now time for some pretty moving pictures. In the following video we are real time plotting in 3D utilizing Hon Bo Xuan’s 3DScatter processing code. The video is in three parts, first showing the 3D plot of the raw unadulterated magnetometer data from a Honeywell HMC5883L, notice the significant Z offset caused by some ferrous material on the PCB. As a result of input saturation, out of bounds of data is discarded and the plot takes longer. The second plot is after running the hard iron auto-solver, we can see the sphere is now centered and it populates very quickly. And the third plot is of the tilt compensated magnetometer data stream. Make some popcorn, sit back and enjoy the romantic comedy of error correction:
The accelerometer used in this test is the Freescale MMA8453Q for which we previously released the Arduino library here: http://krazatchu.ca/wp/2012/02/12/shake-rattle-roll-the-mma8453q-arduino/.