justin kim / HMC5883L

HMC5883L

Dependents:   FacingSouthHouseFinder_WIZwiki-W7500

Fork of HMC5883L by Baser Kandehir

HMC5883L.cpp@3:eca30b3ec1ed, 2015-08-26 (annotated)

Committer:
justinkim
Date:
Wed Aug 26 06:20:51 2015 +0000
Revision:
3:eca30b3ec1ed
Parent:
2:bbc9ad18fd3e 
delete include led library

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:e5f8da308b60 1 /* HMC5883L Digital Compass Library
BaserK 0:e5f8da308b60 2 *
BaserK 0:e5f8da308b60 3 * @author: Baser Kandehir
BaserK 0:e5f8da308b60 4 * @date: August 5, 2015
BaserK 0:e5f8da308b60 5 * @license: MIT license
BaserK 0:e5f8da308b60 6 *
BaserK 0:e5f8da308b60 7 * Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
BaserK 0:e5f8da308b60 8 *
BaserK 0:e5f8da308b60 9 * Permission is hereby granted, free of charge, to any person obtaining a copy
BaserK 0:e5f8da308b60 10 * of this software and associated documentation files (the "Software"), to deal
BaserK 0:e5f8da308b60 11 * in the Software without restriction, including without limitation the rights
BaserK 0:e5f8da308b60 12 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
BaserK 0:e5f8da308b60 13 * copies of the Software, and to permit persons to whom the Software is
BaserK 0:e5f8da308b60 14 * furnished to do so, subject to the following conditions:
BaserK 0:e5f8da308b60 15 *
BaserK 0:e5f8da308b60 16 * The above copyright notice and this permission notice shall be included in
BaserK 0:e5f8da308b60 17 * all copies or substantial portions of the Software.
BaserK 0:e5f8da308b60 18 *
BaserK 0:e5f8da308b60 19 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
BaserK 0:e5f8da308b60 20 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
BaserK 0:e5f8da308b60 21 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
BaserK 0:e5f8da308b60 22 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
BaserK 0:e5f8da308b60 23 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
BaserK 0:e5f8da308b60 24 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
BaserK 0:e5f8da308b60 25 * THE SOFTWARE.
BaserK 0:e5f8da308b60 26 *
BaserK 0:e5f8da308b60 27 */
BaserK 0:e5f8da308b60 28
BaserK 0:e5f8da308b60 29 // Some part of the code is adapted from Adafruit HMC5883 library
BaserK 0:e5f8da308b60 30
BaserK 0:e5f8da308b60 31 #include "HMC5883L.h"
BaserK 0:e5f8da308b60 32
BaserK 0:e5f8da308b60 33 /* NUCLEO F411RE board */
BaserK 2:bbc9ad18fd3e 34 static I2C i2c(D14, D15); // setup i2c (SDA,SCL)
BaserK 0:e5f8da308b60 35
BaserK 0:e5f8da308b60 36 static float Gauss_LSB_XY = 1100.0F; // Varies with gain
BaserK 0:e5f8da308b60 37 static float Gauss_LSB_Z = 980.0F; // Varies with gain
BaserK 0:e5f8da308b60 38
BaserK 0:e5f8da308b60 39 void HMC5883L::setMagGain(MagGain gain)
BaserK 0:e5f8da308b60 40 {
BaserK 0:e5f8da308b60 41 writeByte(HMC5883L_ADDRESS, CONFIG_B, (int8_t)gain);
BaserK 0:e5f8da308b60 42
BaserK 0:e5f8da308b60 43 switch(gain)
BaserK 0:e5f8da308b60 44 {
BaserK 0:e5f8da308b60 45 case MagGain_13:
BaserK 0:e5f8da308b60 46 Gauss_LSB_XY = 1100;
BaserK 0:e5f8da308b60 47 Gauss_LSB_Z = 980;
BaserK 0:e5f8da308b60 48 break;
BaserK 0:e5f8da308b60 49 case MagGain_19:
BaserK 0:e5f8da308b60 50 Gauss_LSB_XY = 855;
BaserK 0:e5f8da308b60 51 Gauss_LSB_Z = 760;
BaserK 0:e5f8da308b60 52 break;
BaserK 0:e5f8da308b60 53 case MagGain_25:
BaserK 0:e5f8da308b60 54 Gauss_LSB_XY = 670;
BaserK 0:e5f8da308b60 55 Gauss_LSB_Z = 600;
BaserK 0:e5f8da308b60 56 break;
BaserK 0:e5f8da308b60 57 case MagGain_40:
BaserK 0:e5f8da308b60 58 Gauss_LSB_XY = 450;
BaserK 0:e5f8da308b60 59 Gauss_LSB_Z = 400;
BaserK 0:e5f8da308b60 60 break;
BaserK 0:e5f8da308b60 61 case MagGain_47:
BaserK 0:e5f8da308b60 62 Gauss_LSB_XY = 400;
BaserK 0:e5f8da308b60 63 Gauss_LSB_Z = 255;
BaserK 0:e5f8da308b60 64 break;
BaserK 0:e5f8da308b60 65 case MagGain_56:
BaserK 0:e5f8da308b60 66 Gauss_LSB_XY = 330;
BaserK 0:e5f8da308b60 67 Gauss_LSB_Z = 295;
BaserK 0:e5f8da308b60 68 break;
BaserK 0:e5f8da308b60 69 case MagGain_81:
BaserK 0:e5f8da308b60 70 Gauss_LSB_XY = 230;
BaserK 0:e5f8da308b60 71 Gauss_LSB_Z = 205;
BaserK 0:e5f8da308b60 72 break;
BaserK 0:e5f8da308b60 73 }
BaserK 0:e5f8da308b60 74 }
BaserK 0:e5f8da308b60 75
BaserK 0:e5f8da308b60 76 void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
BaserK 0:e5f8da308b60 77 {
BaserK 0:e5f8da308b60 78 char data_write[2];
BaserK 0:e5f8da308b60 79 data_write[0]=regAddress; // I2C sends MSB first. Namely >>|regAddress|>>|data|
BaserK 0:e5f8da308b60 80 data_write[1]=data;
BaserK 0:e5f8da308b60 81 i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false);
BaserK 0:e5f8da308b60 82 }
BaserK 0:e5f8da308b60 83
BaserK 0:e5f8da308b60 84 char HMC5883L::readByte(uint8_t address, uint8_t regAddress)
BaserK 0:e5f8da308b60 85 {
BaserK 0:e5f8da308b60 86 char data_read[1]; // will store the register data
BaserK 0:e5f8da308b60 87 char data_write[1];
BaserK 0:e5f8da308b60 88 data_write[0]=regAddress;
BaserK 0:e5f8da308b60 89 i2c.write(address,data_write,1,1); // repeated = true
BaserK 0:e5f8da308b60 90 i2c.read(address,data_read,1,0); // read the data and stop
BaserK 0:e5f8da308b60 91 return data_read[0];
BaserK 0:e5f8da308b60 92 }
BaserK 0:e5f8da308b60 93
BaserK 0:e5f8da308b60 94 void HMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
BaserK 0:e5f8da308b60 95 {
BaserK 0:e5f8da308b60 96 char data[10],data_write[1];
BaserK 0:e5f8da308b60 97 data_write[0]=regAddress;
BaserK 0:e5f8da308b60 98 i2c.write(address,data_write,1,1);
BaserK 0:e5f8da308b60 99 i2c.read(address,data,byteNum,0);
BaserK 0:e5f8da308b60 100 for(int i=0;i<byteNum;i++) // equate the addresses
BaserK 0:e5f8da308b60 101 dest[i]=data[i];
BaserK 0:e5f8da308b60 102 }
BaserK 0:e5f8da308b60 103
BaserK 0:e5f8da308b60 104 void HMC5883L::init()
BaserK 0:e5f8da308b60 105 {
BaserK 0:e5f8da308b60 106 writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78); // Number of samples averaged: 8, Data output rate: 75 Hz
BaserK 0:e5f8da308b60 107 writeByte(HMC5883L_ADDRESS, MODE, 0x00); // Continuous-Measurement Mode
BaserK 0:e5f8da308b60 108 setMagGain(MagGain_13);
BaserK 0:e5f8da308b60 109 wait_ms(10);
BaserK 0:e5f8da308b60 110 }
BaserK 0:e5f8da308b60 111
BaserK 0:e5f8da308b60 112 void HMC5883L::readMagData(float* dest)
BaserK 0:e5f8da308b60 113 {
BaserK 0:e5f8da308b60 114 uint8_t rawData[6]; // x,y,z mag data
BaserK 0:e5f8da308b60 115
BaserK 0:e5f8da308b60 116 /* Read six raw data registers sequentially and write them into data array */
BaserK 0:e5f8da308b60 117 readBytes(HMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]);
BaserK 0:e5f8da308b60 118
BaserK 0:e5f8da308b60 119 /* Turn the MSB LSB into signed 16-bit value */
BaserK 0:e5f8da308b60 120 dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // MAG_XOUT
BaserK 0:e5f8da308b60 121 dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // MAG_ZOUT
BaserK 0:e5f8da308b60 122 dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // MAG_YOUT
BaserK 0:e5f8da308b60 123
BaserK 0:e5f8da308b60 124 /* Convert raw data to magnetic field values in microtesla */
BaserK 0:e5f8da308b60 125 dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
BaserK 0:e5f8da308b60 126 dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
BaserK 0:e5f8da308b60 127 dest[2] = dest[2] / Gauss_LSB_Z * GAUSS_TO_MICROTESLA;
BaserK 0:e5f8da308b60 128 }
BaserK 0:e5f8da308b60 129
BaserK 0:e5f8da308b60 130 double HMC5883L::getHeading()
BaserK 0:e5f8da308b60 131 {
BaserK 0:e5f8da308b60 132 float magData[3];
BaserK 0:e5f8da308b60 133 readMagData(magData);
BaserK 0:e5f8da308b60 134
BaserK 0:e5f8da308b60 135 /* Calculate the heading while Z axis of the module is pointing up */
BaserK 0:e5f8da308b60 136 double heading = atan2(magData[1], magData[0]);
BaserK 0:e5f8da308b60 137
BaserK 0:e5f8da308b60 138 // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
BaserK 0:e5f8da308b60 139 // declinationAngle can be found here http://www.magnetic-declination.com/
BaserK 0:e5f8da308b60 140 // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
BaserK 0:e5f8da308b60 141 float declinationAngle = 0.096;
BaserK 0:e5f8da308b60 142 heading += declinationAngle;
BaserK 0:e5f8da308b60 143
BaserK 0:e5f8da308b60 144 // Correct for when signs are reversed.
BaserK 0:e5f8da308b60 145 if(heading < 0)
BaserK 0:e5f8da308b60 146 heading += 2*PI;
BaserK 0:e5f8da308b60 147
BaserK 0:e5f8da308b60 148 // Check for wrap due to addition of declination.
BaserK 0:e5f8da308b60 149 if(heading > 2*PI)
BaserK 0:e5f8da308b60 150 heading -= 2*PI;
BaserK 0:e5f8da308b60 151
BaserK 0:e5f8da308b60 152 /* Convert radian to degrees */
BaserK 0:e5f8da308b60 153 heading = heading * 180 / PI;
BaserK 0:e5f8da308b60 154
BaserK 0:e5f8da308b60 155 return heading;
BaserK 0:e5f8da308b60 156 }