Top Banner

Click here to load reader

6DOF Arduino

Aug 22, 2014




6DOF Arduino: Compass & AccelerometerThis 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 its now time for some pretty moving pictures. In the following video we are real time plotting in 3D utilizing Hon Bo Xuans 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:

Shake, Rattle & Roll: The MMA8453Q & Arduino

So we finally finished putting the polish on our latest Arduino library designed for use with the MMA8453Q Accelerometer and its siblings the MMA8451 and MMA8452. This library gets you going with the basics on this accelerometer and it also allows you to dig in a little deeper in to some of the advanced features, like motion detection and shake detection without much effort. This little QFN chip is not only cheap but its packed with 3 Axis of flavorful features. Its on board 10 bit ADC coupled to the I2C interface means fast and accurate 3 axis measurements as well as built in free fall, pulse and jolt detection on programmable interrupt pins. As illustrated below, its a 3.3 volt part requiring additional level shifting to interface a 5 volt Arduino, this can be accomplished with a logic level N channel FET and a schottky diode:

So lets first get the library setup. Go ahead and download it from our github repo. The next thing you will need to do is download the wonderful and necessary i2c master library which our library depends on. This library is not only faster then the regular Arduino Wire library, it also follows the proper I2C specifications and allows for the sending of a repeated start bit which is required by the MMA845x family. So after you got both those libraries downloaded place them in your Arduino Libraries folder, and we are ready to get started. When you fire up your Arduino app you will notice a new set of 4 examples in your examples menu. The first example we will take a look at is the ultra simple DataMode example. When you

up up this sketch you will see right away just how simple it is, lets break it down anyway though, just in case. At the top you will see the include of the library and its dependency library, the i2C lib, and the instantiation of the accel object.1

#include 2

#include 3

MMA8453_n0m1 accel;4

In the setup function, you call the function dataMode to set the accelerometer into raw data output mode. This function has 2 parameters. A boolean, which is true for high resolution mode (10bit), and false for low resolution mode (8bit). The next parameter is to set the range of gravitational force the accelerometer will be sensitive to. These values, are +/-2g(waving your project around in your hand), +/-4g(your drag racing project), or +/-8g(your atmosphere re-entry vehicle project).1

accel.dataMode(true, 2);

After that there is almost nothing left to do. Just call the update function at the top of your loop, which retrieves the latest values from the accelerometer, then use those values however you feel.1





Well thats it! Expecting some fancy complex gravitational voodoo, matrix math or pointer arithmetic? Sorry to disappoint. Ill try to step it up a little in the next example thenhowever theres a good chance it will be almost as simple.

HMC5883L Compass Tutorial with Arduino LibraryThe HMC5883L library we used is from here: Compass Tutorial with Arduino LibraryUsing a magnetometer can be a little tricky, especially if your unsure about the formulas to use to get the correct bearing and when other magnetic objects are interfering with your signal. We've created a library for our HMC5883L Breakout Board, which will also be compatible with other HMC5883L breakout boards made by other manufacturers. Join us whilst we cover the following: Understand what is a magnetometer and how they work. Introduce the HMC5883L Arduino Library Explain how to extract data from the HMC5883L. Explain how to calculate a bearing from this data.

So, assuming you have an HMC5883L Breakout Board and an Arduino. We will walk through using the Love Electronics HMC5883L breakout board to output our bearing. Note: The HMC5883L is not the same as the HMC5883. This library only covers the HMC5883L sold on our website.

How do compasses work?Firstly an introduction, a (standard handheld) compass works by aligning itself to the earths magnetic field. Because the compass' needle is a ferrous material, it aligns swings on its bearing in the center as the magnetic field of the earth pulls it into alignment. These magnetic fields expand throughout the surface of the earth (and beyond) so we can use them to help us tell us which direction we facing. Our magnetometer uses these magnetic fields, however it doesn't pull on a little needle inside it! (It

probably wouldn't fit anyway). Inside our magnetometer are three magneto-resistive sensors on three axis. These can be quite complicated to understand (not to mention explain!), it is sufficient to say that the effect of magnetic fields on these sensors adjust the current flow through the sensor. By applying a scale to this current, we can tell the magnetic force (measured in Gauss) on this sensor. For a detailed explanation to magneto-resistive sensors use this application note: Magneto-Resistive Sensors. By co