Robotic Compass Blues

bm004_compass

Whew!  Just spent a good four hours writing code that turned out to be pretty simple.  We sell the BM004 electronic compass that’s great for robotics.  But I got the robotic compass blues.

The BM004 is based on ST Micro’s LSM303DLHC (the datasheet is here in our design files).  The module has a 3D magnetometer, and a 3D accelerometer, both accessed through an I2C interface.  ST Micro’s got a nice application note showing how to convert the signals from the sensor combo to a tilt compensated compass (get a copy of it here).  Based on the application note I wrote some simple code for an Ardunio Uno to get a heading reading that wasn’t tilt-compensated.  Here’s that code.

/*
BM004_Arduino_compass_simple:  This program reads the magnetometer registers 
from ST Micro's LSM303DLHC.  The register values are used to generate a heading
value.  NOTE:  If the IC is not level the heading values will not be accurate.

Schematics associated with the BM004 moudle may be used for hardware wiring information.
see www.solutions-cubed.com for additional information.

*/

#include <Wire.h>
#include <Math.h>

    typedef struct field
    {
      int x, y, z;
      byte xl,xh,yl,yh,zl,zh;
    } field;

    field mag; // magnetometer register values
    float Heading;

/*  
Send register address and the byte value you want to write the magnetometer and 
loads the destination register with the value you send
*/
void WriteMagRegister(byte data, byte regaddress)
{
    Wire.beginTransmission(0x1E);   // Else use magnetometer address
    Wire.write(regaddress);
    Wire.write(data);  
    Wire.endTransmission();     
}

/*  
Send register address to this function and it returns byte value
for the magnetometer register's contents 
*/
byte ReadMagRegister(byte regaddress)
{
    byte data;
    Wire.beginTransmission(0x1E);   // Else use magnetometer address  
    Wire.write(regaddress);
    Wire.endTransmission();

    Wire.requestFrom(0x1E,1);   // Else use magnetometer address
    data = Wire.read();
    Wire.endTransmission();   
    return data;  
}  

void init_Compass(void)
{
    WriteMagRegister(0x94,0x00);  // Enable temperature sensor, set output to 30Hz
    WriteMagRegister(0xe0,0x01);  // set gain to +/-8.1Gauss
    WriteMagRegister(0x00,0x02);  // Enable magnetometer constant conversions
}

/*
Reads the X,Y,Z axis values from the magnetometer sends the values to the 
serial monitor.
*/
void get_Magnetometer(void)
{
  // magnetometer values
  mag.xh = ReadMagRegister(0x03);
  mag.xl = ReadMagRegister(0x04);
  mag.yh = ReadMagRegister(0x07);
  mag.yl = ReadMagRegister(0x08);
  mag.zh = ReadMagRegister(0x05);
  mag.zl = ReadMagRegister(0x06);

  // convert to ints
  mag.x = (int)(mag.xh<<8|mag.xl); 
  mag.y = (int)(mag.yh<<8|mag.yl); 
  mag.z = (int)(mag.zh<<8|mag.zl);

  // send register values to the serial monitor 
/*
  Serial.print("Mag XAxis=");
  Serial.print(mag.x,DEC);  
  Serial.print(" ");
  Serial.print("Mag YAxis=");
  Serial.print(mag.y,DEC);  
  Serial.print(" ");
  Serial.print("Mag ZAxis=");
  Serial.print(mag.z,DEC);    
  Serial.println("");    
*/
}  

void get_Heading(void)
{
  // arctangent of y/x converted to degrees
  Heading = 180*atan2((double)mag.y,(double)mag.x)/PI;

  if (Heading < 0)
      Heading +=360;

  // send register values to the serial monitor 
    Serial.print("Heading=");
    Serial.println(Heading, 4);    
}  

void setup() {
    Wire.begin();
    Serial.begin(9600);  // start serial for output
    init_Compass();
}

void loop() {
    get_Magnetometer();
    get_Heading();
    delay(100);
}

You can download the file for your use here.    ST Micro is pretty explicit (in the app note) about the need for the IC to be on a flat level surface for it to provide accurate readings without the tilt compensation.  I didn’t have my IC entirely flat and was off by 20 degrees in one direction.  I was only looking at the range over 180 degrees, and it was accurate at the end points.  I spent forever checking and rechecking my code before I figured out I just had it mounted wrong.  That was fun.

I guess the thing to do now is write the code for tilt compensation.  I know there’s some on the web, but I’ll take a stab at writing it myself.

Speak Your Mind

*