HMC5883L.cpp

00001 /*   HMC5883L Digital Compass Library
00002 *
00003 *    @author: Baser Kandehir 
00004 *    @date: August 5, 2015
00005 *    @license: MIT license
00006 *     
00007 *   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
00008 *
00009 *   Permission is hereby granted, free of charge, to any person obtaining a copy
00010 *   of this software and associated documentation files (the "Software"), to deal
00011 *   in the Software without restriction, including without limitation the rights
00012 *   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00013 *   copies of the Software, and to permit persons to whom the Software is
00014 *   furnished to do so, subject to the following conditions:
00015 *
00016 *   The above copyright notice and this permission notice shall be included in
00017 *   all copies or substantial portions of the Software.
00018 *
00019 *   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00020 *   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00021 *   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00022 *   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00023 *   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00024 *   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00025 *   THE SOFTWARE.
00026 *
00027 */
00028 
00029 // Some part of the code is adapted from Adafruit HMC5883 library
00030 
00031 #include "HMC5883L.h"
00032 
00033 /* NUCLEO F411RE board */
00034 static I2C i2c(D14, D15);         // setup i2c (SDA,SCL)  
00035 
00036 static float Gauss_LSB_XY = 1100.0F; // Varies with gain
00037 static float Gauss_LSB_Z =  980.0F;  // Varies with gain
00038  
00039 void HMC5883L::setMagGain(MagGain gain)
00040 {
00041     writeByte(HMC5883L_ADDRESS, CONFIG_B, (int8_t)gain);    
00042     
00043     switch(gain)
00044     {
00045         case MagGain_13:
00046             Gauss_LSB_XY = 1100;
00047             Gauss_LSB_Z  = 980;
00048             break;
00049         case MagGain_19:
00050             Gauss_LSB_XY = 855;
00051             Gauss_LSB_Z  = 760;
00052             break;
00053         case MagGain_25:
00054             Gauss_LSB_XY = 670;
00055             Gauss_LSB_Z  = 600;
00056             break;
00057         case MagGain_40:
00058             Gauss_LSB_XY = 450;
00059             Gauss_LSB_Z  = 400;
00060             break;
00061         case MagGain_47:
00062             Gauss_LSB_XY = 400;
00063             Gauss_LSB_Z  = 255;
00064             break;
00065         case MagGain_56:
00066             Gauss_LSB_XY = 330;
00067             Gauss_LSB_Z  = 295;
00068             break;
00069         case MagGain_81:
00070             Gauss_LSB_XY = 230;
00071             Gauss_LSB_Z  = 205;
00072             break;
00073     }  
00074 }
00075 
00076 void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
00077 {
00078     char data_write[2];
00079     data_write[0]=regAddress;           // I2C sends MSB first. Namely  >>|regAddress|>>|data|
00080     data_write[1]=data;
00081     i2c.write(address,data_write,2,0);  // i2c.write(int address, char* data, int length, bool repeated=false);  
00082 }
00083 
00084 char HMC5883L::readByte(uint8_t address, uint8_t regAddress)
00085 {
00086     char data_read[1];                   // will store the register data    
00087     char data_write[1];
00088     data_write[0]=regAddress;
00089     i2c.write(address,data_write,1,1);   // repeated = true
00090     i2c.read(address,data_read,1,0);     // read the data and stop
00091     return data_read[0];
00092 } 
00093 
00094 void HMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
00095 {
00096     char data[10],data_write[1];  
00097     data_write[0]=regAddress;      
00098     i2c.write(address,data_write,1,1);
00099     i2c.read(address,data,byteNum,0);
00100     for(int i=0;i<byteNum;i++)          // equate the addresses
00101         dest[i]=data[i];
00102 }
00103 
00104 void HMC5883L::init()
00105 {
00106     writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78);  // Number of samples averaged: 8, Data output rate: 75 Hz
00107     writeByte(HMC5883L_ADDRESS, MODE,     0x00);  // Continuous-Measurement Mode
00108     setMagGain(MagGain_13);
00109     wait_ms(10);
00110 }
00111 
00112 void HMC5883L::readMagData(float* dest)
00113 {
00114     uint8_t rawData[6]; // x,y,z mag data
00115     
00116     /* Read six raw data registers sequentially and write them into data array */
00117     readBytes(HMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]); 
00118     
00119     /* Turn the MSB LSB into signed 16-bit value */
00120     dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]);  // MAG_XOUT
00121     dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // MAG_ZOUT
00122     dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // MAG_YOUT 
00123     
00124     /* Convert raw data to magnetic field values in microtesla */
00125      dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
00126      dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
00127      dest[2] = dest[2] / Gauss_LSB_Z  * GAUSS_TO_MICROTESLA;
00128 }
00129 
00130 double HMC5883L::getHeading()
00131 {
00132     float magData[3];
00133     readMagData(magData);
00134     
00135     /* Calculate the heading while Z axis of the module is pointing up */
00136     double heading = atan2(magData[1], magData[0]);
00137     
00138     // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
00139     // declinationAngle can be found here http://www.magnetic-declination.com/
00140     // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
00141     float declinationAngle = 0.096;
00142     heading += declinationAngle;
00143     
00144     // Correct for when signs are reversed.
00145     if(heading < 0)
00146         heading += 2*PI;
00147     
00148     // Check for wrap due to addition of declination.
00149     if(heading > 2*PI)
00150         heading -= 2*PI;
00151     
00152     /* Convert radian to degrees */
00153     heading = heading * 180 / PI;  
00154     
00155     return heading;    
00156 }