Arcadie Cracan / HMC5983

Compass sensor library

Dependents:   compassDemo weather_station_proj weather_station_project weather_station_proj_v1_2

HMC5983.cpp@3:c2cd6cc71ee2, 2018-07-05 (annotated)

Committer:
acracan
Date:
Thu Jul 05 17:59:23 2018 +0000
Revision:
3:c2cd6cc71ee2
Parent:
2:4debef04091d 
Swap Z and Y in internal vector

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acracan 0:cffff4c45a14 1 /*
acracan 0:cffff4c45a14 2 * HMC5983.cpp - library header
acracan 0:cffff4c45a14 3 *
acracan 0:cffff4c45a14 4 * simple library for the HMC5983 sensor from Honeywell
acracan 0:cffff4c45a14 5 * adaptation form MBED from the Arduino library
acracan 0:cffff4c45a14 6 *
acracan 0:cffff4c45a14 7 * (c) 2014 Korneliusz Jarzebski, www.jarzebski.pl
acracan 0:cffff4c45a14 8 * (c) 2014 David Cuartielles, Arduino LLC
acracan 0:cffff4c45a14 9 * (c) 2016 Abel Romero, www.abelromero.com
acracan 0:cffff4c45a14 10 * (c) 2018 Arcadie Cracan
acracan 0:cffff4c45a14 11 */
acracan 0:cffff4c45a14 12
acracan 0:cffff4c45a14 13 // MISSING: EARTH DECLINATION ANGLE
acracan 0:cffff4c45a14 14 // In other words, we are not making any compensation for the earth's north pole location vs the magnetic measurement
acracan 0:cffff4c45a14 15
acracan 0:cffff4c45a14 16 #include "HMC5983.h"
acracan 0:cffff4c45a14 17 #include <new>
acracan 0:cffff4c45a14 18
acracan 0:cffff4c45a14 19 HMC5983::HMC5983(PinName sda, PinName scl) : i2c_(*reinterpret_cast<I2C*>(i2cRaw))
acracan 0:cffff4c45a14 20 {
acracan 0:cffff4c45a14 21 // Placement new to avoid additional heap memory allocation.
acracan 0:cffff4c45a14 22 new(i2cRaw) I2C(sda, scl);
acracan 2:4debef04091d 23
acracan 0:cffff4c45a14 24 init();
acracan 0:cffff4c45a14 25 }
acracan 0:cffff4c45a14 26
acracan 2:4debef04091d 27 HMC5983::HMC5983(I2C &i2c): i2c_(i2c)
acracan 2:4debef04091d 28 {
acracan 2:4debef04091d 29 init();
acracan 0:cffff4c45a14 30 }
acracan 0:cffff4c45a14 31
acracan 0:cffff4c45a14 32 bool HMC5983::init()
acracan 0:cffff4c45a14 33 {
acracan 2:4debef04091d 34 const float initial_m[9] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
acracan 2:4debef04091d 35 const float initial_b[3] = {0.0f, 0.0f, 0.0f};
acracan 2:4debef04091d 36 if ((fastRegister8(HMC5983_REG_IDENT_A) != 0x48)
acracan 2:4debef04091d 37 || (fastRegister8(HMC5983_REG_IDENT_B) != 0x34)
acracan 2:4debef04091d 38 || (fastRegister8(HMC5983_REG_IDENT_C) != 0x33)) {
acracan 2:4debef04091d 39 return false;
acracan 2:4debef04091d 40 }
acracan 0:cffff4c45a14 41
acracan 2:4debef04091d 42 // Set Gain Range
acracan 2:4debef04091d 43 setRange(HMC5983_RANGE_8_1GA);
acracan 2:4debef04091d 44 // Set DataRate 220Hz ~4.5ms
acracan 2:4debef04091d 45 setDataRate(HMC5983_DATARATE_220HZ);
acracan 2:4debef04091d 46 // Set number of samples to average
acracan 2:4debef04091d 47 setSampleAverages(HMC5983_SAMPLEAVERAGE_2);
acracan 2:4debef04091d 48 // Set Mode
acracan 2:4debef04091d 49 setMeasurementMode(HMC5983_CONTINOUS);
acracan 2:4debef04091d 50 setCalibrationMatrices(initial_m, initial_b);
acracan 2:4debef04091d 51 H[0] = 0;
acracan 2:4debef04091d 52 H[1] = 0;
acracan 2:4debef04091d 53 H[2] = 0;
acracan 2:4debef04091d 54 // Setup DRDY int
acracan 0:cffff4c45a14 55 // if (ISR_callback != NULL) {
acracan 0:cffff4c45a14 56 // pinMode(3, INPUT_PULLUP);
acracan 0:cffff4c45a14 57 // attachInterrupt(digitalPinToInterrupt(3), ISR_callback, FALLING);
acracan 0:cffff4c45a14 58 // }
acracan 2:4debef04091d 59 return true;
acracan 0:cffff4c45a14 60 }
acracan 0:cffff4c45a14 61
acracan 0:cffff4c45a14 62 /*
acracan 0:cffff4c45a14 63 From datasheet for the HMC5983
acracan 0:cffff4c45a14 64 Below is an example of a (power-on) initialization process for “continuous-measurement mode” via I2C interface:
acracan 0:cffff4c45a14 65 1. Write CRA (00) – send 0x3C 0x00 0x70 (8-average, 15 Hz default or any other rate, normal measurement)
acracan 0:cffff4c45a14 66 2. Write CRB (01) – send 0x3C 0x01 0xA0 (Gain=5, or any other desired gain)
acracan 0:cffff4c45a14 67 3. For each measurement query:
acracan 0:cffff4c45a14 68 Write Mode (02) – send 0x3C 0x02 0x01 (Single-measurement mode)
acracan 0:cffff4c45a14 69 Wait 6 ms or monitor status register or DRDY hardware interrupt pin
acracan 0:cffff4c45a14 70 Send 0x3D 0x06 (Read all 6 bytes. If gain is changed then this data set is using previous gain)
acracan 0:cffff4c45a14 71 Convert three 16-bit 2’s compliment hex values to decimal values and assign to X, Z, Y, respectively.
acracan 0:cffff4c45a14 72 (Self addition:)
acracan 0:cffff4c45a14 73 4. Convert the magnetic information into a compass value
acracan 0:cffff4c45a14 74 REGARDING THE CALCULATION OF THE ACTUAL HEADING VALUE
acracan 0:cffff4c45a14 75 From AN-203 http://www51.honeywell.com/aero/common/documents/myaerospacecatalog-documents/Defense_Brochures-documents/Magnetic__Literature_Application_notes-documents/AN203_Compass_Heading_Using_Magnetometers.pdf
acracan 0:cffff4c45a14 76 The magnetic compass heading can be determined (in degrees) from the magnetometer's x and y readings by using the
acracan 0:cffff4c45a14 77 following set of equations:
acracan 0:cffff4c45a14 78 Direction (y>0) = 90 - [arcTAN(x/y)]*180/PI
acracan 0:cffff4c45a14 79 Direction (y<0) = 270 - [arcTAN(x/y)]*180/PI
acracan 0:cffff4c45a14 80 Direction (y=0, x<0) = 180.0
acracan 0:cffff4c45a14 81 Direction (y=0, x>0) = 0.0
acracan 0:cffff4c45a14 82 */
acracan 0:cffff4c45a14 83
acracan 2:4debef04091d 84 void HMC5983::setRange(hmc5983_range_t range)
acracan 2:4debef04091d 85 {
acracan 0:cffff4c45a14 86
acracan 0:cffff4c45a14 87 writeRegister8(HMC5983_REG_CONFIG_B, range << 5);
acracan 0:cffff4c45a14 88 }
acracan 0:cffff4c45a14 89
acracan 0:cffff4c45a14 90 hmc5983_range_t HMC5983::getRange(void)
acracan 0:cffff4c45a14 91 {
acracan 0:cffff4c45a14 92 return (hmc5983_range_t)((readRegister8(HMC5983_REG_CONFIG_B) >> 5));
acracan 0:cffff4c45a14 93 }
acracan 0:cffff4c45a14 94
acracan 2:4debef04091d 95 void HMC5983::setMeasurementMode(hmc5983_mode_t mode)
acracan 2:4debef04091d 96 {
acracan 0:cffff4c45a14 97 uint8_t value;
acracan 0:cffff4c45a14 98
acracan 0:cffff4c45a14 99 value = readRegister8(HMC5983_REG_MODE);
acracan 0:cffff4c45a14 100 value &= 0b11111100;
acracan 0:cffff4c45a14 101 value |= mode;
acracan 0:cffff4c45a14 102
acracan 0:cffff4c45a14 103 writeRegister8(HMC5983_REG_MODE, value);
acracan 0:cffff4c45a14 104 }
acracan 0:cffff4c45a14 105
acracan 2:4debef04091d 106 hmc5983_mode_t HMC5983::getMeasurementMode(void)
acracan 2:4debef04091d 107 {
acracan 0:cffff4c45a14 108 uint8_t value;
acracan 0:cffff4c45a14 109
acracan 0:cffff4c45a14 110 value = readRegister8(HMC5983_REG_MODE);
acracan 0:cffff4c45a14 111 value &= 0b00000011;
acracan 0:cffff4c45a14 112
acracan 0:cffff4c45a14 113 return (hmc5983_mode_t)value;
acracan 0:cffff4c45a14 114 }
acracan 0:cffff4c45a14 115
acracan 2:4debef04091d 116 void HMC5983::setDataRate(hmc5983_dataRate_t dataRate)
acracan 2:4debef04091d 117 {
acracan 0:cffff4c45a14 118 uint8_t value;
acracan 0:cffff4c45a14 119
acracan 0:cffff4c45a14 120 value = readRegister8(HMC5983_REG_CONFIG_A);
acracan 0:cffff4c45a14 121 value &= 0b11100011;
acracan 0:cffff4c45a14 122 value |= (dataRate << 2);
acracan 0:cffff4c45a14 123
acracan 0:cffff4c45a14 124 writeRegister8(HMC5983_REG_CONFIG_A, value);
acracan 0:cffff4c45a14 125 }
acracan 0:cffff4c45a14 126
acracan 2:4debef04091d 127 hmc5983_dataRate_t HMC5983::getDataRate(void)
acracan 2:4debef04091d 128 {
acracan 0:cffff4c45a14 129 uint8_t value;
acracan 0:cffff4c45a14 130
acracan 0:cffff4c45a14 131 value = readRegister8(HMC5983_REG_CONFIG_A);
acracan 0:cffff4c45a14 132 value &= 0b00011100;
acracan 0:cffff4c45a14 133 value >>= 2;
acracan 0:cffff4c45a14 134
acracan 0:cffff4c45a14 135 return (hmc5983_dataRate_t)value;
acracan 0:cffff4c45a14 136 }
acracan 0:cffff4c45a14 137
acracan 2:4debef04091d 138 void HMC5983::setSampleAverages(hmc5983_sampleAverages_t sampleAverages)
acracan 2:4debef04091d 139 {
acracan 0:cffff4c45a14 140 uint8_t value;
acracan 0:cffff4c45a14 141
acracan 0:cffff4c45a14 142 value = readRegister8(HMC5983_REG_CONFIG_A);
acracan 0:cffff4c45a14 143 value &= 0b10011111;
acracan 0:cffff4c45a14 144 value |= (sampleAverages << 5);
acracan 0:cffff4c45a14 145
acracan 0:cffff4c45a14 146 writeRegister8(HMC5983_REG_CONFIG_A, value);
acracan 0:cffff4c45a14 147 }
acracan 0:cffff4c45a14 148
acracan 2:4debef04091d 149 hmc5983_sampleAverages_t HMC5983::getSampleAverages(void)
acracan 2:4debef04091d 150 {
acracan 0:cffff4c45a14 151 uint8_t value;
acracan 0:cffff4c45a14 152
acracan 0:cffff4c45a14 153 value = readRegister8(HMC5983_REG_CONFIG_A);
acracan 0:cffff4c45a14 154 value &= 0b01100000;
acracan 0:cffff4c45a14 155 value >>= 5;
acracan 0:cffff4c45a14 156
acracan 0:cffff4c45a14 157 return (hmc5983_sampleAverages_t)value;
acracan 0:cffff4c45a14 158 }
acracan 0:cffff4c45a14 159
acracan 0:cffff4c45a14 160 // Write byte to register
acracan 2:4debef04091d 161 void HMC5983::writeRegister8(uint8_t reg, uint8_t value)
acracan 2:4debef04091d 162 {
acracan 2:4debef04091d 163 char cmd[2];
acracan 2:4debef04091d 164
acracan 2:4debef04091d 165 cmd[0] = reg;
acracan 2:4debef04091d 166 cmd[1] = value;
acracan 2:4debef04091d 167 i2c_.write(HMC5983_ADDRESS, cmd, 2);
acracan 0:cffff4c45a14 168 }
acracan 0:cffff4c45a14 169
acracan 0:cffff4c45a14 170 // Read byte to register
acracan 2:4debef04091d 171 uint8_t HMC5983::fastRegister8(uint8_t reg)
acracan 2:4debef04091d 172 {
acracan 2:4debef04091d 173 uint8_t value;
acracan 0:cffff4c45a14 174
acracan 2:4debef04091d 175 i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
acracan 2:4debef04091d 176 i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
acracan 0:cffff4c45a14 177
acracan 2:4debef04091d 178 return value;
acracan 0:cffff4c45a14 179 }
acracan 0:cffff4c45a14 180
acracan 0:cffff4c45a14 181 // Read byte from register
acracan 2:4debef04091d 182 uint8_t HMC5983::readRegister8(uint8_t reg)
acracan 2:4debef04091d 183 {
acracan 2:4debef04091d 184 uint8_t value;
acracan 0:cffff4c45a14 185
acracan 2:4debef04091d 186 i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
acracan 2:4debef04091d 187 i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
acracan 2:4debef04091d 188
acracan 2:4debef04091d 189 return value;
acracan 0:cffff4c45a14 190
acracan 0:cffff4c45a14 191 // Wire.requestFrom(HMC5983_ADDRESS, 1);
acracan 0:cffff4c45a14 192 // while(!Wire.available()) {};
acracan 0:cffff4c45a14 193 // value = Wire.read();
acracan 0:cffff4c45a14 194 }
acracan 0:cffff4c45a14 195
acracan 0:cffff4c45a14 196 // Read word from register
acracan 2:4debef04091d 197 int16_t HMC5983::readRegister16(uint8_t reg)
acracan 2:4debef04091d 198 {
acracan 2:4debef04091d 199 int16_t value;
acracan 2:4debef04091d 200 char resp[2];
acracan 0:cffff4c45a14 201
acracan 2:4debef04091d 202 i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
acracan 2:4debef04091d 203 i2c_.read(HMC5983_ADDRESS, resp, 2);
acracan 0:cffff4c45a14 204
acracan 0:cffff4c45a14 205 // Wire.requestFrom(HMC5983_ADDRESS, 2);
acracan 0:cffff4c45a14 206 // while(!Wire.available()) {};
acracan 0:cffff4c45a14 207
acracan 2:4debef04091d 208 value = (uint8_t)resp[0] << 8 | (uint8_t)resp[1];
acracan 0:cffff4c45a14 209
acracan 2:4debef04091d 210 return value;
acracan 0:cffff4c45a14 211 }
acracan 0:cffff4c45a14 212
acracan 2:4debef04091d 213 void HMC5983::getHeadingRegs()
acracan 2:4debef04091d 214 {
acracan 2:4debef04091d 215 // the values for X, Y & Z must be read in X, Z & Y order.
acracan 2:4debef04091d 216 char data[6];
acracan 2:4debef04091d 217 writeRegister8(HMC5983_OUT_X_MSB, 0); // Select MSB X register
acracan 2:4debef04091d 218 i2c_.read(HMC5983_ADDRESS, data, 6);
acracan 2:4debef04091d 219 uint8_t X_MSB = data[0];
acracan 2:4debef04091d 220 uint8_t X_LSB = data[1];
acracan 2:4debef04091d 221 uint8_t Z_MSB = data[2];
acracan 2:4debef04091d 222 uint8_t Z_LSB = data[3];
acracan 2:4debef04091d 223 uint8_t Y_MSB = data[4];
acracan 2:4debef04091d 224 uint8_t Y_LSB = data[5];
acracan 2:4debef04091d 225
acracan 2:4debef04091d 226 // compose byte for X, Y, Z's LSB & MSB 8bit registers
acracan 2:4debef04091d 227 H[0] = (uint16_t(X_MSB) << 8) | X_LSB;
acracan 3:c2cd6cc71ee2 228 H[1] = (uint16_t(Y_MSB) << 8) | Y_LSB;
acracan 3:c2cd6cc71ee2 229 H[2] = (uint16_t(Z_MSB) << 8) | Z_LSB;
acracan 2:4debef04091d 230 }
acracan 0:cffff4c45a14 231
acracan 2:4debef04091d 232 void HMC5983::computeCalibratedHeadingVector(float *v)
acracan 2:4debef04091d 233 {
acracan 2:4debef04091d 234 float Hub[3];
acracan 2:4debef04091d 235
acracan 2:4debef04091d 236 int i, j;
acracan 2:4debef04091d 237
acracan 2:4debef04091d 238 // remove bias
acracan 2:4debef04091d 239 for (i = 0; i < 3; i++) {
acracan 2:4debef04091d 240 Hub[i] = H[i] - _b[i];
acracan 2:4debef04091d 241 v[i] = 0.0f;
acracan 2:4debef04091d 242 }
acracan 2:4debef04091d 243
acracan 2:4debef04091d 244 // apply matrix transformation
acracan 2:4debef04091d 245 for (i = 0; i < 3; i++) {
acracan 2:4debef04091d 246 for (j = 0; j < 3; j++) {
acracan 2:4debef04091d 247 v[i] += _m[3*i+j]*Hub[j];
acracan 2:4debef04091d 248 }
acracan 2:4debef04091d 249 }
acracan 1:fb6804e865fd 250 }
acracan 0:cffff4c45a14 251
acracan 2:4debef04091d 252 void HMC5983::setCalibrationMatrices(const float *m, const float *b)
acracan 2:4debef04091d 253 {
acracan 2:4debef04091d 254 int i, j;
acracan 0:cffff4c45a14 255
acracan 2:4debef04091d 256 for (i = 0; i < 3; i++) {
acracan 2:4debef04091d 257 for (j = 0; j < 3; j++) {
acracan 2:4debef04091d 258 _m[3*i+j] = m[3*i+j];
acracan 2:4debef04091d 259 }
acracan 2:4debef04091d 260 _b[i] = b[i];
acracan 2:4debef04091d 261 }
acracan 1:fb6804e865fd 262 }
acracan 1:fb6804e865fd 263
acracan 2:4debef04091d 264 float HMC5983::readHeading(bool calibrate)
acracan 2:4debef04091d 265 {
acracan 2:4debef04091d 266 // declare the heading variable we'll be returning
acracan 2:4debef04091d 267 float result = 0;
acracan 2:4debef04091d 268 float cH[3];
acracan 2:4debef04091d 269 int i;
acracan 2:4debef04091d 270
acracan 1:fb6804e865fd 271 getHeadingRegs();
acracan 2:4debef04091d 272 for (i = 0; i < 3; i++) {
acracan 2:4debef04091d 273 cH[i] = H[i];
acracan 2:4debef04091d 274 }
acracan 2:4debef04091d 275
acracan 2:4debef04091d 276 if (calibrate) {
acracan 2:4debef04091d 277 computeCalibratedHeadingVector(cH);
acracan 2:4debef04091d 278 }
acracan 2:4debef04091d 279 // this is the correct way, fixed from original David's work.
acracan 2:4debef04091d 280 // corrected following datasheet and his own comments...xD
acracan 2:4debef04091d 281 // even corrected from datasheet, the 90-270 angle is a bit confusing, but the 360º are captured.
acracan 2:4debef04091d 282 if (cH[1] > 0.0f) result = 90.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI;
acracan 2:4debef04091d 283 if (cH[1] < 0.0f) result = 270.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI;
acracan 2:4debef04091d 284 if (cH[1] == 0.0f && cH[0] < 0.0f) result = 180.0f;
acracan 2:4debef04091d 285 if (cH[1] == 0.0f && cH[0] > 0.0f) result = 0.0f;
acracan 2:4debef04091d 286
acracan 2:4debef04091d 287 return result;
acracan 2:4debef04091d 288 }
acracan 2:4debef04091d 289
acracan 2:4debef04091d 290 void HMC5983::readHeadingVector(float *v, bool calibrate)
acracan 2:4debef04091d 291 {
acracan 2:4debef04091d 292 getHeadingRegs();
acracan 2:4debef04091d 293 if (calibrate) {
acracan 2:4debef04091d 294 computeCalibratedHeadingVector(v);
acracan 2:4debef04091d 295 }
acracan 2:4debef04091d 296 else {
acracan 2:4debef04091d 297 v[0] = H[0];
acracan 2:4debef04091d 298 v[1] = H[1];
acracan 2:4debef04091d 299 v[2] = H[2];
acracan 2:4debef04091d 300 }
acracan 0:cffff4c45a14 301 }