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;}
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);
}