Bruno Horta / HMC5883L

HMC5883L Digital Compass Library

Fork of HMC5883L by Baser Kandehir

HMC5883L.cpp@3:f14ad02770e4, 2018-05-30 (annotated)

Committer:
brunohorta
Date:
Wed May 30 19:14:58 2018 +0000
Revision:
3:f14ad02770e4
Parent:
2:bbc9ad18fd3e 
ble

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BaserK 0:e5f8da308b60 1
BaserK 0:e5f8da308b60 2 #include "HMC5883L.h"
brunohorta 3:f14ad02770e4 3 static I2C i2c(p4, p2); // setup i2c (SDA,SCL)
BaserK 0:e5f8da308b60 4
brunohorta 3:f14ad02770e4 5 uint16_t mode;
brunohorta 3:f14ad02770e4 6 float declination_offset_radians;
BaserK 0:e5f8da308b60 7
brunohorta 3:f14ad02770e4 8 void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data){
BaserK 0:e5f8da308b60 9 char data_write[2];
BaserK 0:e5f8da308b60 10 data_write[0]=regAddress; // I2C sends MSB first. Namely >>|regAddress|>>|data|
BaserK 0:e5f8da308b60 11 data_write[1]=data;
BaserK 0:e5f8da308b60 12 i2c.write(address,data_write,2,0); // i2c.write(int address, char* data, int length, bool repeated=false);
BaserK 0:e5f8da308b60 13 }
BaserK 0:e5f8da308b60 14
BaserK 0:e5f8da308b60 15 char HMC5883L::readByte(uint8_t address, uint8_t regAddress)
BaserK 0:e5f8da308b60 16 {
BaserK 0:e5f8da308b60 17 char data_read[1]; // will store the register data
BaserK 0:e5f8da308b60 18 char data_write[1];
BaserK 0:e5f8da308b60 19 data_write[0]=regAddress;
BaserK 0:e5f8da308b60 20 i2c.write(address,data_write,1,1); // repeated = true
BaserK 0:e5f8da308b60 21 i2c.read(address,data_read,1,0); // read the data and stop
BaserK 0:e5f8da308b60 22 return data_read[0];
BaserK 0:e5f8da308b60 23 }
BaserK 0:e5f8da308b60 24
BaserK 0:e5f8da308b60 25 void HMC5883L::readBytes(uint8_t address, uint8_t regAddress, uint8_t byteNum, uint8_t* dest)
BaserK 0:e5f8da308b60 26 {
BaserK 0:e5f8da308b60 27 char data[10],data_write[1];
BaserK 0:e5f8da308b60 28 data_write[0]=regAddress;
BaserK 0:e5f8da308b60 29 i2c.write(address,data_write,1,1);
BaserK 0:e5f8da308b60 30 i2c.read(address,data,byteNum,0);
BaserK 0:e5f8da308b60 31 for(int i=0;i<byteNum;i++) // equate the addresses
BaserK 0:e5f8da308b60 32 dest[i]=data[i];
BaserK 0:e5f8da308b60 33 }
BaserK 0:e5f8da308b60 34
brunohorta 3:f14ad02770e4 35 void HMC5883L::init(){
brunohorta 3:f14ad02770e4 36 declination_offset_radians = 0 - (( 54 + (1/60 * 4) ) * (M_PI / 180));
brunohorta 3:f14ad02770e4 37 mode = COMPASS_CONTINUOUS | COMPASS_SCALE_130 | COMPASS_VERTICAL_X_EAST;
BaserK 0:e5f8da308b60 38 writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78); // Number of samples averaged: 8, Data output rate: 75 Hz
brunohorta 3:f14ad02770e4 39 writeByte(HMC5883L_ADDRESS, MODE, mode & 0x03); // Continuous-Measurement Mode
BaserK 0:e5f8da308b60 40 wait_ms(10);
BaserK 0:e5f8da308b60 41 }
brunohorta 3:f14ad02770e4 42 void HMC5883L::setScale( uint16_t scale ){
brunohorta 3:f14ad02770e4 43 // Scale is the bits marked S in mode
brunohorta 3:f14ad02770e4 44 // xxxxxxxxxxxSSSMM
brunohorta 3:f14ad02770e4 45 mode = (mode & ~0x1C) | (scale & 0x1C);
brunohorta 3:f14ad02770e4 46 writeByte(HMC5883L_ADDRESS,CONFIG_B, (( mode >> 2 ) & 0x07) << 5);
brunohorta 3:f14ad02770e4 47 }
BaserK 0:e5f8da308b60 48
brunohorta 3:f14ad02770e4 49 /** Set the orientation to one of COMPASS_HORIZONTAL_X_NORTH
brunohorta 3:f14ad02770e4 50 * through COMPASS_VERTICAL_Y_WEST
brunohorta 3:f14ad02770e4 51 *
brunohorta 3:f14ad02770e4 52 */
brunohorta 3:f14ad02770e4 53
brunohorta 3:f14ad02770e4 54 void HMC5883L::setOrientation( uint16_t orientation )
BaserK 0:e5f8da308b60 55 {
brunohorta 3:f14ad02770e4 56 // Orientation is the bits marked XXXYYYZZZ in mode
brunohorta 3:f14ad02770e4 57 // xxXXXYYYZZZxxxxx
brunohorta 3:f14ad02770e4 58 mode = (mode & ~0x3FE0) | (orientation & 0x3FE0);
brunohorta 3:f14ad02770e4 59 }
brunohorta 3:f14ad02770e4 60
brunohorta 3:f14ad02770e4 61 float s_mag_north, s_mag_west;
brunohorta 3:f14ad02770e4 62 double HMC5883L::getHeading(){
brunohorta 3:f14ad02770e4 63
brunohorta 3:f14ad02770e4 64 float sample[3];
brunohorta 3:f14ad02770e4 65 readMagData(sample);
brunohorta 3:f14ad02770e4 66
brunohorta 3:f14ad02770e4 67
brunohorta 3:f14ad02770e4 68
brunohorta 3:f14ad02770e4 69 float heading;
brunohorta 3:f14ad02770e4 70
brunohorta 3:f14ad02770e4 71 // Determine which of the Axes to use for North and West (when compass is "pointing" north)
brunohorta 3:f14ad02770e4 72 float mag_north, mag_west = -100000;
brunohorta 3:f14ad02770e4 73
brunohorta 3:f14ad02770e4 74 switch((mode >> 5) & 0x07 )
brunohorta 3:f14ad02770e4 75 {
brunohorta 3:f14ad02770e4 76 case COMPASS_NORTH: mag_north = sample[2]; break;
brunohorta 3:f14ad02770e4 77 case COMPASS_SOUTH: mag_north = 0-sample[2]; break;
brunohorta 3:f14ad02770e4 78 case COMPASS_WEST: mag_west = sample[2]; break;
brunohorta 3:f14ad02770e4 79 case COMPASS_EAST: mag_west = 0-sample[2]; break;
brunohorta 3:f14ad02770e4 80
brunohorta 3:f14ad02770e4 81 // Don't care
brunohorta 3:f14ad02770e4 82 case COMPASS_UP:
brunohorta 3:f14ad02770e4 83 case COMPASS_DOWN:
brunohorta 3:f14ad02770e4 84 break;
brunohorta 3:f14ad02770e4 85 }
brunohorta 3:f14ad02770e4 86
brunohorta 3:f14ad02770e4 87 // Y = bits 3 - 5
brunohorta 3:f14ad02770e4 88 switch(((mode >> 5) >> 3) & 0x07 )
brunohorta 3:f14ad02770e4 89 {
brunohorta 3:f14ad02770e4 90 case COMPASS_NORTH: mag_north = sample[1]; break;
brunohorta 3:f14ad02770e4 91 case COMPASS_SOUTH: mag_north = 0-sample[1]; ; break;
brunohorta 3:f14ad02770e4 92 case COMPASS_WEST: mag_west = sample[1]; break;
brunohorta 3:f14ad02770e4 93 case COMPASS_EAST: mag_west = 0-sample[1]; break;
brunohorta 3:f14ad02770e4 94
brunohorta 3:f14ad02770e4 95 // Don't care
brunohorta 3:f14ad02770e4 96 case COMPASS_UP:
brunohorta 3:f14ad02770e4 97 case COMPASS_DOWN:
brunohorta 3:f14ad02770e4 98 break;
brunohorta 3:f14ad02770e4 99 }
brunohorta 3:f14ad02770e4 100
brunohorta 3:f14ad02770e4 101 // X = bits 6 - 8
brunohorta 3:f14ad02770e4 102 switch(((mode >> 5) >> 6) & 0x07 )
brunohorta 3:f14ad02770e4 103 {
brunohorta 3:f14ad02770e4 104 case COMPASS_NORTH: mag_north = sample[0]; break;
brunohorta 3:f14ad02770e4 105 case COMPASS_SOUTH: mag_north = 0-sample[0]; break;
brunohorta 3:f14ad02770e4 106 case COMPASS_WEST: mag_west = sample[0]; break;
brunohorta 3:f14ad02770e4 107 case COMPASS_EAST: mag_west = 0-sample[0]; break;
brunohorta 3:f14ad02770e4 108
brunohorta 3:f14ad02770e4 109 // Don't care
brunohorta 3:f14ad02770e4 110 case COMPASS_UP:
brunohorta 3:f14ad02770e4 111 case COMPASS_DOWN:
brunohorta 3:f14ad02770e4 112 break;
brunohorta 3:f14ad02770e4 113 }
brunohorta 3:f14ad02770e4 114 if(s_mag_north == -100000) {
brunohorta 3:f14ad02770e4 115 s_mag_north = mag_north;
brunohorta 3:f14ad02770e4 116 }
brunohorta 3:f14ad02770e4 117 if(s_mag_west == -100000) {
brunohorta 3:f14ad02770e4 118 s_mag_west = mag_west;
brunohorta 3:f14ad02770e4 119 }
brunohorta 3:f14ad02770e4 120 // calculate heading from the north and west magnetic axes
brunohorta 3:f14ad02770e4 121 heading = atan2(mag_west - s_mag_west, mag_north- s_mag_north);
brunohorta 3:f14ad02770e4 122 // Adjust the heading by the declination
brunohorta 3:f14ad02770e4 123 heading += declination_offset_radians;
brunohorta 3:f14ad02770e4 124
brunohorta 3:f14ad02770e4 125 // Correct for when signs are reversed.
brunohorta 3:f14ad02770e4 126 if(heading < 0)
brunohorta 3:f14ad02770e4 127 heading += 2*M_PI;
brunohorta 3:f14ad02770e4 128
brunohorta 3:f14ad02770e4 129 // Check for wrap due to addition of declination.
brunohorta 3:f14ad02770e4 130 if(heading > 2*M_PI)
brunohorta 3:f14ad02770e4 131 heading -= 2*M_PI;
brunohorta 3:f14ad02770e4 132
brunohorta 3:f14ad02770e4 133 // Convert radians to degrees for readability.
brunohorta 3:f14ad02770e4 134 heading = heading * 180/M_PI;
brunohorta 3:f14ad02770e4 135 return heading ;
brunohorta 3:f14ad02770e4 136 }
brunohorta 3:f14ad02770e4 137
brunohorta 3:f14ad02770e4 138
brunohorta 3:f14ad02770e4 139 void HMC5883L::readMagData(float* dest){
BaserK 0:e5f8da308b60 140 uint8_t rawData[6]; // x,y,z mag data
BaserK 0:e5f8da308b60 141
BaserK 0:e5f8da308b60 142 /* Read six raw data registers sequentially and write them into data array */
BaserK 0:e5f8da308b60 143 readBytes(HMC5883L_ADDRESS, OUT_X_MSB, 6, &rawData[0]);
BaserK 0:e5f8da308b60 144
BaserK 0:e5f8da308b60 145 /* Turn the MSB LSB into signed 16-bit value */
BaserK 0:e5f8da308b60 146 dest[0] = (int16_t)(((int16_t)rawData[0]<<8) | rawData[1]); // MAG_XOUT
BaserK 0:e5f8da308b60 147 dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]); // MAG_ZOUT
BaserK 0:e5f8da308b60 148 dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]); // MAG_YOUT
BaserK 0:e5f8da308b60 149
BaserK 0:e5f8da308b60 150
brunohorta 3:f14ad02770e4 151 }