The magnetometer arrived on Thursday (yay for Adafruit and their speedy shipping!); today I spent most of the day getting it working.

There were a few issues which I needed to overcome.

First, I had to read the raw values over i2c.  This is easy enough; the only gotcha was that I didn't read the data sheet close enough, and I failed to notice that the axis are read in X, Z, Y order rather than X, Y, Z. (Really?!  I wonder if there is a technical reason for this ordering, or what ).

After I got the raw values, I used atan2 to convert the x,y co-ordinates into an angle (heading).  The problem here was that (probably due to the magnetic fields of the servo motors) the Y axis was shifted very far down.  This meant that all the angles were being read as negative.  See a graph of the raw values below:

By adding a constant offset to x and y, we got the readings to be properly centered around the origin.

Next challenge was that I am operating in a magnetically noisy environment (there are 18 servos within centimeters of the magnetometer).  Implementing a low pass filter against headings is actually much harder than you would think.  (You have to remember the 'wrap around' at +/- PI).  In the end, I apply a low pass filter on the raw x, y co-ordinates.  A timer polls the magnetometer multiple times a second to ensure that the filtered heading is always accurate and up to date.

 Finally, once I had accurate, filtered headings available at any time, I was able to implement a simple PID algorithm which compares the actual heading to the desired heading and makes any required corrections to keep the robot on course.  This works very well: see for instance the video below, demonstrating the same programmed walk with and without using the magnetometer for course correction.

Some of my next steps include:

•  Use the magnetometer when turning in place to ensure that we turn to the correct angle
• Add an auto calibration routine, which will be saved to EEPROM, so that we don't need to hard code the x,y offsets (which will likely be different for each robot anyway).
• Provide access to the raw magnetometer heading from the Processing API (corrected for mounting angle)