Pages: [1]   Go Down
Author Topic: Problem with HMC5883L  (Read 1527 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
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
Logged

Offline Offline
Edison Member
*
Karma: 58
Posts: 2078
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Do you have a compass ? A real one with a needle ? To compare with HMC5883L ?
If the HMC5883L is near your computer or near wires or near a power supply, the magnetic direction would be influenced a lot.

You can use a long USB cable, and carefully rotate the sensor, and see what the output is.
Perhaps you could even enlarge the distance to the other components.

The gyro will drift, but not that fast. Normally the magnetometer and the gyro combined in a filter will result in a stable heading.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Unfortunately, I don't have a compass. I don't think the computer influences the direction that much. When using the MultiWii (http://www.multiwii.com/) software, the magnetometer works perfectly.

That's exactly what I'm going to do. I now use a Kalman filter for roll and pitch, and want to expand the filter for yaw
Logged

Rapa Nui
Offline Offline
Edison Member
*
Karma: 60
Posts: 2086
Pukao hats cleaning services
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

The HMC5883L must be calibrated..
Google "HMC5883 calibration"
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I found this on magnetometer calibration:

Quote
Basically, you need to calibrate the compass. Have the raw X and Y data display on the serial terminal.
rotate the compass to find the max X value, then rotate the compass 180 degrees and note this value. Also note the value of Y at this point.
You need to offset the raw X and Y values, so that the magnitude of max X and Min X are similar. You also need to have the Y value near zero, as the X value is near its max or min value.
Same goes for X , as Y is near it's min or max value X should be close to zero. Just keep adjusting the offset so you get as close as possible.

One problem is that the Y-value doesn't change much when i rotate the magnetometer about the Z-axis. The X-value changes from approx. -40 to 220, while the Y-value changes from -380 to -400. Is this normal?
Logged

Rapa Nui
Offline Offline
Edison Member
*
Karma: 60
Posts: 2086
Pukao hats cleaning services
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

The only lib I found which works somehow and does include calibration is:
https://github.com/rob42/FreeIMU-20121106_1323/blob/master/libraries/HMC58X3/examples/HMC58X3_basic/HMC58X3_basic.pde

https://github.com/rob42/FreeIMU-20121106_1323/tree/master/libraries/HMC58X3
« Last Edit: May 11, 2013, 10:54:29 am by pito » Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Thank you, Pito

With the help from the library, I finally got it working. Here is how I fixed it.

Based on the code provided by Pito, I made a 2D scatter plot. From the figure to the left, we see that the origin of the circle is not in (0,0). By subtracting the offset values, I was able to move the circle to (0,0). Now, by using atan2(y,x) the heading was calculated and it worked smiley
Figure: http://bildr.no/view/1613557
Now I just have to figure out what I did wrong and they did right  smiley-razz And then, tilt compensation  smiley-lol
Logged

Rapa Nui
Offline Offline
Edison Member
*
Karma: 60
Posts: 2086
Pukao hats cleaning services
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

I played a lot with the chip in past and my lessons learned are as follows:
1. do not put a strong magnet close to the chip..
2. metallic objects (even small) close to the chip will destroy your scatter
3. mag field in a living room is not defined..
smiley
Logged

Pages: [1]   Go Up
Jump to: