6DOF Arduino

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-Solver2. Accelerometer Yaw Pitch & Roll Calculator3. 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://n0m1.com/2012/02/12/shake-rattle-roll-the-mma8453q-arduino/.

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. It’s 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, it’s 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

2

3

4

#include <I2C.h>

#include <MMA8453_n0m1.h>

MMA8453_n0m1 accel;

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

2

3

4

5

accel.update();

Serial.println(accel.x());

Serial.println(accel.y());

Serial.println(accel.z());

Well that’s it! Expecting some fancy complex gravitational voodoo, matrix math or pointer arithmetic? Sorry to disappoint. I’ll try to step it up a little in the next example then…however there’s a good chance it will be almost as simple.

HMC5883L Compass Tutorial with Arduino Library

The HMC5883L library we used is from here:https://www.loveelectronics.co.uk/Tutorials/8/hmc5883l-tutorial-and-arduino-library

HMC5883L Compass Tutorial with Arduino Library

Using 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 combining information about two or more of these axis we can start to use the difference in the magnetic fields in the sensors to infer our bearing to magnetic north.

How do we use one?Okay, so now we know how to use one, the first step is to get some data out of our compass. The HMC5883L is a device which communicates over I2C, a really easy communication protocol to use, and our favorite way to interface with our breakout boards here at Love Electronics. All you need to do is plug the breakout board into your breadboard and connect up the following pins to your Arduino:

Arduino GND -> HMC5883L GND Arduino 3.3V -> HMC5883L VCC Arduino A4 (SDA) -> HMC5883L SDA Arduino A5 (SCL) -> HMC5883L SCL You will also need to add two 'pull-up' resistors to enable I2C. For this, connect two 4.7k or 10k

resistors between SDA and VCC, and SCL and VCC.

Now of course to talk to the HMC5883L we will need some code, helpfully we've written an arduino library for the HMC5883L which makes this really easy. Simply download the Arduino Library for HMC5883L and extract it to your library folder in your Arduino installation. Mine is here: C:\Program Files (x86)\arduino-0021\libraries\

Once you have the library installed start your Arduino IDE and we can get coding. If you just want to go and get all the code up without coding along, simply open the HMC5883L_Example file from the Arduino Menu, otherwise we can code away together and you can fully understand everything we are going to write.

Using the HMC5883L Arduino LibraryWhat we want to accomplish is an Arduino sketch that will tell us the which direction we are pointing in degrees, so it should read 0° for when we are point at magnetic north, and 180° when we are pointed south.

For a quick reality check, the bearing you get from your compass will be a little off, the compass senses magnetic fields, so any ferreous material anywhere near your compass can affect your output considerably. Also any kind of radio waves (I'm looking at you mobile phone!) and don't even let it see any

stereo speakers!!

So, open your Arduino IDE and begin a new sketch, and quickly start off by writing the initial setup and loop method placeholders.

Then we must import the HMC5883L Arduino Library for us to use. Because you installed the library into your libraries folder you should be able to add it by selecting the following menu option.

The HMC5883L communicates over I2C, so we must also import the Wire library (do this the same way) and add the following code to the setup() method to start the Wire library when we begin our Sketch:

Wire.begin();

We will now able to use devices on the I2C bus. We also want to communicate with our computer to report our findings, so initialize the serial port by adding the following two lines into setup():

Serial.begin(9600); Serial.println("Serial started.");

So, if you run this sketch we should get "Serial started." reported via the serial window. Great!Now we need to declare an instance of the HMC5883L we can use throughout our sketch. So lets add an HMC5883L as a global variable by adding outside the setup() and loop() methods, and initialize it inside the setup() method.

// Add this outside the methods as a global variable. HMC5883L compass;

// Add this to the end of setup() to create an instance of HMC5883L. compass = HMC5883L();

Once we have an instance of the compass we need to set it up, the compass needs to know what kind of gain (response scale) to work out, and how to output its measurements. We can configure the compass by adding the following lines after creating the compass in setup():

Serial.println("Setting scale to +/- 1.3 Ga"); int error = compass.SetScale(1.3); // Set the scale of the compass. if(error != 0) // If there is an error, print it out. Serial.println(compass.GetErrorText(error));

Serial.println("Setting measurement mode to continuous."); error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous if(error != 0) // If there is an error, print it out. Serial.println(compass.GetErrorText(error));

This will set the gain of the device to 1.3 Gauss (Ga), this means the device is able to report magnetic fields up to -/+ 1.3 Gauss. The lower you can make this number the more precice the compass will be, however if you have other objects interferring with the compass you may need to raise it to avoid overloading the sensor. Bear in mind that the compass can only accept certain gains, so check the datasheet to see what is available. If you choose an invalid gain the application will let you know by means of the error variable the Set method returns.

Your HMC5883L should now be configured and taking measurements, all the remains is to ask the sensor the data! To make sure we are on the same page, here is my current sketch:

So the HMC5883L Arduino Library can provide two different values for you to use. You can call either of the following:

// Retrive the raw values from the compass (not scaled). MagnetometerRaw raw = compass.ReadRawAxis(); // Retrive the scaled values from the compass (scaled to the configured scale). MagnetometerScaled scaled = compass.ReadScaledAxis();

ReadRawAxis returns the values retrived straight from the magnetometer, if you are not interested in the actual magnetic strength of the field you can use this to return a MagnetometerRaw structure, which has three fields for you to access the values on each axis:

MagnetometerRaw raw = compass.ReadRawAxis(); int xAxis = raw.XAxis; int yAxis = raw.YAxis; int zAxis = raw.ZAxis;

The ReadScaledAxis provides the same data, in the same structure (named MagnetometerScaled), however these values are scaled to the gain we set the device to operate (-/+ 1.3 Ga) when we called compass.SetScale(float gauss);. You can use this data when you want to know the actual magnetic value each sensor is seeing.

Calculate your bearingNow we know how to use the arduino library to communicate over I2C to the HMC5883L triple axis magnetometer chip we can put a little bit of maths together to calculate the bearing.

What we are going to do is add the following code to the loop() method that is going to retrive the values from the device and do the actual bearing calculation:

void loop(){ // Retrive the raw values from the compass (not scaled). MagnetometerRaw raw = compass.ReadRawAxis(); // Retrived the scaled values from the compass (scaled to the configured scale). MagnetometerScaled scaled = compass.ReadScaledAxis(); // Calculate heading when the magnetometer is level, then correct for signs of axis. float heading = atan2(raw.YAxis, raw.XAxis); // Correct for when signs are reversed. if(heading < 0) heading += 2*PI;

// Convert radians to degrees for readability. float headingDegrees = heading * 180/M_PI;

// Output the data via the serial port. Output(raw, scaled, heading, headingDegrees);}

We also need to add the Output method we refer to at the end of this method:

// Output the data down the serial port.void Output(MagnetometerRaw raw, MagnetometerScaled scaled, float heading, float headingDegrees){ Serial.print("Raw:\t"); Serial.print(raw.XAxis); Serial.print(" "); Serial.print(raw.YAxis); Serial.print(" "); Serial.print(raw.ZAxis); Serial.print(" \tScaled:\t"); Serial.print(scaled.XAxis); Serial.print(" "); Serial.print(scaled.YAxis); Serial.print(" "); Serial.print(scaled.ZAxis);

Serial.print(" \tHeading:\t"); Serial.print(heading); Serial.print(" Radians \t"); Serial.print(headingDegrees); Serial.println(" Degrees \t");}

You can now go ahead and run the sketch. Assuming you have got all the code in and connected your compass properly you should now be getting the following being reported from your serial port!

You should be able to rotate your device and see the bearing rotate from 0 to 360 degrees! Try to keep the compass away from anything magnetic to avoid interference, but try holding something like your mobile phone next to the compass to see how bad the effect can be on the results.

For Extra MarksNow you have the compass heading, you need to know that the magnetic field is not perfect around the earth, there are changes in the ground, and the core of the earth that affect the readings, and also this even changes over time! This is called magnetic delincation, and it can totally destroy what would otherwise be a good compass reading. For our American users, the effect can be quite pronounced, however the effect is negligable in the UK. Here is a map of this phenomenon, showing how it changes over time:Wikipedia

Luckly we can account for this effect in our equations quite easily, at least we can if we are not flying around the world! (Sorry Boeing...)By going to this website and clicking on your location you are given a Magnetic Declination.

Take this value and enter it in Wolfram Alpha like so: http://www.wolframalpha.com/input/?i=%284%C2%B0+21%27%29+in+radians and save the result given in milliradians mrad (75.92 mrad for me).

We are going combine this value with our heading to a correct for the magnetic declination.Change the calcuation in the loop() method to the following:

// Calculate heading when the magnetometer is level, then correct for signs of axis. float heading = atan2(raw.YAxis, raw.XAxis); // Your mrad result / 1000.00 (to turn it into radians). float declinationAngle = 75.92 / 1000.0; // If you have an EAST declination, use += declinationAngle, if you have a WEST declination, use -= declinationAngle heading += declinationAngle; // Correct for when signs are reversed. if(heading < 0) heading += 2*PI; // Check for wrap due to addition of declination. if(heading > 2*PI) heading -= 2*PI; // Convert radians to degrees for readability. float headingDegrees = heading * 180/M_PI;

The next step.

The problem with our current solution is that whilst it works well when the magnetometer is flat, as soon

as you till the compass your result will be wrong, the more so with more tilt. In our next tutorial about the

HMC5883L we will introduce using the ADXL345 triple axis accelerometer to provide a tilt corrected

implementation. If you would like to see that tutorial made, post a comment to let us know!

Tilt Compensating a Compass with an Accelerometer

IntroductionA compass is a navigation instrument that provides us with a heading parallel to the surface of the earth. The compass always points North so that you can use your deviation from this direction to calculate your heading.

Compasses work by detecting the magnetic fields produced by the hot rotating iron core at the centre of the earth. The strength of the earth's magnetic field is about 0.5 to 0.6 gauss and has a component parallel to the earth's surface that always points toward the magnetic north pole. Traditional compasses work by using these fields to rotate a ferrous metal rod in a small container. The magnetic fields align the metal rod along this component to point to magnetic north. Using this information, along with a scale on the outside of the compass we can work out our heading (the direction we are facing).

The device we are going to use today, called a magnetometer, does what it says, it measures magnetic fields. These devices are similar to traditional compasses in that the magnetic fields from the earths core act on small magnetoresistors on three axis (for a 3 axis magnetometer) which adjust current flow through the sensor. By applying a scale to this current, we can calculate the magnetic force (measured in Gauss) on the sensor.For a (very) detailed explanation to magneto-resistive sensors read this application note: Magneto-Resistive Sensors.

An important thing to understand about magnetometers, called compasses for sake of simplicity is that they do not provide us with a direction. They provide us with a way to measure magnetism which we can use to calculate a direction. The compass is only created once we use the magnetometer in some calculations, as a whole system.

The magnetometer we will be using today is the HMC5883L Triple Axis Magnetometer. This chip is a bargain for including triple axis sensors at up to 8 gauss on a tiny footprint.

A simple calclation we can use to create a compass is below. When the device is level, (pitch (Xb) and roll (Yb) are at 0 degrees). The compass heading can be determined like so:

The local earth magnetic field has a fixed component Hh on the horizontal plane pointing to the earths magnetic north. This is measured by the magnetic sensor axis XM and YM (here named as Xh and Yh). Using this we can calculate the heading angle using this simple equation:

Heading = arctan(Yh/Xh)

This simple method is discussed in this earlier compass tutorial.

The Tilt ProblemA problem that traditional compasses have is that they need to be held flat to function. If you hold a compass at right angles it will not work at all, and if you tilt it to 45 degrees the reading will be more inaccurate the further the compass is tilted. This problem occurs because the compass is only using the X and Y axis of the earths magnetic field (the compass needle is fixed onto a bearing that will only allow the needle to swivel on one axis). When the compass is not parallel to these axis the amount of magnetism felt by the needle will change based on how out of alignment the compass is to these axis.

Remembering that our magnetometer only forms a compass when integrated into a system with some calculations, we would also suffer this problem if we only used the same two axis (X and Y) that the traditional compass uses. Of course, if in our application the compass is always going to be held flat this isn't a problem. You can in this case just create a very simple compass using just a triple axis magnetometer using this earlier tutorial. If however we want to be able to compensate our compass for tilt up to 40 degrees, we will need a way to include in our calculations the third axis, Z, which (when tilted) now collects the magnetic field lost by X and Y when they are tilted out of alignment.

Of course, one cannot simply add this value to the result to get an accurate heading. We first need to know how the device is tilted, so we know how to integrate the Z axis measurement properly, thus correct for our tilt. In other words, we need to know our orientation first. We will do this by incorporating a triple axis accelerometer into our compass system.

Accelerometers

The accelerometer is a device which measures acceleration. Simple enough, but there are two different types of acceleration: static acceleration (tilt) and dynamic acceleration (movement).The accelerometer outputs a measurement telling the user in what direction on its axis force is being applied. The good news is, the accelerometer picks up all kinds of movement, meaning you can get just about anything from it. The bad news is, the accelerometer picks up all kinds of movement, meaning you have no idea what your looking at! Usually when dealing with an accelerometer, we want to get the orientation of the sensor, which basically means which way is the sensor tilted. With one axis accelerometers we can act a bit like a spirit level and measure how one axis is tilted relative to gravity, but by adding more axes into our accelerometer we can begin to figure out our orientation in 3 dimensions.So, how do they work? Well the accelerometer is basically a mass, suspended by a spring. The measurement you are getting from the device is the amount these springs are flexing.This is a illustration of an accelerometer; they are not actually constructed this way, this is just to aid your understanding! Here you can see the mass (in blue) is suspended by four springs attached to the frame. At the moment all these springs are zero, which means no force is being applied to the mass relative to the frame, but this is not actually what you see when your accelerometer is sitting on the desk next to you.

You actually see something more like this: This is because gravity is acting on the mass and is pulling it down. The accelerometer is measuring 1 g because that is the amount of gravity you experience on the surface of the earth. So when you have an accelerometer and you think you are measuring nothing, you are actually measuring the force of gravity!

The accelerometer also measures movement, so if you move the accelerometer from side to side, the result looks like this.

However usually you will never see these results, it's more likely you see the all mashed together. This looks like a big mess, like this:

The accelerometer we will be using is the ADXL345 Triple Axis Accelerometer. This little chip packs triple axis sensing, at up to +/- 16g measurements!

Tilt Compensation EquationSo far we have discussed what a compass is, how tilt affects the results, and that we are going to use an accelerometer to detect our orientation to correct for this. This how we are going to use both the HMC5883L magnetometer and ADXL345 accelerometer to create a tilt compensated compass.

When the device is tilted, pitch and roll angles are not 0°. The diagram below the pitch and roll angles are measured by a 3 axis accelerometer. XM, YM and ZM (the measurement axis on the magnetometer) will be compensated to obtain Xh and Yh. Once have corrected the Xh and Yh measurements, we can use the first equation to calculate our heading.

Xh = XM * cos(Pitch) + ZM * sin(Pitch)Yh = XM * sin(Roll) * sin(Pitch) + YM * cos(Roll) - ZM * sin(Roll) * cos(Pitch)

Once we have tilt compensated Xh and Yh, we can now use the same equation as before to find our heading!

Heading = arctan(Yh/Xh)

SetupOkay, now we know exactly what we want to achive, we can begin creating our compass. For this we will need:

An Arduino Uno or Arduino Mega HMC5883L Triple Axis Magnetometer ADXL345 Triple Axis Accelerometer Two resistors, between 4.7K and 10K. A Green LED A small breadboard and jumper wires.

You'll also need on the software side:

The Arduino development environment . The Love Electronics HMC5883L Arduino Library V2 The Love Electronics ADXL345 Arduino Library V1

Once we have our parts, we can assemble them on the breadboard. Begin by soldering male breakaway headers into your magnetometer and accelerometer and plug them into your breadboard. Then connect the devices to your Arduino by following the instructions below, or take a look at the finished picture.

Connect Arduino GND to HMC5883L GND. Connect Arduino 3.3V to HMC5883L VCC. Connect Arduino Analog 4 (I2C Data) to HMC5883L SDA (Pin 20 on an Arduino Mega). Connect Arduino Analog 5 (I2C Clock) to HMC5883L SCL (Pin 21 on an Arduino Mega). Connect Arduino GND to ADXL345 GND. Connect Arduino 3.3V to ADXL345 VCC, SDO and CS. Connect Arduino 3.3V to ADXL345 SDO. Connect Arduino 3.3V to ADXL345 CS. Connect ADXL345 Analog 4 (I2C Data) to Arduino SDA (Pin 20 on an Arduino Mega). Connect ADXL345 Analog 5 (I2C Clock) to Arduino SCL (Pin 21 on an Arduino Mega). Connect Arduino Analog 4 (I2C Data) to Arduino 3.3V using one of your resistors (Pin 20 on an

Arduino Mega). Connect Arduino Analog 5 (I2C Clock) to Arduino 3.3Vusing one of your resistors (Pin 21 on

an Arduino Mega).

Your done connecting your HMC5883L and ADXL345 to the Arduino. I usually like to include an LED so I know what my sketch is doing just by looking. This step is optional, for this I'm connecting a Green LED to digital pin 2 and GND on my Arduino. (Attach the longer pin on the LED to GND, and the shorter pin to your digital pin).

You should end up with something like this:

ProgrammingNow you have the circuit assembed, we can start programming! Now this part is really easy. We've put all the complicated parts about communication with the ADXL345 Triple Axis Accelerometer and HMC5883L Triple Axis Magnetometer in our libraries, so all you have to do is used sensibly named methods to get the accelerometer and magnetometer to do what you need. Lets start up with a simple sketch to see if you've got the circuit assembled properly.Check out the code below that connects to the triple axis accelerometer and magnetometer and lets us know via the Serial port and our status LED if we can communicate with the devices:

arduino codeStartup Sketch

// Reference the I2C Library#include <Wire.h>// Reference the HMC5883L Compass Library#include <HMC5883L.h>// Reference the ADXL345 Accelerometer Library#include <ADXL345.h>

// Store our compass as a variable.HMC5883L compass;// Store our accelerometer as a variable.ADXL345 accel;// Set up a pin we are going to use to indicate our status using an LED.int statusPin = 2; // I'm using digital pin 2.int areConnected = 0; // Store our connection status here.

void setup(){ Serial.begin(9600); // Initialize the serial port. Wire.begin(); // Start the I2C interface. pinMode(statusPin, OUTPUT); // Ready an LED to indicate our status.

compass = HMC5883L(); // Construct a new HMC5883 compass. accel = ADXL345(); // Construct a new ADXL345 accelerometer. compass.EnsureConnected(); accel.EnsureConnected(); if(compass.IsConnected && accel.IsConnected) { areConnected = true; Serial.println("Connected to HMC5883L and ADXL345."); digitalWrite(statusPin, HIGH); } else { areConnected = false; digitalWrite(statusPin, LOW); if(compass.IsConnected == 0) Serial.println("Could not connect to HMC5883L."); if(accel.IsConnected == 0)

Serial.println("Could not connect to ADXL345."); }}

void loop() {}

You should see this when you open the Serial Port window in the Arduino Enviroment, your status LED should also be lit:

If you see this instead, there is a problem in the circuit, take a look at the instructions above and see if you can resolve whatever the problem is. If you get real stuck, email us at [email protected] and we'd be happy to give you a hand.

Once we know the devices are connected properly, we can move on to getting data out of the devices. But hold on! We haven't told the compass and accelerometer we want any data yet!Add the following code to the bottom of the setup() routine to configure the triple axis magnetometer and triple axis accelerometer:

arduino codeConfigure ADXL345 and HMC5883L

if(areConnected) { compass.SetScale(1.3); // Set the scale of the compass. compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous

accel.SetRange(2, true); // Set the range of the accelerometer to a maximum of 2G. accel.EnableMeasurements(); // Tell the accelerometer to start taking measurements. } if(areConnected) { compass.SetScale(1.3); // Set the scale of the compass. compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous

accel.SetRange(2, true); // Set the range of the accelerometer to a maximum of 2G. accel.EnableMeasurements(); // Tell the accelerometer to start taking measurements. }

Great, now the ADXL345 will know to report on measurements between +2G and -2G; the HMC5883L will report on measurements between +1.3Gauss and -1.3Gauss. Both devices have been told to enable measurements. They are now beavering away measuring acceleration and magnetism. To listen to this data, we are going to replace our loop() routine with this one:

arduino codeLoop Routine

void loop(){ if(areConnected) { MagnetometerScaled magnetometerReadings = compass.ReadScaledAxis(); AccelerometerScaled accelerometerReadings = accel.ReadScaledAxis(); float headingNTC = CalculateHeadingNotTiltCompensated(magnetometerReadings); float headingTC = CalculateHeadingTiltCompensated(magnetometerReadings, accelerometerReadings); // Output the data via the serial port. Output(headingNTC, headingTC); }}

This loop routine reads scaled data from the HMC5883L and ADXL345 into the two variables, magnetometerReadings and accelerometerReadings. The code then calls two methods, CalculateHeadingNotTiltCompensated and CalculateHeadingTiltCompensated which calculate heading using both algorithms. Once complete, it passes the results to an Output method which reports the results to us via Serial. Here is the code for these methods:

arduino codeCalculation and Output Methods

float CalculateHeadingTiltCompensated(MagnetometerScaled mag, AccelerometerScaled acc){ // We are swapping the accelerometers axis as they are opposite to the compass the way we have them mounted. // We are swapping the signs axis as they are opposite. // Configure this for your setup. float accX = -acc.YAxis; float accY = -acc.XAxis; float rollRadians = asin(accY); float pitchRadians = asin(accX); // We cannot correct for tilt over 40 degrees with this algorthem, if the board is tilted as such, return 0. if(rollRadians > 0.78 || rollRadians < -0.78 || pitchRadians > 0.78 || pitchRadians < -0.78) { return 0; } // Some of these are used twice, so rather than computing them twice in the algorithem we precompute them before hand. float cosRoll = cos(rollRadians); float sinRoll = sin(rollRadians); float cosPitch = cos(pitchRadians); float sinPitch = sin(pitchRadians); // The tilt compensation algorithem. float Xh = mag.XAxis * cosPitch + mag.ZAxis * sinPitch; float Yh = mag.XAxis * sinRoll * sinPitch + mag.YAxis * cosRoll - mag.ZAxis * sinRoll * cosPitch; float heading = atan2(Yh, Xh); return heading;}

float CalculateHeadingNotTiltCompensated(MagnetometerScaled mag){ // Calculate heading when the magnetometer is level, then correct for signs of axis. float heading = atan2(mag.YAxis, mag.XAxis); return heading;}

float RadiansToDegrees(float rads){ // Correct for when signs are reversed. if(rads < 0) rads += 2*PI; // Check for wrap due to addition of declination. if(rads > 2*PI) rads -= 2*PI; // Convert radians to degrees for readability. float heading = rads * 180/PI; return heading;}

void Output(float headingNTC, float headingTC){ Serial.print("Heading (Not Compensated):\\t"); Serial.print(RadiansToDegrees(headingNTC)); Serial.print("\\tHeading (Tilt Compensated):\\t"); Serial.println(RadiansToDegrees(headingTC));}

Great! Download this code to your Arduino and press run! You should get the following results from your

serial window:

When the device is kept level, both the compensated and non compensated readings should be very

similar, but when you incline the compass the not compensated should change whilst the tilt

compensated output should remain stable! If this is so, you've succesfully created a tilt compensated

compass!

If you've had trouble following any parts of this tutorial, try downloading the full example sketch, which is

exactly what we have made here.

Now, the compass we have created will only compensate for tilt up to 40 degrees. This is because without

a more complicated algorithem around our accelerometer we are unable to reliably detect the amount of

tilt past 40 degrees.