Hi guys,
I've have a 6DOF IMU from sparkfun (ITG3200/ADXL345) that is working great. In order to compensate for the drift in yaw, I purchased a magnetometer HMC5883L. I'm able to get data from the sensor, but the heading is completely wrong.
Here is a plot showing the magnetometer turn 360 degrees around, which clearly is wrong.
http://bildr.no/view/1613333
The magnetometer and IMU are connected as shown in this figure
http://bildr.no/view/1613335
My code:
#include <Wire.h>
int gyroResult[3], accelResult[3], magnetResult[3];
float dt = 0.01;
float loopTime=100;
float biasGyroX, biasGyroY, biasGyroZ, biasAccelX, biasAccelY, biasAccelZ, biasMagnetX, biasMagnetY, biasMagnetZ;
float ax, ay, az;
float pitchAccel = 0;
float rollAccel = 0;
float angle_roll;
float angle_pitch;
float alpha=0.98;
float rollGyro = 0;
float pitchGyro = 0;
float mx, my, mz, hx, hy, hz;
float scale = 0.92;
float declination = 133*PI/10800;
float yawMagnet = 0;
unsigned long loopStartTime = 0;
void setup() {
int totalGyroXValues = 0;
int totalGyroYValues = 0;
int totalGyroZValues = 0;
int totalAccelXValues = 0;
int totalAccelYValues = 0;
int totalAccelZValues = 0;
int totalMagnetXValues = 0;
int totalMagnetYValues = 0;
int totalMagnetZValues = 0;
int i;
Wire.begin();
Serial.begin(115200);
writeTo(0x53,0x31,0x09); //Set accelerometer to 11bit, +/-4g
writeTo(0x53,0x2D,0x08); //Set accelerometer to measure mode
writeTo(0x68,0x16,0x1A); //Set gyro to +/-2000deg/sec and 98Hz low pass filter
writeTo(0x68,0x15,0x09); //Set gyro to 100Hz sample rate
writeTo(0x1E,0x01,0x20); //Set magnetometer range +/- 1.3 Ga
writeTo(0x1E,0x02,0x00); //Set magnetometer to continuous mode
delay(100); //wait for gyro
for (i=0; i<50; i++) {
getGyroscopeReadings(gyroResult);
getAccelerometerReadings(accelResult);
totalGyroXValues += gyroResult[0];
totalGyroYValues += gyroResult[1];
totalGyroZValues += gyroResult[2];
totalAccelXValues += accelResult[0];
totalAccelYValues += accelResult[1];
totalAccelZValues += accelResult[2];
delay(50);
}
biasGyroX = totalGyroXValues / 50;
biasGyroY = totalGyroYValues / 50;
biasGyroZ = totalGyroZValues / 50;
biasAccelX = totalAccelXValues / 50;
biasAccelY = totalAccelYValues / 50;
biasAccelZ = (totalAccelZValues / 50) - 256;
//Serial.print("Pitch gyro\tPitch accel\tPitch Kalman\t");
//Serial.print("Roll gyro\tRoll accel\tRoll Kalman\n");
}
void loop() {
dt/=1000;
getGyroscopeReadings(gyroResult);
getAccelerometerReadings(accelResult);
getMagnetometerReadings(magnetResult);
ax=(accelResult[0] - biasAccelX) / 256;
ay=(accelResult[1] - biasAccelY) / 256;
az=(accelResult[2] - biasAccelZ) / 256;
mx=(magnetResult[0])*scale;
my=(magnetResult[1])*scale;
mz=(magnetResult[2])*scale;
//Calculate pitch angle and angle velocity
rollAccel = atan(ay / az) * (180 / PI);
rollGyro = rollGyro + ((gyroResult[1] - biasGyroY) / 14.375) * dt;
//Calculate pitch angle and angle velocity
pitchAccel = -atan(ax / sqrt(ay*ay+az*az)) * (180.0 / PI);
pitchGyro = pitchGyro + ((gyroResult[0] - biasGyroX) / 14.375) * dt;
//Calculate yaw angle
yawMagnet = atan2(my , mx)*(180/PI);
yawMagnet += (declination * 180/PI);
//Mapping
if(yawMagnet<0) yawMagnet += 360;
else if(yawMagnet>360) yawMagnet -= 360;
//Serial monitor
Serial.print(rollAccel);
Serial.print("\t");
Serial.print(pitchAccel);
Serial.print("\t");
Serial.print(yawMagnet);
Serial.println();
dt = millis() - loopStartTime;
if(dt<100) delay(loopTime-dt);
loopStartTime = millis();
}
/**************************************************************************************/
/*************** Functions ********************/
/**************************************************************************************/
void writeTo(byte device, byte toAddress, byte val) {
Wire.beginTransmission(device);
Wire.write(toAddress);
Wire.write(val);
Wire.endTransmission();
}
void readFrom(byte device, byte fromAddress, int num, byte result[]) {
Wire.beginTransmission(device);
Wire.write(fromAddress);
Wire.endTransmission();
Wire.requestFrom((int)device, num);
int i = 0;
while(Wire.available()) {
result[i] = Wire.read();
i++;
}
}
void getGyroscopeReadings(int gyroResult[]) {
byte buffer[6];
readFrom(0x68,0x1D,6,buffer);
gyroResult[0] = (((int)buffer[0]) << 8 ) | buffer[1];
gyroResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
gyroResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
}
void getAccelerometerReadings(int accelResult[]) {
byte buffer[6];
readFrom(0x53,0x32,6,buffer);
accelResult[0] = (((int)buffer[1]) << 8 ) | buffer[0];
accelResult[1] = (((int)buffer[3]) << 8 ) | buffer[2];
accelResult[2] = (((int)buffer[5]) << 8 ) | buffer[4];
}
void getMagnetometerReadings(int magnetResult[]) {
byte buffer[6];
readFrom(0x1E,0x03,6,buffer);
magnetResult[0] = (((int)buffer[0]) << 8 ) | buffer[1];
magnetResult[1] = (((int)buffer[2]) << 8 ) | buffer[3];
magnetResult[2] = (((int)buffer[4]) << 8 ) | buffer[5];
}
Regards
Sondre