HMC5883L Digital Compass Library
Fork of HMC5883L by
HMC5883L.cpp@2:bbc9ad18fd3e, 2015-08-05 (annotated)
- Committer:
- BaserK
- Date:
- Wed Aug 05 13:16:12 2015 +0000
- Revision:
- 2:bbc9ad18fd3e
- Parent:
- 0:e5f8da308b60
- Child:
- 3:65db23abde25
i2c object is static now
Who changed what in which revision?
User | Revision | Line number | New 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 | } |