HMC6343.cpp

00001 /**
00002  * @author Aaron Berk
00003  * @author Serge Sozonoff
00004  * Partially based on the work of Aaron Berk for the HMC6352
00005  *
00006  * @section LICENSE
00007  *
00008  * Copyright (c) 2010 ARM Limited
00009  *
00010  * Permission is hereby granted, free of charge, to any person obtaining a copy
00011  * of this software and associated documentation files (the "Software"), to deal
00012  * in the Software without restriction, including without limitation the rights
00013  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00014  * copies of the Software, and to permit persons to whom the Software is
00015  * furnished to do so, subject to the following conditions:
00016  *
00017  * The above copyright notice and this permission notice shall be included in
00018  * all copies or substantial portions of the Software.
00019  *
00020  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00021  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00022  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00023  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00024  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00025  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00026  * THE SOFTWARE.
00027  *
00028  * @section DESCRIPTION
00029  *
00030  * Honeywell HMC6343 tilt compensated digital compass.
00031  *
00032  * Datasheet:
00033  *
00034  * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6343.pdf
00035  */
00036 
00037 /**
00038  * Includes
00039  */
00040 #include "HMC6343.h"
00041 
00042 short h, r, p;
00043 
00044 template<class TYPE> inline TYPE BIT(const TYPE & x) {
00045     return TYPE(1) << x;
00046 }
00047 
00048 template<class TYPE> inline bool IsBitSet(const TYPE & x, const TYPE & y) {
00049     return 0 != (x & y);
00050 }
00051 
00052 HMC6343::HMC6343(PinName sda, PinName scl) {
00053     i2c_ = new I2C(sda, scl);
00054     i2c_->frequency(100000);
00055     operationMode_ = getOpMode();
00056 }
00057 
00058 HMC6343::HMC6343(I2C& p_i2c) : i2c_(&p_i2c) {
00059     i2c_->frequency(100000);
00060     operationMode_ = getOpMode();
00061 }
00062 
00063 void HMC6343::sampleHeading(Heading* p_heading) {
00064 
00065     char tx[1];
00066     char rx[6];
00067 
00068     tx[0] = HMC6343_GET_HEADING_DATA;
00069     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
00070     wait_ms(1);
00071 
00072     i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 6, true);
00073     
00074     h = ((((int)rx[0] << 8) | (int)rx[1]));
00075     r = ((((int)rx[4] << 8) | (int)rx[5]));
00076     p = ((((int)rx[2] << 8) | (int)rx[3]));
00077 
00078     p_heading->heading = (float)h / 10;
00079     p_heading->roll = (float)r / 10;
00080     p_heading->pitch = (float)p / 10;
00081 }
00082 
00083 
00084 void HMC6343::setReset(void) {
00085     char tx[1];
00086     tx[0] = HMC6343_RESET;
00087     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
00088     wait_ms(500);
00089 }
00090 
00091 void HMC6343::setCalibrationMode(int exitOrEnter) {
00092     char tx[1];
00093     int delay = 0;
00094 
00095     tx[0] = exitOrEnter;
00096 
00097     if (exitOrEnter == HMC6343_EXIT_CALIB) {
00098         delay = 50;
00099     } else if (exitOrEnter == HMC6343_ENTER_CALIB) {
00100         delay = 1;
00101     }
00102 
00103     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
00104     wait_ms(delay);
00105 }
00106 
00107 int HMC6343::getSlaveAddress(void) {
00108     return readEeprom(HMC6343_SLAVE_ADDR);
00109 }
00110 
00111 int HMC6343::getOffset(int axis) {
00112 
00113     char rx[2] = {0x00, 0x00};
00114 
00115     if (axis == HMC6343_X_AXIS) {
00116         rx[0] = readEeprom(HMC6343_XOFFSET_MSB);
00117         rx[1] = readEeprom(HMC6343_XOFFSET_LSB);
00118 
00119     } else if (axis == HMC6343_Y_AXIS) {
00120         rx[0] = readEeprom(HMC6343_YOFFSET_MSB);
00121         rx[1] = readEeprom(HMC6343_YOFFSET_LSB);
00122 
00123     } else {
00124         rx[0] = readEeprom(HMC6343_ZOFFSET_MSB);
00125         rx[1] = readEeprom(HMC6343_ZOFFSET_LSB);
00126     }
00127 
00128     return ((rx[0] << 8) | (rx[1]));
00129 
00130 }
00131 
00132 char HMC6343::getMeasurementRate() {
00133     if (IsBitSet(operationMode_, HMC6343_CM_MR_10HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_5HZ)) { return 10; }
00134     else if (IsBitSet(operationMode_, HMC6343_CM_MR_5HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_10HZ)) { return 5; }
00135     else return 1;
00136 }
00137 
00138 int HMC6343::getSoftwareVersion(void) {
00139     return readEeprom(HMC6343_SOFT_VER);
00140 }
00141 
00142 int HMC6343::getOpMode(void) {
00143     char tx[1];
00144     tx[0] = HMC6343_GET_OPMODE;
00145 
00146     char rx[2];
00147 
00148     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
00149     wait_ms(1);
00150     i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 2, true);
00151 
00152     operationMode_ = (rx[1] << 8) | (rx[0]);
00153 
00154     return operationMode_;
00155 }
00156 
00157 bool HMC6343::isOpModeFlagSet(int flag) {
00158     return IsBitSet(operationMode_, flag);
00159 }
00160 
00161 void HMC6343::setOpMode(int opMode) {
00162     writeShort(HMC6343_OPMOD_REG1, (short)opMode);
00163     operationMode_ = getOpMode();
00164 }
00165 
00166 void HMC6343::writeShort(int lsb_address, short data) {
00167     writeEeprom(lsb_address, data & 0x00FF);
00168     writeEeprom(lsb_address + 1, data >> 8);
00169 }
00170 
00171 short HMC6343::readShort(int lsb_eprom_address) {
00172     return (short)(readEeprom(lsb_eprom_address + 1) << 8) | (readEeprom(lsb_eprom_address));
00173 }
00174 
00175 void HMC6343::setMagneticDeviation(float data) {
00176     short v;
00177     v = (short)(data * 100); // move decimal right two places
00178     if (v <= 1800 && v >= -1800) {
00179         writeShort(HMC6343_DEV_LSB, v);
00180     }
00181 }
00182 
00183 float HMC6343::getMagneticDeviation() {
00184     return (float)(readShort(HMC6343_DEV_LSB)) / 100;
00185 }
00186 
00187 void HMC6343::setMagneticVariation(float data) {
00188     short v;
00189     v = (short)(data * 100); // move decimal right two places
00190     if (v <= 1800 && v >= -1800) {
00191         writeShort(HMC6343_VAR_LSB, v);
00192     }
00193 }
00194 
00195 float HMC6343::getMagneticVariation() {
00196     return (float)(readShort(HMC6343_VAR_LSB)) / 100;
00197 }
00198 
00199 void HMC6343::setIIRFilter(short data) {
00200     writeShort(HMC6343_IIRF_LSB, data);
00201 }
00202 
00203 short HMC6343::getIIRFilter() {
00204     return (short)readShort(HMC6343_IIRF_LSB);
00205 }
00206 
00207 void HMC6343::setMagOffset(int axis, int offset) {
00208     if (axis == HMC6343_X_AXIS) {
00209         writeShort(HMC6343_XOFFSET_LSB, offset);
00210     } else if (axis == HMC6343_Y_AXIS) {
00211         writeShort(HMC6343_YOFFSET_LSB, offset);
00212     } else {
00213         writeShort(HMC6343_ZOFFSET_LSB, offset);
00214     }
00215 }
00216 
00217 int HMC6343::getMagOffset(int axis) {
00218     if (axis == HMC6343_X_AXIS) {
00219         return readShort(HMC6343_XOFFSET_LSB);
00220     } else if (axis == HMC6343_Y_AXIS) {
00221         return readShort(HMC6343_YOFFSET_LSB);
00222     } else {
00223         return readShort(HMC6343_ZOFFSET_LSB);
00224     }
00225 }
00226 
00227 void HMC6343::writeEeprom(int address, int data) {
00228     char tx[3];
00229 
00230     tx[0] = HMC6343_EEPROM_WRITE;
00231     tx[1] = address;
00232     tx[2] = data;
00233 
00234     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 3, false);
00235     wait_ms(10);
00236 }
00237 
00238 int HMC6343::readEeprom(int address) {
00239     char tx[2];
00240     char rx[1];
00241 
00242     tx[0] = HMC6343_EEPROM_READ;
00243     tx[1] = address;
00244 
00245     i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 2, false);
00246     wait_ms(1);
00247     i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 1);
00248     wait_ms(1);
00249 
00250     return rx[0];
00251 }