HMC5883L.cpp

00001 /*
00002  * @file HMC5883L.cpp
00003  * @author Tyler Weaver
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  * For use with the Sparkfun 9 Degrees of Freedom - Sensor Stick
00026  *
00027  * Datasheet:
00028  *
00029  * http://dlnmh9ip6v2uc.cloudfront.net/datasheets/Sensors/Magneto/HMC5883L-FDS.pdf
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 
00055     setMode(CONTINUOUS_MODE); // continuous sample mode
00056 }
00057 
00058 void HMC5883L::setConfigurationA(char config)
00059 {
00060     char cmd[2];
00061     cmd[0] = CONFIG_A_REG; // register a address
00062     cmd[1] = config;
00063     
00064     i2c_.write(I2C_ADDRESS, cmd, 2);
00065 }
00066 
00067 void HMC5883L::setConfigurationB(char config)
00068 {
00069     char cmd[2];
00070     cmd[0] = CONFIG_B_REG; // register b address
00071     cmd[1] = config;
00072     
00073     i2c_.write(I2C_ADDRESS, cmd, 2);
00074 }
00075 
00076 char HMC5883L::getConfigurationA()
00077 {
00078     char cmd[2];
00079     cmd[0] = CONFIG_A_REG; // register a address
00080     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00081     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00082     return cmd[1];
00083 }
00084 
00085 char HMC5883L::getConfigurationB()
00086 {
00087     char cmd[2];
00088     cmd[0] = CONFIG_B_REG; // register b address
00089     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00090     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00091     return cmd[1];
00092 }
00093 
00094 void HMC5883L::setMode(char mode = SINGLE_MODE)
00095 {
00096     char cmd[2];
00097     cmd[0] = MODE_REG; // mode register address
00098     cmd[1] = mode;
00099     i2c_.write(I2C_ADDRESS,cmd,2);
00100 }
00101 
00102 char HMC5883L::getMode()
00103 {
00104     char cmd[2];
00105     cmd[0] = MODE_REG; // mode register
00106     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00107     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00108     return cmd[1];
00109 }
00110 
00111 char HMC5883L::getStatus()
00112 {
00113     char cmd[2];
00114     cmd[0] = STATUS_REG; // status register
00115     i2c_.write(I2C_ADDRESS, cmd, 1, true);
00116     i2c_.read(I2C_ADDRESS, &cmd[1], 1, false);
00117     return cmd[1];
00118 }
00119 
00120 void HMC5883L::getXYZ(int16_t output[3])
00121 {
00122     char cmd[2];
00123     char data[6];
00124     cmd[0] = 0x03; // starting point for reading
00125     i2c_.write(I2C_ADDRESS, cmd, 1, true); // set the pointer to the start of x
00126     i2c_.read(I2C_ADDRESS, data, 6, false);
00127     
00128     for(int i = 0; i < 3; i++) // fill the output variables
00129         output[i] = int16_t(((unsigned char)data[i*2] << 8) | (unsigned char)data[i*2+1]);
00130 }
00131 
00132 double HMC5883L::getHeadingXY(int Calib_x,int Calib_y)
00133 {
00134     int16_t raw_data[3];
00135     getXYZ(raw_data);
00136     double heading = atan2(static_cast<double>(raw_data[0]+Calib_x), static_cast<double>(raw_data[2])+Calib_y); // heading = arctan(Y/X)
00137     
00138     // TODO: declenation angle compensation
00139     
00140     if(heading < 0.0) // fix sign
00141         heading += PI2;
00142         
00143     if(heading > PI2) // fix overflow
00144         heading -= PI2;
00145         
00146     return heading;
00147 }