Diff: HMC5883L.cpp

Revision:

3:f14ad02770e4

Parent:

2:bbc9ad18fd3e

--- a/HMC5883L.cpp	Wed Aug 05 13:16:12 2015 +0000
+++ b/HMC5883L.cpp	Wed May 30 19:14:58 2018 +0000
@@ -1,80 +1,11 @@
-/*   HMC5883L Digital Compass Library
-*
-*    @author: Baser Kandehir 
-*    @date: August 5, 2015
-*    @license: MIT license
-*     
-*   Copyright (c) 2015, Baser Kandehir, baser.kandehir@ieee.metu.edu.tr
-*
-*   Permission is hereby granted, free of charge, to any person obtaining a copy
-*   of this software and associated documentation files (the "Software"), to deal
-*   in the Software without restriction, including without limitation the rights
-*   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-*   copies of the Software, and to permit persons to whom the Software is
-*   furnished to do so, subject to the following conditions:
-*
-*   The above copyright notice and this permission notice shall be included in
-*   all copies or substantial portions of the Software.
-*
-*   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-*   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-*   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-*   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-*   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-*   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-*   THE SOFTWARE.
-*
-*/
-
-// Some part of the code is adapted from Adafruit HMC5883 library
 
 #include "HMC5883L.h"
-
-/* NUCLEO F411RE board */
-static I2C i2c(D14, D15);         // setup i2c (SDA,SCL)  
+static I2C i2c(p4, p2);         // setup i2c (SDA,SCL)  
 
-static float Gauss_LSB_XY = 1100.0F; // Varies with gain
-static float Gauss_LSB_Z =  980.0F;  // Varies with gain
- 
-void HMC5883L::setMagGain(MagGain gain)
-{
-    writeByte(HMC5883L_ADDRESS, CONFIG_B, (int8_t)gain);    
-    
-    switch(gain)
-    {
-        case MagGain_13:
-            Gauss_LSB_XY = 1100;
-            Gauss_LSB_Z  = 980;
-            break;
-        case MagGain_19:
-            Gauss_LSB_XY = 855;
-            Gauss_LSB_Z  = 760;
-            break;
-        case MagGain_25:
-            Gauss_LSB_XY = 670;
-            Gauss_LSB_Z  = 600;
-            break;
-        case MagGain_40:
-            Gauss_LSB_XY = 450;
-            Gauss_LSB_Z  = 400;
-            break;
-        case MagGain_47:
-            Gauss_LSB_XY = 400;
-            Gauss_LSB_Z  = 255;
-            break;
-        case MagGain_56:
-            Gauss_LSB_XY = 330;
-            Gauss_LSB_Z  = 295;
-            break;
-        case MagGain_81:
-            Gauss_LSB_XY = 230;
-            Gauss_LSB_Z  = 205;
-            break;
-    }  
-}
+ uint16_t  mode;
+float    declination_offset_radians;
 
-void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data)
-{
+void HMC5883L::writeByte(uint8_t address, uint8_t regAddress, uint8_t data){
     char data_write[2];
     data_write[0]=regAddress;           // I2C sends MSB first. Namely  >>|regAddress|>>|data|
     data_write[1]=data;
@@ -101,16 +32,111 @@
         dest[i]=data[i];
 }
 
-void HMC5883L::init()
-{
+void HMC5883L::init(){
+    declination_offset_radians =  0 - (( 54 + (1/60 * 4) ) * (M_PI / 180));
+    mode = COMPASS_CONTINUOUS | COMPASS_SCALE_130 | COMPASS_VERTICAL_X_EAST;  
     writeByte(HMC5883L_ADDRESS, CONFIG_A, 0x78);  // Number of samples averaged: 8, Data output rate: 75 Hz
-    writeByte(HMC5883L_ADDRESS, MODE,     0x00);  // Continuous-Measurement Mode
-    setMagGain(MagGain_13);
+    writeByte(HMC5883L_ADDRESS, MODE,     mode & 0x03);  // Continuous-Measurement Mode
     wait_ms(10);
 }
+void HMC5883L::setScale( uint16_t scale ){
+  // Scale is the bits marked S in mode
+  //    xxxxxxxxxxxSSSMM  
+  mode = (mode & ~0x1C) | (scale & 0x1C);
+  writeByte(HMC5883L_ADDRESS,CONFIG_B, (( mode >> 2 ) & 0x07) << 5);
+}
 
-void HMC5883L::readMagData(float* dest)
+/** Set the orientation to one of COMPASS_HORIZONTAL_X_NORTH 
+ * through COMPASS_VERTICAL_Y_WEST
+ *  
+ */
+
+void HMC5883L::setOrientation( uint16_t orientation )
 {
+  // Orientation is the bits marked XXXYYYZZZ in mode
+  //    xxXXXYYYZZZxxxxx
+  mode = (mode & ~0x3FE0) | (orientation & 0x3FE0);    
+}
+
+float s_mag_north, s_mag_west;
+double HMC5883L::getHeading(){
+      
+    float sample[3];
+    readMagData(sample);
+  
+  
+  
+  float heading;    
+  
+  // Determine which of the Axes to use for North and West (when compass is "pointing" north)
+  float mag_north, mag_west = -100000;
+
+ switch((mode >> 5) & 0x07 )
+  {
+    case COMPASS_NORTH: mag_north = sample[2]; break;
+    case COMPASS_SOUTH: mag_north = 0-sample[2]; break;
+    case COMPASS_WEST:  mag_west  = sample[2]; break;
+    case COMPASS_EAST:  mag_west  = 0-sample[2]; break;
+      
+    // Don't care
+    case COMPASS_UP:
+    case COMPASS_DOWN:
+     break;
+  }
+  
+  // Y = bits 3 - 5
+  switch(((mode >> 5) >> 3) & 0x07 )
+  {
+    case COMPASS_NORTH: mag_north = sample[1];  break;
+    case COMPASS_SOUTH: mag_north = 0-sample[1]; ;  break;
+    case COMPASS_WEST:  mag_west  = sample[1];  break;
+    case COMPASS_EAST:  mag_west  = 0-sample[1];  break;
+      
+    // Don't care
+    case COMPASS_UP:
+    case COMPASS_DOWN:
+     break;
+  }
+  
+  // X = bits 6 - 8
+  switch(((mode >> 5) >> 6) & 0x07 )
+  {
+    case COMPASS_NORTH: mag_north = sample[0]; break;
+    case COMPASS_SOUTH: mag_north = 0-sample[0]; break;
+    case COMPASS_WEST:  mag_west  = sample[0]; break;
+    case COMPASS_EAST:  mag_west  = 0-sample[0]; break;
+      
+    // Don't care
+    case COMPASS_UP:
+    case COMPASS_DOWN:
+     break;
+  }
+    if(s_mag_north == -100000) {
+        s_mag_north = mag_north;
+    }
+    if(s_mag_west == -100000) {
+        s_mag_west = mag_west;
+    }
+  // calculate heading from the north and west magnetic axes
+  heading = atan2(mag_west - s_mag_west, mag_north- s_mag_north);
+  // Adjust the heading by the declination
+  heading += declination_offset_radians;
+  
+  // Correct for when signs are reversed.
+  if(heading < 0)
+    heading += 2*M_PI;
+    
+ // Check for wrap due to addition of declination.
+  if(heading > 2*M_PI)
+    heading -= 2*M_PI;
+    
+  // Convert radians to degrees for readability.
+  heading =  heading * 180/M_PI; 
+  return heading ;
+}
+
+
+void HMC5883L::readMagData(float* dest){
     uint8_t rawData[6]; // x,y,z mag data
     
     /* Read six raw data registers sequentially and write them into data array */
@@ -121,36 +147,5 @@
     dest[2] = (int16_t)(((int16_t)rawData[2]<<8) | rawData[3]);  // MAG_ZOUT
     dest[1] = (int16_t)(((int16_t)rawData[4]<<8) | rawData[5]);  // MAG_YOUT 
     
-    /* Convert raw data to magnetic field values in microtesla */
-     dest[0] = dest[0] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
-     dest[1] = dest[1] / Gauss_LSB_XY * GAUSS_TO_MICROTESLA;
-     dest[2] = dest[2] / Gauss_LSB_Z  * GAUSS_TO_MICROTESLA;
-}
 
-double HMC5883L::getHeading()
-{
-    float magData[3];
-    readMagData(magData);
-    
-    /* Calculate the heading while Z axis of the module is pointing up */
-    double heading = atan2(magData[1], magData[0]);
-    
-    // After calculating heading declination angle should be added to heading which is the error of the magnetic field in specific location.
-    // declinationAngle can be found here http://www.magnetic-declination.com/
-    // For Ankara (my location) declinationAngle is ~5.5 degrees (0.096 radians)
-    float declinationAngle = 0.096;
-    heading += declinationAngle;
-    
-    // Correct for when signs are reversed.
-    if(heading < 0)
-        heading += 2*PI;
-    
-    // Check for wrap due to addition of declination.
-    if(heading > 2*PI)
-        heading -= 2*PI;
-    
-    /* Convert radian to degrees */
-    heading = heading * 180 / PI;  
-    
-    return heading;    
-}
+}
\ No newline at end of file