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:

  1. Compass Hard Iron Offset Auto-Solver
  2. Accelerometer Yaw Pitch & Roll Calculator
  3. 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/.

41 Responses to “6DOF Arduino: Compass & Accelerometer

  • Hi, this library looks exactly what I need 🙂 I just installed it but the examples use a library for the HMC5883L. Which one are you using? Could you give me a link where I can download it. I’m trying to to get tilt compensated values from a HMC5883L with a ADXL345 but I’m pretty new to programming so this library would be ideal for me. Thanks 🙂

  • Nice work!

    Would you mind posting the board design file or schematic?

  • Hi. Is this library possible to compile for the Sparkfun SEN-10736?
    http://www.sparkfun.com/products/10736

  • Hi David,

    Here is a link to the schematic of the board shown in the post.
    http://n0m1.com/wp-content/uploads/2012/02/tilt-comp-sch1.png

    Thanks,
    Michael

  • Hi Anders,

    It will work with any hardware, as long as it can provide 3 axis compass and accelerometer data. The only difference with the sparkfun board is the accelerometer, which will require it’s own library to access.

    Michael

  • Thanks for quick reply Michael, yeah I tried to replace it with the ADXL345 library but the implementation was so different that I would have to rewrite your entire lib to get it to work :/

    Anders

  • Different? Rewrite the entire library?
    Both the ADXL345 and the MMA8453 are 3-axis digital accelerometers that give raw measurements for each axis.

    Here, via Google, is an Arduino Library to set the mode and access the data registers of the ADXL345: http://bildr.org/2011/03/adxl345-arduino/

    Just pass the compass and accelerometer data into the function like this: sixDOF.compCompass (mag.X, mag.Y, mag.Z, accel.X, accel.Y, accel.Z, true);

    Michael

  • Thanks! Ive come a little furher (Im a high level programmer C# so this is new to me :D)

    The adxl lib requires wire and if i include that lib i the HMC lib complaints instead

    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:23:23: error: WProgram.h: No such file or directory
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp: In member function ‘void HMC5883L::Write(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:110: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:111: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp: In member function ‘uint8_t* HMC5883L::Read(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:118: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:129: error: ‘class TwoWire’ has no member named ‘receive’

    Do you have any ideas? Thanks a million!

  • Hi Anders,

    The HMC5883L requires the repeated start command and the wire library included with Arduino is off spec on this point.
    The best solution is to use the awesome i2c library by Wayne Truchsess of DSS circuits.
    http://dsscircuits.com/articles/arduino-i2c-master-library.html
    His i2c library is also smaller and faster than the stock wire/i2c.

    In the ADXL345 library, you will have to find & replace the wire commands with the corresponding i2c commands. As example, from –> to:
    wire.begin(address); –> i2c.begin(address);
    Wire.read(); –> I2c.read(address, numberBytes) ;

    Michael

  • Thanks for reply and all your help

    I tried doing this before you write your reply 😀 But Im not a c-programmer and I run into ambiguous calls (It cant determine which write method to use) I changed the ADX write method to take the correct parameters, but it wont help get the same error.. the adx write method after my change (also changed in the h file)

    // Writes val to address register on device
    void ADXL345::writeTo(uint8_t address, uint8_t val) {
    I2c.write(ADXL345_DEVICE, address, val);
    }

  • Actually got the ADX lib compile with i2c will now try with your lib 😛

  • Awesome!

  • The ADXL part works (Tried the ADXL sample and it works) but the HMC code gives errors

    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:23:23: error: WProgram.h: No such file or directory
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp: In member function ‘void HMC5883L::Write(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:110: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:111: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp: In member function ‘uint8_t* HMC5883L::Read(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:118: error: ‘class TwoWire’ has no member named ‘send’
    C:Program Files (x86)arduino-1.0librariesHMC5883LHMC5883L.cpp:129: error: ‘class TwoWire’ has no member named ‘receive’

  • Yeah, find that out, but i still get errors :/

    HMC5883L_Example:0: error: stray ” in program
    HMC5883L_Example:-1: error: variable or field ‘Output’ declared void
    HMC5883L_Example:-1: error: ‘MagnetometerRaw’ was not declared in this scope
    HMC5883L_Example:-1: error: ‘MagnetometerScaled’ was not declared in this scope
    HMC5883L_Example:-1: error: expected primary-expression before ‘float’
    HMC5883L_Example:-1: error: expected primary-expression before ‘float’
    In file included from HMC5883L_Example.cpp:23:
    C:Program Files (x86)arduino-1.0librariesWire/Wire.h:28: error: expected constructor, destructor, or type conversion before ‘class’
    C:Program Files (x86)arduino-1.0librariesWire/Wire.h:67: error: ‘TwoWire’ does not name a type
    HMC5883L_Example.cpp: In function ‘void setup()’:
    HMC5883L_Example:34: error: ‘Wire’ was not declared in this scope
    HMC5883L_Example.cpp: In function ‘void loop()’:
    HMC5883L_Example:83: error: ‘Output’ was not declared in this scope

  • sorry, wrong paste. here it is

    HMC5883LHMC5883L.cpp.o: In function `HMC5883L::Write(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:121: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:121: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:121: undefined reference to `TwoWire::beginTransmission(int)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:122: undefined reference to `TwoWire::write(unsigned char)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:123: undefined reference to `TwoWire::write(unsigned char)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:124: undefined reference to `TwoWire::endTransmission()’
    HMC5883LHMC5883L.cpp.o: In function `HMC5883L::Read(int, int)’:
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:129: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:129: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:129: undefined reference to `TwoWire::beginTransmission(int)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:130: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:130: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:130: undefined reference to `TwoWire::write(unsigned char)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:131: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:131: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:131: undefined reference to `TwoWire::endTransmission()’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:133: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:133: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:133: undefined reference to `TwoWire::beginTransmission(int)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:134: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:134: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:134: undefined reference to `TwoWire::requestFrom(int, int)’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:137: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:137: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:137: undefined reference to `TwoWire::available()’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:141: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:141: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:141: undefined reference to `TwoWire::read()’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:144: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:144: undefined reference to `Wire’
    C:Program Files (x86)arduino-1.0librariesHMC5883L/HMC5883L.cpp:144: undefined reference to `TwoWire::endTransmission()’

  • Got it to work! 😀 I think ill have to write a tutorial for the none Ardino experts 😛

  • Sorry for spamming your comment board Michael. I have huge problems with drift on my Sparkfun board and I was hoping this lib with the hard iron calibrator and ruling out the gyro will help me get more stable result, Im going to use it for headtracking so zero drift is important.

    I got it to build and run, but for some reason the Yaw is not responing, instead roll or pitch is mapped to the Yaw axis (I do not know which side on the card is pitch and which is roll) Heres a youtube clip showing the output http://www.youtube.com/watch?v=2BxxYVEs7rw

    And here is my converted code to use ADXL345

    http://pastebin.com/dTAsygbN

    Best regards and thanks for all your help, Anders

    http://pastebin.com/dTAsygbN

  • Hi Michael.
    Do you have any input on this?

    Much obliged, Anders

  • It sounds like the orientations between your compass and accelerometer are not right. It took me a few times to get mine orientated correctly as my X&Y magnetic were not the same as X&Y accelerometer. IIRC roll will come out last but I can’t test this as my hardware is a part at the moment.

  • I’m trying to use this library with an HMC5883L & an ADXL335 (not an ADXL345). I have tried just feeding the values from them into the sixDOF.compCompass method but (as far as I can tell) this method doesn’t have any knowledge of the scale that the readings from my ADXL335 are?

    In the examples included with the library you do “accel.dataMode(true, 2);” which sets the range of the ADXL345 to 2G, however the ADXL335 has a scale of 3G which the arduino sees as a value between 260 & 415.

    Do I have to manually scale the readings from my ADXL335 or does it not matter as it only needs to calculate the difference between the axis? At the moment the tilt compensation adds 100-200 degrees even when it is all flat on my desk?

  • The library is ratio-metric and includes a scaler function that uses a common multiplier to maximize the values and minimize quantization loses.
    So it’s best not to prescale as it will require extra resources without benefit.

    Michael

  • Thanks. Can you think of any reason that I am getting incorrect behaviour if it isn’t due to scaling? As an example, when both the accelerometer & magnetometer are flat on my desk with all of their axis aligned/parallel, the uncompensated heading seems okay but the compensated heading is wildly out – such as 194 uncompensated results in 305 compensated.

  • I just noticed that the uncompensated reading was odd & replaced;

    head1 = sixDOF.atan2Int(-raw.XAxis, raw.ZAxis);

    with;

    head1 = sixDOF.atan2Int(raw.YAxis, raw.XAxis);

    which gives me a ‘correct’ uncompensated bearing (I think) but the compensated one is still out. But now I am thinking that it is a mistake with which axis I am using, so I am going to try switching them around & changing their sign, etc.

  • You may also have to experiment with the mix of the accelerometer and magnetometer data. In my hardware I have the X & Y swapped and rotated, only the Z axis matches on both.

  • Am I right in thinking that wrt swapping/rotating accelerometer/magnetometer data, their readings for a particular axis should both increase or decrease when tilted in a particular direction?

    Atm my accelerometer seems to be the exact opposite of my magnetometer, but if I negate all of the accelerometer readings the result is still off.

  • Apologies for spamming the comments section, but I’m tearing my hair out over this one!

    I’ve confirmed the orientation of the axes in my accelerometer & magnetometer according to the ‘NED’ convention that the Freescale Semi application note uses. I have appropriately reversed the accelerometer X axis using the map() function & reversed the magnetometer Z & Y axes using minus signs.

    The roll & pitch values I get are correct; at least as far as they increase with clockwise rotation about positive x & y axes respectively. But yaw I’m not so sure of. The final compensated heading is still completely off, maybe because of something wrong with yaw?

    I’m so confused >.<

  • I just got through trouble-shooting my gps/SeaTalk – position, drift, windspeed, wind-direction, depth, and heading repeater for installation in the boat. My question is about hard iron compensation. Is this the compensation achieved by adjusting magnets and iron balls near a magnetic compass?

    On a purpose made marine electronic navigation compass, the compensation routine is to toggle the compensation mode, then pilot the boat through 720 degrees. I assume that this lets the compass software recognize spurious magnetic effects which would influence the reading and develop “compensating” corrections.

    Is this hard iron compensation?

    best regards.

  • Hello
    I don’t understend, do I need to rotate the compass when running Hard Iron Offset?
    Best regards,
    Vitaly

  • Yes, that is correct.
    Hard iron correction requires a set of data points around a sphere.

  • and how long? 🙂

  • Not how long, but how far…
    It looks for an assortment of near axis values, so give it a pitch, a roll, then a yaw and it should have picked all the points in needs…

  • I have the same errors that you posted. How did you end up getting rid of them?

  • Nice article, thank you for sharing with everyone. At this point I want to use your atan2 function for non-compensated compass heading. In your sketch Tilt_comp_6dof_example.ino you have the following note:

    // this is correct orientation for non tilt comp – notice the result is not the same as tilt comp
    head1 = sixDOF.atan2Int ( – raw.XAxis, raw.ZAxis); //use the 6dof atan2 because its faster

    My question is what is the correct orientation of the board, since you are using X and Z (and not X and Y) axis? I have HMC6883L board from Sparkfun. Do I rotate the board 90 degrees and in which direction? A picture would be nice too 🙂

    Many thanks.

  • Hi Michael – big thanks for this amazing library, it’s going to save me a ton of work.

    Quick question – the hard iron offsets, once calculated do I just add them to the raw values returned from my compass (I’m using an LSM303DLHC) before passing those adjusted values into compCompass?

    Cheers… Mike

  • On further reading I’ve discovered that the answer to my question is “yes”.

  • Awesome, glad to hear you like it!

  • Hi,

    Thanks for the library. I’ve been trying to get a decent tilt-compensated heading out of it for a few hours now and I think I’m nearly there, but mostly through trial and error, so I don’t think I have an optimal setup.

    I’m using an ADXL345 and the HMC5883L, with the X/Y/Z axis aligned between the two, with the X facing forward, the Y pointing left and the Z up.

    Now even though the co-ordinate systems on both should be aligned, I’ve had the best results with the following config:

    sixDOF.compCompass(compassX, -compassY, -compassZ, accelX, accelY, accelZ, true);
    float compHeading = sixDOF.atan2Int(-sixDOF.yAxisComp(), sixDOF.xAxisComp());

    This seems pretty stable in all orientations except when I roll left in the direction of Y+.

    My question is, have you got any idea why I have to flip the Y/Z compass readings to get it to work, and can you spot any way to improve my compensated output?

    Many thanks.

  • Hi,

    I am trying to get the hard iron auto calibration working using your example.
    The while donecombo == 0 loop gets stuck. I output x,y,z axis within the loop and they change.

    I turned the IMU in all directions several times but donecombo stays 0.

    Could you give me a hint on what I may do wrong?

    Thanks
    Robert

  • Hi,
    has anyone ported this excellent library to the pololu LMS303D 6 axis board (three axis compass and a three axis accelerometer)

    I will give it a try and or start looking for a board with the HMC5883L with a ADXL345 on it
    Thanks

Leave a Reply

Your email address will not be published. Required fields are marked *