Serge Sozonoff / HMC6343

HMC6343.cpp@2:2645c4d75671, 2011-06-17 (annotated)

Committer:
ssozonoff
Date:
Fri Jun 17 22:35:48 2011 +0000
Revision:
2:2645c4d75671
Parent:
1:bdf678f27614 
Fixed issue with Pitch and Roll negative values

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ssozonoff 1:bdf678f27614 1 /**
ssozonoff 1:bdf678f27614 2 * @author Aaron Berk
ssozonoff 1:bdf678f27614 3 * @author Serge Sozonoff
ssozonoff 1:bdf678f27614 4 * Partially based on the work of Aaron Berk for the HMC6352
ssozonoff 1:bdf678f27614 5 *
ssozonoff 1:bdf678f27614 6 * @section LICENSE
ssozonoff 1:bdf678f27614 7 *
ssozonoff 1:bdf678f27614 8 * Copyright (c) 2010 ARM Limited
ssozonoff 1:bdf678f27614 9 *
ssozonoff 1:bdf678f27614 10 * Permission is hereby granted, free of charge, to any person obtaining a copy
ssozonoff 1:bdf678f27614 11 * of this software and associated documentation files (the "Software"), to deal
ssozonoff 1:bdf678f27614 12 * in the Software without restriction, including without limitation the rights
ssozonoff 1:bdf678f27614 13 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
ssozonoff 1:bdf678f27614 14 * copies of the Software, and to permit persons to whom the Software is
ssozonoff 1:bdf678f27614 15 * furnished to do so, subject to the following conditions:
ssozonoff 1:bdf678f27614 16 *
ssozonoff 1:bdf678f27614 17 * The above copyright notice and this permission notice shall be included in
ssozonoff 1:bdf678f27614 18 * all copies or substantial portions of the Software.
ssozonoff 1:bdf678f27614 19 *
ssozonoff 1:bdf678f27614 20 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
ssozonoff 1:bdf678f27614 21 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
ssozonoff 1:bdf678f27614 22 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
ssozonoff 1:bdf678f27614 23 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
ssozonoff 1:bdf678f27614 24 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
ssozonoff 1:bdf678f27614 25 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
ssozonoff 1:bdf678f27614 26 * THE SOFTWARE.
ssozonoff 1:bdf678f27614 27 *
ssozonoff 1:bdf678f27614 28 * @section DESCRIPTION
ssozonoff 1:bdf678f27614 29 *
ssozonoff 1:bdf678f27614 30 * Honeywell HMC6343 tilt compensated digital compass.
ssozonoff 1:bdf678f27614 31 *
ssozonoff 1:bdf678f27614 32 * Datasheet:
ssozonoff 1:bdf678f27614 33 *
ssozonoff 1:bdf678f27614 34 * http://www.ssec.honeywell.com/magnetic/datasheets/HMC6343.pdf
ssozonoff 1:bdf678f27614 35 */
ssozonoff 1:bdf678f27614 36
ssozonoff 1:bdf678f27614 37 /**
ssozonoff 1:bdf678f27614 38 * Includes
ssozonoff 1:bdf678f27614 39 */
ssozonoff 1:bdf678f27614 40 #include "HMC6343.h"
ssozonoff 1:bdf678f27614 41
ssozonoff 2:2645c4d75671 42 short h, r, p;
ssozonoff 2:2645c4d75671 43
ssozonoff 1:bdf678f27614 44 template<class TYPE> inline TYPE BIT(const TYPE & x) {
ssozonoff 1:bdf678f27614 45 return TYPE(1) << x;
ssozonoff 1:bdf678f27614 46 }
ssozonoff 1:bdf678f27614 47
ssozonoff 1:bdf678f27614 48 template<class TYPE> inline bool IsBitSet(const TYPE & x, const TYPE & y) {
ssozonoff 1:bdf678f27614 49 return 0 != (x & y);
ssozonoff 1:bdf678f27614 50 }
ssozonoff 1:bdf678f27614 51
ssozonoff 1:bdf678f27614 52 HMC6343::HMC6343(PinName sda, PinName scl) {
ssozonoff 1:bdf678f27614 53 i2c_ = new I2C(sda, scl);
ssozonoff 1:bdf678f27614 54 i2c_->frequency(100000);
ssozonoff 1:bdf678f27614 55 operationMode_ = getOpMode();
ssozonoff 1:bdf678f27614 56 }
ssozonoff 1:bdf678f27614 57
ssozonoff 1:bdf678f27614 58 HMC6343::HMC6343(I2C& p_i2c) : i2c_(&p_i2c) {
ssozonoff 1:bdf678f27614 59 i2c_->frequency(100000);
ssozonoff 1:bdf678f27614 60 operationMode_ = getOpMode();
ssozonoff 1:bdf678f27614 61 }
ssozonoff 1:bdf678f27614 62
ssozonoff 1:bdf678f27614 63 void HMC6343::sampleHeading(Heading* p_heading) {
ssozonoff 1:bdf678f27614 64
ssozonoff 1:bdf678f27614 65 char tx[1];
ssozonoff 1:bdf678f27614 66 char rx[6];
ssozonoff 1:bdf678f27614 67
ssozonoff 1:bdf678f27614 68 tx[0] = HMC6343_GET_HEADING_DATA;
ssozonoff 1:bdf678f27614 69 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
ssozonoff 1:bdf678f27614 70 wait_ms(1);
ssozonoff 1:bdf678f27614 71
ssozonoff 1:bdf678f27614 72 i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 6, true);
ssozonoff 2:2645c4d75671 73
ssozonoff 2:2645c4d75671 74 h = ((((int)rx[0] << 8) | (int)rx[1]));
ssozonoff 2:2645c4d75671 75 r = ((((int)rx[4] << 8) | (int)rx[5]));
ssozonoff 2:2645c4d75671 76 p = ((((int)rx[2] << 8) | (int)rx[3]));
ssozonoff 1:bdf678f27614 77
ssozonoff 2:2645c4d75671 78 p_heading->heading = (float)h / 10;
ssozonoff 2:2645c4d75671 79 p_heading->roll = (float)r / 10;
ssozonoff 2:2645c4d75671 80 p_heading->pitch = (float)p / 10;
ssozonoff 1:bdf678f27614 81 }
ssozonoff 1:bdf678f27614 82
ssozonoff 1:bdf678f27614 83
ssozonoff 1:bdf678f27614 84 void HMC6343::setReset(void) {
ssozonoff 1:bdf678f27614 85 char tx[1];
ssozonoff 1:bdf678f27614 86 tx[0] = HMC6343_RESET;
ssozonoff 1:bdf678f27614 87 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
ssozonoff 1:bdf678f27614 88 wait_ms(500);
ssozonoff 1:bdf678f27614 89 }
ssozonoff 1:bdf678f27614 90
ssozonoff 1:bdf678f27614 91 void HMC6343::setCalibrationMode(int exitOrEnter) {
ssozonoff 1:bdf678f27614 92 char tx[1];
ssozonoff 1:bdf678f27614 93 int delay = 0;
ssozonoff 1:bdf678f27614 94
ssozonoff 1:bdf678f27614 95 tx[0] = exitOrEnter;
ssozonoff 1:bdf678f27614 96
ssozonoff 1:bdf678f27614 97 if (exitOrEnter == HMC6343_EXIT_CALIB) {
ssozonoff 1:bdf678f27614 98 delay = 50;
ssozonoff 1:bdf678f27614 99 } else if (exitOrEnter == HMC6343_ENTER_CALIB) {
ssozonoff 1:bdf678f27614 100 delay = 1;
ssozonoff 1:bdf678f27614 101 }
ssozonoff 1:bdf678f27614 102
ssozonoff 1:bdf678f27614 103 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
ssozonoff 1:bdf678f27614 104 wait_ms(delay);
ssozonoff 1:bdf678f27614 105 }
ssozonoff 1:bdf678f27614 106
ssozonoff 1:bdf678f27614 107 int HMC6343::getSlaveAddress(void) {
ssozonoff 1:bdf678f27614 108 return readEeprom(HMC6343_SLAVE_ADDR);
ssozonoff 1:bdf678f27614 109 }
ssozonoff 1:bdf678f27614 110
ssozonoff 1:bdf678f27614 111 int HMC6343::getOffset(int axis) {
ssozonoff 1:bdf678f27614 112
ssozonoff 1:bdf678f27614 113 char rx[2] = {0x00, 0x00};
ssozonoff 1:bdf678f27614 114
ssozonoff 1:bdf678f27614 115 if (axis == HMC6343_X_AXIS) {
ssozonoff 1:bdf678f27614 116 rx[0] = readEeprom(HMC6343_XOFFSET_MSB);
ssozonoff 1:bdf678f27614 117 rx[1] = readEeprom(HMC6343_XOFFSET_LSB);
ssozonoff 1:bdf678f27614 118
ssozonoff 1:bdf678f27614 119 } else if (axis == HMC6343_Y_AXIS) {
ssozonoff 1:bdf678f27614 120 rx[0] = readEeprom(HMC6343_YOFFSET_MSB);
ssozonoff 1:bdf678f27614 121 rx[1] = readEeprom(HMC6343_YOFFSET_LSB);
ssozonoff 1:bdf678f27614 122
ssozonoff 1:bdf678f27614 123 } else {
ssozonoff 1:bdf678f27614 124 rx[0] = readEeprom(HMC6343_ZOFFSET_MSB);
ssozonoff 1:bdf678f27614 125 rx[1] = readEeprom(HMC6343_ZOFFSET_LSB);
ssozonoff 1:bdf678f27614 126 }
ssozonoff 1:bdf678f27614 127
ssozonoff 1:bdf678f27614 128 return ((rx[0] << 8) | (rx[1]));
ssozonoff 1:bdf678f27614 129
ssozonoff 1:bdf678f27614 130 }
ssozonoff 1:bdf678f27614 131
ssozonoff 1:bdf678f27614 132 char HMC6343::getMeasurementRate() {
ssozonoff 1:bdf678f27614 133 if (IsBitSet(operationMode_, HMC6343_CM_MR_10HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_5HZ)) { return 10; }
ssozonoff 1:bdf678f27614 134 else if (IsBitSet(operationMode_, HMC6343_CM_MR_5HZ) && !IsBitSet(operationMode_, HMC6343_CM_MR_10HZ)) { return 5; }
ssozonoff 1:bdf678f27614 135 else return 1;
ssozonoff 1:bdf678f27614 136 }
ssozonoff 1:bdf678f27614 137
ssozonoff 1:bdf678f27614 138 int HMC6343::getSoftwareVersion(void) {
ssozonoff 1:bdf678f27614 139 return readEeprom(HMC6343_SOFT_VER);
ssozonoff 1:bdf678f27614 140 }
ssozonoff 1:bdf678f27614 141
ssozonoff 1:bdf678f27614 142 int HMC6343::getOpMode(void) {
ssozonoff 1:bdf678f27614 143 char tx[1];
ssozonoff 1:bdf678f27614 144 tx[0] = HMC6343_GET_OPMODE;
ssozonoff 1:bdf678f27614 145
ssozonoff 1:bdf678f27614 146 char rx[2];
ssozonoff 1:bdf678f27614 147
ssozonoff 1:bdf678f27614 148 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 1);
ssozonoff 1:bdf678f27614 149 wait_ms(1);
ssozonoff 1:bdf678f27614 150 i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 2, true);
ssozonoff 1:bdf678f27614 151
ssozonoff 1:bdf678f27614 152 operationMode_ = (rx[1] << 8) | (rx[0]);
ssozonoff 1:bdf678f27614 153
ssozonoff 1:bdf678f27614 154 return operationMode_;
ssozonoff 1:bdf678f27614 155 }
ssozonoff 1:bdf678f27614 156
ssozonoff 1:bdf678f27614 157 bool HMC6343::isOpModeFlagSet(int flag) {
ssozonoff 1:bdf678f27614 158 return IsBitSet(operationMode_, flag);
ssozonoff 1:bdf678f27614 159 }
ssozonoff 1:bdf678f27614 160
ssozonoff 1:bdf678f27614 161 void HMC6343::setOpMode(int opMode) {
ssozonoff 1:bdf678f27614 162 writeShort(HMC6343_OPMOD_REG1, (short)opMode);
ssozonoff 1:bdf678f27614 163 operationMode_ = getOpMode();
ssozonoff 1:bdf678f27614 164 }
ssozonoff 1:bdf678f27614 165
ssozonoff 1:bdf678f27614 166 void HMC6343::writeShort(int lsb_address, short data) {
ssozonoff 1:bdf678f27614 167 writeEeprom(lsb_address, data & 0x00FF);
ssozonoff 1:bdf678f27614 168 writeEeprom(lsb_address + 1, data >> 8);
ssozonoff 1:bdf678f27614 169 }
ssozonoff 1:bdf678f27614 170
ssozonoff 1:bdf678f27614 171 short HMC6343::readShort(int lsb_eprom_address) {
ssozonoff 1:bdf678f27614 172 return (short)(readEeprom(lsb_eprom_address + 1) << 8) | (readEeprom(lsb_eprom_address));
ssozonoff 1:bdf678f27614 173 }
ssozonoff 1:bdf678f27614 174
ssozonoff 1:bdf678f27614 175 void HMC6343::setMagneticDeviation(float data) {
ssozonoff 1:bdf678f27614 176 short v;
ssozonoff 1:bdf678f27614 177 v = (short)(data * 100); // move decimal right two places
ssozonoff 1:bdf678f27614 178 if (v <= 1800 && v >= -1800) {
ssozonoff 1:bdf678f27614 179 writeShort(HMC6343_DEV_LSB, v);
ssozonoff 1:bdf678f27614 180 }
ssozonoff 1:bdf678f27614 181 }
ssozonoff 1:bdf678f27614 182
ssozonoff 1:bdf678f27614 183 float HMC6343::getMagneticDeviation() {
ssozonoff 1:bdf678f27614 184 return (float)(readShort(HMC6343_DEV_LSB)) / 100;
ssozonoff 1:bdf678f27614 185 }
ssozonoff 1:bdf678f27614 186
ssozonoff 1:bdf678f27614 187 void HMC6343::setMagneticVariation(float data) {
ssozonoff 1:bdf678f27614 188 short v;
ssozonoff 1:bdf678f27614 189 v = (short)(data * 100); // move decimal right two places
ssozonoff 1:bdf678f27614 190 if (v <= 1800 && v >= -1800) {
ssozonoff 1:bdf678f27614 191 writeShort(HMC6343_VAR_LSB, v);
ssozonoff 1:bdf678f27614 192 }
ssozonoff 1:bdf678f27614 193 }
ssozonoff 1:bdf678f27614 194
ssozonoff 1:bdf678f27614 195 float HMC6343::getMagneticVariation() {
ssozonoff 1:bdf678f27614 196 return (float)(readShort(HMC6343_VAR_LSB)) / 100;
ssozonoff 1:bdf678f27614 197 }
ssozonoff 1:bdf678f27614 198
ssozonoff 1:bdf678f27614 199 void HMC6343::setIIRFilter(short data) {
ssozonoff 1:bdf678f27614 200 writeShort(HMC6343_IIRF_LSB, data);
ssozonoff 1:bdf678f27614 201 }
ssozonoff 1:bdf678f27614 202
ssozonoff 1:bdf678f27614 203 short HMC6343::getIIRFilter() {
ssozonoff 1:bdf678f27614 204 return (short)readShort(HMC6343_IIRF_LSB);
ssozonoff 1:bdf678f27614 205 }
ssozonoff 1:bdf678f27614 206
ssozonoff 1:bdf678f27614 207 void HMC6343::setMagOffset(int axis, int offset) {
ssozonoff 1:bdf678f27614 208 if (axis == HMC6343_X_AXIS) {
ssozonoff 1:bdf678f27614 209 writeShort(HMC6343_XOFFSET_LSB, offset);
ssozonoff 1:bdf678f27614 210 } else if (axis == HMC6343_Y_AXIS) {
ssozonoff 1:bdf678f27614 211 writeShort(HMC6343_YOFFSET_LSB, offset);
ssozonoff 1:bdf678f27614 212 } else {
ssozonoff 1:bdf678f27614 213 writeShort(HMC6343_ZOFFSET_LSB, offset);
ssozonoff 1:bdf678f27614 214 }
ssozonoff 1:bdf678f27614 215 }
ssozonoff 1:bdf678f27614 216
ssozonoff 1:bdf678f27614 217 int HMC6343::getMagOffset(int axis) {
ssozonoff 1:bdf678f27614 218 if (axis == HMC6343_X_AXIS) {
ssozonoff 1:bdf678f27614 219 return readShort(HMC6343_XOFFSET_LSB);
ssozonoff 1:bdf678f27614 220 } else if (axis == HMC6343_Y_AXIS) {
ssozonoff 1:bdf678f27614 221 return readShort(HMC6343_YOFFSET_LSB);
ssozonoff 1:bdf678f27614 222 } else {
ssozonoff 1:bdf678f27614 223 return readShort(HMC6343_ZOFFSET_LSB);
ssozonoff 1:bdf678f27614 224 }
ssozonoff 1:bdf678f27614 225 }
ssozonoff 1:bdf678f27614 226
ssozonoff 1:bdf678f27614 227 void HMC6343::writeEeprom(int address, int data) {
ssozonoff 1:bdf678f27614 228 char tx[3];
ssozonoff 1:bdf678f27614 229
ssozonoff 1:bdf678f27614 230 tx[0] = HMC6343_EEPROM_WRITE;
ssozonoff 1:bdf678f27614 231 tx[1] = address;
ssozonoff 1:bdf678f27614 232 tx[2] = data;
ssozonoff 1:bdf678f27614 233
ssozonoff 1:bdf678f27614 234 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 3, false);
ssozonoff 1:bdf678f27614 235 wait_ms(10);
ssozonoff 1:bdf678f27614 236 }
ssozonoff 1:bdf678f27614 237
ssozonoff 1:bdf678f27614 238 int HMC6343::readEeprom(int address) {
ssozonoff 1:bdf678f27614 239 char tx[2];
ssozonoff 1:bdf678f27614 240 char rx[1];
ssozonoff 1:bdf678f27614 241
ssozonoff 1:bdf678f27614 242 tx[0] = HMC6343_EEPROM_READ;
ssozonoff 1:bdf678f27614 243 tx[1] = address;
ssozonoff 1:bdf678f27614 244
ssozonoff 1:bdf678f27614 245 i2c_->write((HMC6343_I2C_ADDRESS << 1) & 0xFE, tx, 2, false);
ssozonoff 1:bdf678f27614 246 wait_ms(1);
ssozonoff 1:bdf678f27614 247 i2c_->read((HMC6343_I2C_ADDRESS << 1) | 0x01, rx, 1);
ssozonoff 1:bdf678f27614 248 wait_ms(1);
ssozonoff 1:bdf678f27614 249
ssozonoff 1:bdf678f27614 250 return rx[0];
ssozonoff 1:bdf678f27614 251 }