HMC5883L.cpp

00001 /*
00002  * @file HMC5883L.cpp
00003  * @author Oskar Lopez de Gamboa
00004  *
00005  * @section LICENSE
00006  *
00007  * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
00008  * and associated documentation files (the "Software"), to deal in the Software without restriction,
00009  * including without limitation the rights to use, copy, modify, merge, publish, distribute,
00010  * sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is
00011  * furnished to do so, subject to the following conditions:
00012  *
00013  * The above copyright notice and this permission notice shall be included in all copies or
00014  * substantial portions of the Software.
00015  *
00016  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
00017  * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
00018  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
00019  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00020  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00021  *
00022  * @section DESCRIPTION
00023  *
00024  * HMC5883L 3-Axis Digital Compas IC
00025  * The library done by Tyler Weaver with:
00026  *
00027  *  *Corrected the XZY order instead of the previous XYZ to match the datasheet.
00028  *  *Added Declination compensation by a define
00029  * 
00030  */
00031 
00032 #include "HMC5883L.h"
00033 #include <new>
00034 
00035 HMC5883L::HMC5883L(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
00036 {
00037     // Placement new to avoid additional heap memory allocation.
00038     new(i2cRaw) I2C(sda, scl);
00039     
00040     init();
00041 }
00042 
00043 HMC5883L::~HMC5883L()
00044 {
00045     // If the I2C object is initialized in the buffer in this object, call destructor of it.
00046     if(&i2c_ == reinterpret_cast<I2C*>(&i2cRaw))
00047         reinterpret_cast<I2C*>(&i2cRaw)->~I2C();
00048 }
00049 
00050 void HMC5883L::init()
00051 {
00052     // init - configure your setup here
00053     setConfigurationA(AVG8_SAMPLES | OUTPUT_RATE_15); // 8 sample average, 15Hz, normal mode
00054     setConfigurationB(0x20); // default gain
00055     setMode(CONTINUOUS_MODE); // continuous sample mode
00056     wait(0.006);//wait 6ms as told in the datasheet
00057 }
00058 
00059 void HMC5883L::setConfigurationA(char config)
00060 {
00061     char cmd[2];
00062     cmd[0] = CONFIG_A_REG; // register a address
00063     cmd[1] = config;
00064     
00065     i2c_.write(I2C_ADDRESS, cmd, 2);
00066 }
00067 
00068 void HMC5883L::setConfigurationB(char config)
00069 {
00070     char cmd[2];
00071     cmd[0] = CONFIG_B_REG; // register b address
00072     cmd[1] = config;
00073     
00074     i2c_.write(I2C_ADDRESS, cmd, 2);
00075 }
00076 
00077 char HMC5883L::getConfigurationA()
00078 {
00079     char cmd[2];
00080     cmd[0] = CONFIG_A_REG; // register a address
00081     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00082     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00083     return cmd[1];
00084 }
00085 
00086 char HMC5883L::getConfigurationB()
00087 {
00088     char cmd[2];
00089     cmd[0] = CONFIG_A_REG; // register b address
00090     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00091     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00092     return cmd[1];
00093 }
00094 
00095 void HMC5883L::setMode(char mode = SINGLE_MODE)
00096 {
00097     char cmd[2];
00098     cmd[0] = MODE_REG; // mode register address
00099     cmd[1] = mode;
00100     i2c_.write(I2C_ADDRESS,cmd,2);
00101 }
00102 
00103 char HMC5883L::getMode()
00104 {
00105     char cmd[2];
00106     cmd[0] = MODE_REG; // mode register
00107     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00108     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00109     return cmd[1];
00110 }
00111 
00112 char HMC5883L::getStatus()
00113 {
00114     char cmd[2];
00115     cmd[0] = STATUS_REG; // status register
00116     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00117     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00118     return cmd[1];
00119 }
00120 
00121 void HMC5883L::getXYZ(int16_t output[3])
00122 {
00123     char cmd[2];
00124     char data[6];
00125     cmd[0] = 0x03; // starting point for reading
00126     i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
00127     i2c_.read(I2C_ADDRESS, data, 6, false);
00128     
00129     for(int i = 0; i < 3; i++) // fill the output variables
00130         output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
00131 }
00132 
00133 double HMC5883L::getHeadingXY()
00134 {
00135     int16_t raw_data[3];
00136     getXYZ(raw_data);
00137     //The  HMC5883L gives X Z Y order
00138     double heading = atan2(static_cast<double>(raw_data[2]), static_cast<double>(raw_data[0])); // heading = arctan(Y/X)
00139     
00140     
00141     heading += DECLINATION_ANGLE;
00142     
00143     if(heading < 0.0) // fix sign
00144         heading += PI2;
00145         
00146     if(heading > PI2) // fix overflow
00147         heading -= PI2;
00148         
00149     return heading;
00150 }