• Main Page
  • Related Pages
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

AP_Compass_HMC5843.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
00002 /*
00003         AP_Compass_HMC5843.cpp - Arduino Library for HMC5843 I2C magnetometer
00004         Code by Jordi Muñoz and Jose Julio. DIYDrones.com
00005 
00006         This library is free software; you can redistribute it and/or
00007     modify it under the terms of the GNU Lesser General Public
00008     License as published by the Free Software Foundation; either
00009     version 2.1 of the License, or (at your option) any later version.
00010 
00011         Sensor is conected to I2C port
00012         Sensor is initialized in Continuos mode (10Hz)
00013         
00014         Variables:
00015                 heading : magnetic heading
00016                 heading_x : magnetic heading X component
00017                 heading_y : magnetic heading Y component
00018                 mag_x : Raw X axis magnetometer data
00019                 mag_y : Raw Y axis magnetometer data
00020                 mag_z : Raw Z axis magnetometer data            
00021                 last_update : the time of the last successful reading           
00022         
00023         Methods:
00024                 init() : Initialization of I2C and sensor
00025                 read() : Read Sensor data       
00026                 calculate(float roll, float pitch) : Calculate tilt adjusted heading
00027                 set_orientation(const Matrix3f &rotation_matrix) : Set orientation of compass
00028                 set_offsets(int x, int y, int z) : Set adjustments for HardIron disturbances
00029                 set_declination(float radians) : Set heading adjustment between true north and magnetic north
00030 
00031         To do : code optimization
00032         Mount position : UPDATED
00033                 Big capacitor pointing backward, connector forward
00034                 
00035 */
00036 
00037 // AVR LibC Includes
00038 #include <math.h>
00039 #include "WConstants.h"
00040 
00041 #include <Wire.h>
00042 #include "AP_Compass_HMC5843.h"
00043 
00044 #define COMPASS_ADDRESS       0x1E
00045 #define ConfigRegA           0x00
00046 #define ConfigRegB           0x01
00047 #define magGain              0x20
00048 #define PositiveBiasConfig   0x11
00049 #define NegativeBiasConfig   0x12
00050 #define NormalOperation      0x10
00051 #define ModeRegister         0x02
00052 #define ContinuousConversion 0x00
00053 #define SingleConversion     0x01
00054 
00055 // Constructors ////////////////////////////////////////////////////////////////
00056 AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0)
00057 {
00058   // mag x y z offset initialisation
00059   offset[0] = 0;
00060   offset[1] = 0;
00061   offset[2] = 0;
00062   
00063   // initialise orientation matrix
00064   orientation_matrix = ROTATION_NONE;
00065 }
00066 
00067 // Public Methods //////////////////////////////////////////////////////////////
00068 bool AP_Compass_HMC5843::init(int initialise_wire_lib)
00069 {
00070   unsigned long currentTime = millis();  // record current time
00071   int numAttempts = 0;
00072   int success = 0;
00073   
00074   if( initialise_wire_lib != 0 )
00075       Wire.begin();
00076   
00077   delay(10);
00078   
00079   // calibration initialisation
00080   calibration[0] = 1.0;
00081   calibration[1] = 1.0;
00082   calibration[2] = 1.0;
00083   
00084   while( success == 0 && numAttempts < 5 )
00085   {
00086       // record number of attempts at initialisation
00087           numAttempts++;
00088   
00089           // force positiveBias (compass should return 715 for all channels)
00090           Wire.beginTransmission(COMPASS_ADDRESS);
00091           Wire.send(ConfigRegA);
00092           Wire.send(PositiveBiasConfig);
00093           if (0 != Wire.endTransmission())
00094                  continue;                      // compass not responding on the bus
00095           delay(50);
00096           
00097           // set gains
00098           Wire.beginTransmission(COMPASS_ADDRESS);
00099           Wire.send(ConfigRegB);
00100           Wire.send(magGain);
00101           Wire.endTransmission();
00102           delay(10);  
00103 
00104           Wire.beginTransmission(COMPASS_ADDRESS);
00105           Wire.send(ModeRegister);
00106           Wire.send(SingleConversion);
00107           Wire.endTransmission();
00108           delay(10);
00109           
00110           // read values from the compass
00111           read();
00112           delay(10);
00113 
00114           // calibrate
00115           if( abs(mag_x) > 500 && abs(mag_x) < 1000 && abs(mag_y) > 500 && abs(mag_y) < 1000 && abs(mag_z) > 500 && abs(mag_z) < 1000)
00116           {
00117                   calibration[0] = fabs(715.0 / mag_x);
00118                   calibration[1] = fabs(715.0 / mag_y);
00119                   calibration[2] = fabs(715.0 / mag_z);
00120                   
00121                   // mark success
00122                   success = 1;
00123           }
00124                 
00125           // leave test mode
00126           Wire.beginTransmission(COMPASS_ADDRESS);
00127           Wire.send(ConfigRegA);
00128           Wire.send(NormalOperation);
00129           Wire.endTransmission();
00130           delay(50);
00131 
00132           Wire.beginTransmission(COMPASS_ADDRESS);
00133           Wire.send(ModeRegister);
00134           Wire.send(ContinuousConversion);        // Set continuous mode (default to 10Hz)
00135           Wire.endTransmission();                 // End transmission
00136           delay(50);
00137   }
00138   return(success);
00139 }
00140 
00141 // Read Sensor data
00142 void AP_Compass_HMC5843::read()
00143 {
00144   int i = 0;
00145   byte buff[6];
00146  
00147   Wire.beginTransmission(COMPASS_ADDRESS); 
00148   Wire.send(0x03);        //sends address to read from
00149   Wire.endTransmission(); //end transmission
00150   
00151   //Wire.beginTransmission(COMPASS_ADDRESS); 
00152   Wire.requestFrom(COMPASS_ADDRESS, 6);    // request 6 bytes from device
00153   while(Wire.available())     
00154   { 
00155     buff[i] = Wire.receive();  // receive one byte
00156     i++;
00157   }
00158   Wire.endTransmission(); //end transmission
00159   
00160   if (i==6)  // All bytes received?
00161   {
00162     // MSB byte first, then LSB, X,Y,Z
00163     mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0];    // X axis
00164     mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1];    // Y axis
00165     mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2];    // Z axis
00166     last_update = millis();  // record time of update
00167   }
00168 }
00169 
00170 void AP_Compass_HMC5843::calculate(float roll, float pitch)
00171 {
00172   float headX;
00173   float headY;
00174   float cos_roll;
00175   float sin_roll;
00176   float cos_pitch;
00177   float sin_pitch;
00178   Vector3f rotmagVec;
00179   
00180   cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
00181   sin_roll = sin(roll);
00182   cos_pitch = cos(pitch);
00183   sin_pitch = sin(pitch);
00184   
00185   // rotate the magnetometer values depending upon orientation
00186   if( orientation == 0 )
00187       rotmagVec = Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]);  
00188   else
00189       rotmagVec = orientation_matrix*Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]); 
00190   
00191   // Tilt compensated magnetic field X component:
00192   headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.z*cos_roll*sin_pitch;
00193   // Tilt compensated magnetic field Y component:
00194   headY = rotmagVec.y*cos_roll-rotmagVec.z*sin_roll;
00195   // magnetic heading
00196   heading = atan2(-headY,headX);
00197   
00198   // Declination correction (if supplied)
00199   if( declination != 0.0 ) 
00200   {
00201       heading = heading + declination;
00202       if (heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
00203           heading -= (2.0 * M_PI);
00204       else if (heading < -M_PI)
00205           heading += (2.0 * M_PI);
00206   }
00207         
00208   // Optimization for external DCM use. Calculate normalized components
00209   heading_x = cos(heading);
00210   heading_y = sin(heading);
00211 }
00212 
00213 void AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix)
00214 {
00215     orientation_matrix = rotation_matrix;
00216         if( orientation_matrix == ROTATION_NONE )
00217             orientation = 0;
00218         else
00219             orientation = 1;
00220 }
00221 
00222 void AP_Compass_HMC5843::set_offsets(int x, int y, int z)
00223 {
00224     offset[0] = x;
00225         offset[1] = y;
00226         offset[2] = z;
00227 }
00228 
00229 void AP_Compass_HMC5843::set_declination(float radians)
00230 {
00231     declination = radians;
00232 }

Generated for ArduPilot Libraries by doxygen