Sunday, 21 April 2013

Compass Fun And Games..

After i got an anonymous tip from the person i am calling Anonymous Genius #102, i have been playing with my project that has a compass in it. i have some weird results when i try and compensate for the unit being tilted. i have tried following a few tutorials but they all seem to give different results. The compass on its own on a flat surface gives nice 0-360 degrees and 0-6 radians.

I have my code that runs ok if i tilt it up and down (Pitch the unit) the accelerometer seems to compensate correctly, but when tilt it side to side (Roll the unit) the accelerometer seems to overpower the compass and the results vary wildly. it looks like the accelerometer is the only input.

My device is to be hand held and waved around (up/down, left/right). I need to know the following things.

heading (what direction the device is pointed)
Inclination (the angle to the ground)
how far the unit has moved ( 1 meters to the south 0.5 meters to the east and  3 meters away from the ground)

these bits of data will be used to display where the unit has moved. so if it was mounted to a robot it would be able to tell where in a room it was, what direction its pointing, and if i have enough processing power left over keep a map of where it is and has been.

my code is below if anyone can point out where i stuffed up (it has to be in there somewhere, i just cant see it) drop me a line and set me straight..

 there are a bunch of global variables that are defined else where in the project, this is the code that deals with the compass..

When Get_Reading(float,float) is called you need to supply the Pitch (Pr) and Roll (Rr) values in radians.

  // Reference the I2C Library
#include <Wire.h>
  // Reference the HMC5883L Compass Library
#include <HMC5883L.h>
  int error;
  HMC5883L compass;
  // Store our compass as a variable.
  void COMPASS_INIT(void)
     pinMode(DRDY, INPUT);
    Wire.begin(); // Start the I2C interface.
  compass = HMC5883L(); // Construct a new HMC5883 compass.
  //valid gauss values are: 0.88, 1.3, 1.9, 2.5, 4.0, 4.7, 5.6, 8.1
  error = compass.SetScale(2.5); // Set the scale of the compass.
   if(error != 0) // If there is an error, print it out.
        error = compass.SetScale(1.3); // Set the scale of the compass.
           if(error != 0) // If there is an error, print it out.
  error = compass.SetMeasurementMode(Measurement_Continuous); // Set the measurement mode to Continuous
    if(error != 0) // If there is an error, print it out.

float Get_Heading(float Pr,float Rr)
  while (DRDY == 0);    // loops while compass is busy, i soldered a wire to the DRDY pin of the chip
  // I2c request for current heading 6 bytes to make INT X Y Z
  // do maths to get headings combine pitch and roll to compensate for how the device is being held
  // Retrive the raw values from the compass (not scaled).
  MagnetometerRaw raw = compass.ReadRawAxis();
  // Retrived the scaled values from the compass (scaled to the configured scale).
  MagnetometerScaled scaled = compass.ReadScaledAxis();
  // Values are accessed like so:
  //  int MilliGauss_OnThe_XAxis = scaled.XAxis;// (or YAxis, or ZAxis)

  // check to see if pitch or roll are not past 40' , the formula cant handle more than 40 degrees tilt
  if(Rr > 0.78 || Rr < -0.78 || Pr > 0.78 || Pr < -0.78)
    return headingDegrees;   // if unit is tilted too far return previous value

  float cosRoll = cos(Rr);
  float sinRoll = sin(Rr); 
  float cosPitch = cos(Pr);
  float sinPitch = sin(Pr);

  // The tilt compensation algorithem.
  Xh = scaled.XAxis * cosPitch + scaled.ZAxis * sinPitch;
  Yh = scaled.XAxis * sinRoll * sinPitch + scaled.YAxis * cosRoll - scaled.ZAxis * sinRoll * cosPitch;
  // Calculate heading when the magnetometer is level, then correct for signs of axis.
  // Tilt compensated result
  float heading = atan2(Yh,Xh);
  // Once you have your heading, you must then add your 'Declination Angle', which is the 'Error' of the magnetic field in your location.
  // Find yours here:
  // sydney Magnetic declination: 12° 31' EAST positive inclination: -64° 16'
  //                            : 0.215024564 rads
  // Mine is: 2ÔøΩ 37' W, which is 2.617 Degrees, or (which we need) 0.0456752665 radians, I will use 0.0457
  // If you cannot find your Declination, comment out these two lines, your compass will be slightly off.
  float declinationAngle = 0.0215024564;
  heading += declinationAngle;
  // Correct for when signs are reversed.
  if(heading < 0.0)
    heading += 2.0*PI;
  // Check for wrap due to addition of declination.
  if(heading > 2.0*PI)
    heading -= 2.0*PI;
  // Convert radians to degrees for readability.
  headingRadians = heading;
  headingDegrees = headingRadians * 180.0/PI;

// dump values to the serial port for debugging.
  #ifdef DEBUG_head      
  if (Serial.available()==1)
  return headingDegrees;

UPDATE - Looking over my hardware for possible answers to the above problem, i noticed that the Accelerometer and compass IC's are not mounted in the same directions.. this could be the source of my problem.. basically I Bread boarded the two breakout boards so XY on the compass is YX on the Accelerometer, A bit of a facepalm moment... I will rotate the Compass (it has less pins to re-wire) and post an update with the results.. I probably could sort this out in software but its only 5 wires

No comments:

Post a Comment