Arduino compass.

[bigbadbob]

I've been playing around with the code for my compass today so thought I'd post it in case others want to use it.
I originally had a GY26 compass but it wasn't tilt compensated so was pretty much useless.
Now I use a CMPS12 from robot-electronics.co.uk
compass communication is over I2C bus. you can also talk to it over serial but not with this code.
heading/pitch/roll and calibration status are sent over Easy transfer protocol to my top-side controller.

here's the code-

Code: Select all

#include <Wire.h> //I2C library

byte high_byte = 0;
byte low_byte = 0;
byte angle8 = 0;
byte CalStatus = 0;
char pitch = 0;
char roll = 0;
unsigned int angle16 = 0;
int heading = 0;
boolean calDone = false;


void Compass()
{
  Wire.beginTransmission(0x60);  //starts communication with CMPS12 address 0x60
  Wire.write(0x01);              //heading/pitch/roll register 0x01
  Wire.endTransmission();
 
  // Request 5 bytes from the CMPS12
  // this will give us the 8 bit bearing, both bytes of the 16 bit bearing, pitch and roll
  Wire.requestFrom(0x60, 5);       
  if(Wire.available())
  {
  while(Wire.available() < 5);        // Wait for all 5 bytes to come back
  
  angle8 = Wire.read();      //byte1           
  high_byte = Wire.read(); //byte2
  low_byte = Wire.read(); //byte3
  pitch = Wire.read();      //byte4
  roll = Wire.read();        //byte5
  
  angle16 = high_byte;                 // Calculate 16 bit angle
  angle16 <<= 8;
  angle16 += low_byte;
  }
  heading = angle16/10;// use angle16, it gives better resolution than angle8.
 
  txdata.heading = heading; //0-359//set up the data to be sent to top-side
  txdata.pitch = pitch; //+/-90
  txdata.roll = roll; //+/-90
  
  //now get cmps12 cal status
  Wire.beginTransmission(0x60);  //starts communication with CMPS12 address 0x60
  Wire.write(0x1E);              //cal status register 0x1E
  Wire.endTransmission();
  Wire.requestFrom(0x60, 1);  // Request 1 bytes from the CMPS12     
  if(Wire.available())
  {
  while(Wire.available() < 1);        // Wait for the byte to come back
  CalStatus = Wire.read();               // Read the byte to get cal status
  }
  if ((CalStatus&0x3F)==0x3F)  // acc/mag/gyro cal are valid
  {calDone = true;}
  else
  {calDone = false;}

"&" computes a binary "and" of CalStatus and 0x3F, if the result is 0x3F this shows that accellerometer/magnetometer and gyro are calibrated.
ie: if register 0x1E returns 0x3F (xx111111) then cal is good enough. (x= don't care)
if full system cal is good, register 0x1E returns 0xFF (11111111) but my results are inconsistent. I don't use it.

you can call the following Voids to save and erase compass calibration profiles.

Code: Select all

void Cal()// store new compass cal.
 {
  if (calDone) // if calibration is valid, save it.
  {
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //command register 0x00
  Wire.write(0xF0);    //sends the first byte to save the cal settings
  Wire.endTransmission();
  delay(20);
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //Sends the register we wish to write to
  Wire.write(0xF5);    //sends the second byte to save the cal settings
  Wire.endTransmission();
  delay(20);
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //Sends the register we wish to write to
  Wire.write(0xF6);    //sends the third byte to save the cal settings
  Wire.endTransmission();
  delay(50);
 }
}

Code: Select all

void EraseCal()// erase old compass cal.
 {
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //command register 0x00
  Wire.write(0xE0);    //sends the first byte to erase the cal settings
  Wire.endTransmission();
  delay(20);
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //Sends the register we wish to write to
  Wire.write(0xE5);    //sends the second byte to erase the cal settings
  Wire.endTransmission();
  delay(20);
  Wire.beginTransmission(0x60);  //starts communication with CMPS12
  Wire.write(0x00);                //Sends the register we wish to write to
  Wire.write(0xE2);    //sends the third byte to erase the cal settings.
  Wire.endTransmission();
  delay(50);
}