openXsensor/oXs_hmc5883.cpp

   1 /* =============================================================================
   2 This part is based on Nav6 project (source code is placed under the MIT license)
   3
   4 Copyright (c) 2013 Kauai Labs
   5
   6 Portions of this work are based upon the I2C Dev Library by Jeff Rowberg
   7 (www.i2cdevlib.com) which is open-source licensed under the MIT
   8 License.
   9
  10 This work also incorporates modifications on the official Invensense Motion
  11 Driver v. 5.1 library in order to reduce the size of flash memory.
  12 =============================================================================
  13 */
  14
  15
  16
  17
  18
  19 #include "oXs_hmc5883.h"
  20
  21 #ifdef USE_HMC5883
  22
  23 #ifdef DEBUG
  24   #ifndef GENERATE_MAG_CALIBRATION_DATA
  25 //    #define DEBUG_HMC5883
  26   #endif
  27 #endif
  28
  29     float magHeading ;
  30     boolean newMagHeading ;
  31     const int16_t XMagOffset = XMAG_OFFSET ;
  32     const int16_t YMagOffset = YMAG_OFFSET ;
  33     const int16_t ZMagOffset = ZMAG_OFFSET ;
  34     const float XX  = XXMAG_CORRECTION ;
  35     const float XY  = XYMAG_CORRECTION ;
  36     const float XZ  = XZMAG_CORRECTION  ;
  37     const float YX  = YXMAG_CORRECTION  ;
  38     const float YY  = YYMAG_CORRECTION ;
  39     const float YZ  = YZMAG_CORRECTION ;
  40     const float ZX  = ZXMAG_CORRECTION ;
  41     const float ZY  = ZYMAG_CORRECTION ;
  42     const float ZZ  = ZZMAG_CORRECTION ;
  43
  44
  45 extern struct ONE_MEASUREMENT roll ;
  46 extern struct ONE_MEASUREMENT pitch ;
  47
  48 uint8_t hmc5883WriteByte (uint8_t reg ,  uint8_t character ) {
  49   return I2c.write ( HMC5883L_ADDRESS , reg , character ) ;
  50 }
  51
  52 boolean setup_hmc5883() {  // set up magnetometer
  53             // note: when HMC5883 is on a GY86, I2C has to be accessed through the IMU6050; it requires some special config in the IMU register; this is already done in the IMU initialisation
  54 #ifdef DEBUG_HMC5883
  55     Serial.println(F("Initializing HMC5883..."));
  56 #endif
  57     I2c.begin() ;
  58     I2c.timeOut( 80); //initialise the time out in order to avoid infinite loop
  59
  60 #ifdef DEBUG_HMC5883
  61     uint8_t error ;
  62     error = I2c.read( HMC5883L_ADDRESS , HMC5883L_REG_IDENT_A , 3 ) ;
  63     Serial.print(F("error= "));
  64     Serial.println(error) ;
  65     Serial.print(F("Id= "));
  66     Serial.print(I2c.receive()) ;
  67     Serial.print(F(" , "));
  68     Serial.print(I2c.receive()) ;
  69     Serial.print(F(" , "));
  70     Serial.println(I2c.receive()) ;
  71 #endif
  72     hmc5883WriteByte(HMC5883L_REG_CONFIG_B,  HMC5883L_RANGE_0_88GA << 5); // set the range on 0.88 GA(max sensitivity)
  73     hmc5883WriteByte(HMC5883L_REG_MODE, HMC5883L_CONTINOUS); // set in continuous mode
  74     hmc5883WriteByte(HMC5883L_REG_CONFIG_A , (HMC5883L_SAMPLES_8 << 5) | ( HMC5883L_DATARATE_75HZ << 2)) ; // Takes 8 samples to average, datarate = 75hz , normal measurement (no external magnetic field) )
  75
  76 #ifdef DEBUG_HMC5883
  77     Serial.println(F("HMC5883 init done"));
  78 #endif
  79   return true  ;
  80 }
  81
  82 boolean read_hmc5883() {   // return a flag that is true when a new heading is available
  83   static uint32_t previousMillisHmc ;
  84   uint32_t currentMillisHmc = millis() ;
  85   if( currentMillisHmc > ( previousMillisHmc + 200 ) ) { // read the sensor every 200 msec
  86       I2c.read( HMC5883L_ADDRESS , HMC5883L_REG_OUT_X_M , 6 ) ; // read 6 byte of data
  87
  88       int16_t X = (I2c.receive() << 8 ) |  I2c.receive() ;
  89       int16_t Z = (I2c.receive() << 8 ) |  I2c.receive() ;
  90       int16_t Y = (I2c.receive() << 8 ) |  I2c.receive() ;
  91
  92
  93
  94 #ifdef  GENERATE_MAG_CALIBRATION_DATA
  95     Serial.print( (int) X ) ; Serial.print( " ") ;
  96     Serial.print( (int) Y ) ; Serial.print( " ") ;
  97     Serial.println( (int) Z ) ;
  98 #endif
  99
 100     int16_t XWithOff = X - XMAG_OFFSET ;
 101     int16_t YWithOff = Y - YMAG_OFFSET ;
 102     int16_t ZWithOff = Z - ZMAG_OFFSET ;
 103     float XC = XXMAG_CORRECTION * XWithOff + XYMAG_CORRECTION * YWithOff + XZMAG_CORRECTION * ZWithOff ;
 104     float YC = YXMAG_CORRECTION * XWithOff + YYMAG_CORRECTION * YWithOff + YZMAG_CORRECTION * ZWithOff ;
 105     float ZC = ZXMAG_CORRECTION * XWithOff + ZYMAG_CORRECTION * YWithOff + ZZMAG_CORRECTION * ZWithOff ;
 106
 107     static float X1 ;
 108     static float Y1 ;
 109     static float Z1 ;
 110     static float rollRad1  ;
 111     static float pitchRad1 ;
 112     static float cosRoll ;
 113     static float sinRoll ;
 114     static float cosPitch;
 115     static float sinPitch;
 116
 117     static float Xh ;
 118     static float Yh ;
 119
 120   float rollRad = ((float) roll.value) * 0.0017453293  ; // from 1/10 of degree to rad
 121   float pitchRad = ((float )pitch.value) * 0.0017453293 ;
 122
 123
 124 #ifdef DEBUG_HMC5883
 125       static boolean firstPass = true ;
 126       if ( firstPass ) {
 127         firstPass = false ;
 128         Serial.println(F("X , Y , Z , uncomoensated tilted heading, roll , pitch ==> compensated tilted headings"));
 129       }
 130       Serial.print( (int) X ) ; Serial.print( " , ") ;
 131       Serial.print( (int) Y ) ; Serial.print( "  , ") ;
 132       Serial.print( (int) Z ) ; Serial.print( " - ") ;
 133       Serial.print( (int) ( atan2( YC , XC) * 57.29577951 ) ); Serial.print( "     ") ;
 134       Serial.print( (int) roll.value / 10) ; Serial.print( " ") ; // in deg
 135       Serial.print( (int) pitch.value  / 10 ) ; Serial.print( " ==> ") ;  // in deg
 136 #endif
 137
 138
 139   for ( uint8_t i = 10 ; i <= 10 ; i++) {         // this loop allows to test all combinations of reversing sign (to test them all, replace by i = 0 ; i<= 15
 140     X1 = XC ;
 141     Y1 = YC ;
 142     Z1 = ZC ;
 143     rollRad1 = rollRad ;
 144     pitchRad1 = pitchRad ;
 145     if (i & 0x01)  Y1 = - YC ;
 146     if (i & 0x02)  Z1 = - ZC ;
 147     if (i & 0x04)  rollRad1 = - rollRad ;
 148     if (i & 0x8)  pitchRad1 = - pitchRad ;
 149     //Serial.println( " ") ;
 150     cosRoll = cos(rollRad1);
 151     sinRoll = sin(rollRad1);
 152     cosPitch = cos(pitchRad1);
 153     sinPitch = sin(pitchRad1);
 154
 155     Xh = X1 * cosPitch + Y1 * sinRoll * sinPitch - Z1* cosRoll * sinPitch ;
 156     Yh = Y1 * cosRoll + Z1 * sinRoll ;
 157
 158
 159     if ( Xh == 0 ) {
 160       if (Yh <= 0 ) {
 161         magHeading  = 90 ;
 162       } else {
 163         magHeading  = 270 ;
 164       }
 165     } else {
 166       magHeading  = atan2( Yh , Xh) * 57.29577951 ; //convert in degree
 167     }
 168     newMagHeading = true ;
 169 #ifdef DEBUG_HMC5883
 170      Serial.print( (int) magHeading  ) ;Serial.print( "   ") ;
 171 #endif
 172
 173   } // end for
 174 #ifdef DEBUG_HMC5883
 175       Serial.println( " ") ;
 176 #endif
 177
 178       previousMillisHmc = currentMillisHmc ;
 179       return true ;
 180
 181   } else {
 182     return true ;
 183   }
 184 }   // end of read_hmc5883
 185
 186
 187
 188 #endif // end USE_HMC5883