HMC5883.cpp

00001 #include "HMC5883.h"
00002 
00003 HMC5883::HMC5883(PinName sda, PinName scl) : I2C_Sensor(sda, scl, HMC5883_I2C_ADDRESS)
00004 {   
00005     #warning these three offsets are calibration values to get |MAX| = |MIN|
00006     offset[0] = 189.5; // offset calculated by hand... (min + ((max - min) / 2)
00007     offset[1] = -92.5; // TODO: make this automatic with saving to filesystem
00008     offset[2] = 29;
00009     
00010     // load calibration values
00011     //loadCalibrationValues(scale, 3, "COMPASS_SCALE.txt");
00012     //loadCalibrationValues(offset, 3, "COMPASS_OFFSET.txt");
00013     
00014     // initialize HMC5883
00015     writeRegister(HMC5883_CONF_REG_A, 0x78);                // 8 samples, 75Hz output, normal mode
00016     //writeRegister(HMC5883_CONF_REG_A, 0x19);              // 8 samples, 75Hz output, test mode! (should get constant values from measurement, see datasheet)
00017     writeRegister(HMC5883_CONF_REG_B, 0x20);                // Gain for +- 1.3 gauss (earth compass ~0.6 gauss)
00018     writeRegister(HMC5883_MODE_REG, 0x00);                  // continuous measurement-mode
00019 }
00020 
00021 void HMC5883::read()
00022 {
00023     readraw();
00024     for(int i = 0; i < 3; i++)
00025         data[i] = (float)(raw[i]) - offset[i];
00026 }
00027 
00028 void HMC5883::calibrate(int s)
00029 {
00030     int Min[3];                                             // values for achieved maximum and minimum amplitude in calibrating environment
00031     int Max[3];
00032     
00033     Timer calibrate_timer;                                  // timer to know when calibration is finished
00034     calibrate_timer.start();
00035     
00036     while(calibrate_timer.read() < s)                       // take measurements for s seconds
00037     {
00038         readraw();
00039         for(int i = 0; i < 3; i++) {
00040             Min[i] = Min[i] < raw[i] ? Min[i] : raw[i];      // after each measurement check if there's a new minimum or maximum
00041             Max[i] = Max[i] > raw[i] ? Max[i] : raw[i];
00042         }
00043     }
00044     
00045     for(int i = 0; i < 3; i++) {
00046         offset[i] = (float)Min[i] + ((Max[i] - Min[i]) / 2);
00047         //scale[i]= 2000 / (float)(Max[i]-Min[i]);            // calculate scale and offset out of the measured maxima and minima
00048         //offset[i]= 1000 - (float)(Max[i]) * scale[i];       // the lower bound is -1000, the higher one 1000
00049     }
00050     
00051     //saveCalibrationValues(scale, 3, "COM_SCALE");   // save new scale and offset values to flash
00052     //saveCalibrationValues(offset, 3, "COM_OFFSE");
00053 }
00054 
00055 void HMC5883::readraw()
00056 {
00057     char buffer[6];                                         // 8-Bit pieces of axis data
00058     
00059     readMultiRegister(HMC5883_DATA_OUT_X_MSB, buffer, 6);   // read axis registers using I2C
00060     
00061     raw[0] = (short) (buffer[0] << 8 | buffer[1]);          // join 8-Bit pieces to 16-bit short integers
00062     raw[1] = (short) (buffer[4] << 8 | buffer[5]);          // X, Z and Y (yes, order is stupid like this, see datasheet)
00063     raw[2] = (short) (buffer[2] << 8 | buffer[3]);
00064 }
00065 
00066 float HMC5883::get_angle()
00067 {
00068     #define RAD2DEG     57.295779513082320876798154814105
00069 
00070     float Heading;
00071     
00072     Heading = RAD2DEG * atan2(data[0],data[1]);
00073     Heading += 1.367;                   // correction of the angle between geographical and magnetical north direction, called declination
00074                                         // if you need an east-declination += DecAngle, if you need west-declination -= DecAngle
00075                                         // for me in Switzerland, Bern it's ca. 1.367 degree east
00076                                         // see:     http://magnetic-declination.com/
00077                                         // for me:  http://www.swisstopo.admin.ch/internet/swisstopo/de/home/apps/calc/declination.html
00078     if(Heading < 0)  
00079         Heading += 360;                 // minimum 0 degree
00080         
00081     if(Heading > 360)   
00082         Heading -= 360;                 // maximum 360 degree
00083 
00084     return Heading;
00085 }
00086     
00087