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

APM_Compass.cpp

Go to the documentation of this file.
00001 // -*- tab-width: 4; Mode: C++; c-basic-offset: 3; indent-tabs-mode: t -*-
00002 /*
00003         APM_Compass.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                 lastUpdate : 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                 SetOrientation(const Matrix3f &rotationMatrix) : Set orientation of compass
00028                 SetOffsets(int x, int y, int z) : Set adjustments for HardIron disturbances
00029                 SetDeclination(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 extern "C" {
00037   // AVR LibC Includes
00038   #include <math.h>
00039   #include "WConstants.h"
00040 }
00041 
00042 #include <Wire.h>
00043 #include "APM_Compass.h"
00044 
00045 #define CompassAddress       0x1E
00046 #define ConfigRegA           0x00
00047 #define ConfigRegB           0x01
00048 #define MagGain              0x20
00049 #define PositiveBiasConfig   0x11
00050 #define NegativeBiasConfig   0x12
00051 #define NormalOperation      0x10
00052 #define ModeRegister         0x02
00053 #define ContinuousConversion 0x00
00054 #define SingleConversion     0x01
00055 
00056 // Constructors ////////////////////////////////////////////////////////////////
00057 APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
00058 {
00059   // mag x y z offset initialisation
00060   offset[0] = 0;
00061   offset[1] = 0;
00062   offset[2] = 0;
00063   
00064   // initialise orientation matrix
00065   orientationMatrix = ROTATION_NONE;
00066 }
00067 
00068 // Public Methods //////////////////////////////////////////////////////////////
00069 bool APM_Compass_Class::Init(int initialiseWireLib)
00070 {
00071   unsigned long currentTime = millis();  // record current time
00072   int numAttempts = 0;
00073   int success = 0;
00074   
00075   if( initialiseWireLib != 0 )
00076       Wire.begin();
00077   
00078   delay(10);
00079   
00080   // calibration initialisation
00081   calibration[0] = 1.0;
00082   calibration[1] = 1.0;
00083   calibration[2] = 1.0;
00084   
00085   while( success == 0 && numAttempts < 5 )
00086   {
00087       // record number of attempts at initialisation
00088           numAttempts++;
00089   
00090           // force positiveBias (compass should return 715 for all channels)
00091           Wire.beginTransmission(CompassAddress);
00092           Wire.send(ConfigRegA);
00093           Wire.send(PositiveBiasConfig);
00094           if (0 != Wire.endTransmission())
00095                  continue;                      // compass not responding on the bus
00096           delay(50);
00097           
00098           // set gains
00099           Wire.beginTransmission(CompassAddress);
00100           Wire.send(ConfigRegB);
00101           Wire.send(MagGain);
00102           Wire.endTransmission();
00103           delay(10);  
00104 
00105           Wire.beginTransmission(CompassAddress);
00106           Wire.send(ModeRegister);
00107           Wire.send(SingleConversion);
00108           Wire.endTransmission();
00109           delay(10);
00110           
00111           // read values from the compass
00112           Read();
00113           delay(10);
00114 
00115           // calibrate
00116           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)
00117           {
00118                   calibration[0] = fabs(715.0 / Mag_X);
00119                   calibration[1] = fabs(715.0 / Mag_Y);
00120                   calibration[2] = fabs(715.0 / Mag_Z);
00121                   
00122                   // mark success
00123                   success = 1;
00124           }
00125                 
00126           // leave test mode
00127           Wire.beginTransmission(CompassAddress);
00128           Wire.send(ConfigRegA);
00129           Wire.send(NormalOperation);
00130           Wire.endTransmission();
00131           delay(50);
00132 
00133           Wire.beginTransmission(CompassAddress);
00134           Wire.send(ModeRegister);
00135           Wire.send(ContinuousConversion);        // Set continuous mode (default to 10Hz)
00136           Wire.endTransmission();                 // End transmission
00137           delay(50);
00138   }
00139   return(success);
00140 }
00141 
00142 // Read Sensor data
00143 void APM_Compass_Class::Read()
00144 {
00145   int i = 0;
00146   byte buff[6];
00147  
00148   Wire.beginTransmission(CompassAddress); 
00149   Wire.send(0x03);        //sends address to read from
00150   Wire.endTransmission(); //end transmission
00151   
00152   //Wire.beginTransmission(CompassAddress); 
00153   Wire.requestFrom(CompassAddress, 6);    // request 6 bytes from device
00154   while(Wire.available())     
00155   { 
00156     buff[i] = Wire.receive();  // receive one byte
00157     i++;
00158   }
00159   Wire.endTransmission(); //end transmission
00160   
00161   if (i==6)  // All bytes received?
00162   {
00163     // MSB byte first, then LSB, X,Y,Z
00164     Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0];    // X axis
00165     Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1];    // Y axis
00166     Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2];    // Z axis
00167     lastUpdate = millis();  // record time of update
00168   }
00169 }
00170 
00171 void APM_Compass_Class::Calculate(float roll, float pitch)
00172 {
00173   float Head_X;
00174   float Head_Y;
00175   float cos_roll;
00176   float sin_roll;
00177   float cos_pitch;
00178   float sin_pitch;
00179   Vector3f rotMagVec;
00180   
00181   cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
00182   sin_roll = sin(roll);
00183   cos_pitch = cos(pitch);
00184   sin_pitch = sin(pitch);
00185   
00186   // rotate the magnetometer values depending upon orientation
00187   if( orientation == 0 )
00188       rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);  
00189   else
00190       rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); 
00191   
00192   // Tilt compensated Magnetic field X component:
00193   Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00194   // Tilt compensated Magnetic field Y component:
00195   Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00196   // Magnetic Heading
00197   Heading = atan2(-Head_Y,Head_X);
00198   
00199   // Declination correction (if supplied)
00200   if( declination != 0.0 ) 
00201   {
00202       Heading = Heading + declination;
00203       if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
00204           Heading -= (2.0 * M_PI);
00205       else if (Heading < -M_PI)
00206           Heading += (2.0 * M_PI);
00207   }
00208         
00209   // Optimization for external DCM use. Calculate normalized components
00210   Heading_X = cos(Heading);
00211   Heading_Y = sin(Heading);
00212 }
00213 
00214 void APM_Compass_Class::SetOrientation(const Matrix3f &rotationMatrix)
00215 {
00216     orientationMatrix = rotationMatrix;
00217         if( orientationMatrix == ROTATION_NONE )
00218             orientation = 0;
00219         else
00220             orientation = 1;
00221 }
00222 
00223 void APM_Compass_Class::SetOffsets(int x, int y, int z)
00224 {
00225     offset[0] = x;
00226         offset[1] = y;
00227         offset[2] = z;
00228 }
00229 
00230 void APM_Compass_Class::SetDeclination(float radians)
00231 {
00232     declination = radians;
00233 }
00234 
00235 
00236 // Constructors ////////////////////////////////////////////////////////////////
00237 APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0)
00238 {
00239   // mag x y z offset initialisation
00240   offset[0] = 0;
00241   offset[1] = 0;
00242   offset[2] = 0;
00243   
00244   // initialise orientation matrix
00245   orientationMatrix = ROTATION_NONE;
00246 }
00247 
00248 // Public Methods //////////////////////////////////////////////////////////////
00249 bool APM_Compass_HIL_Class::Init(int initialiseWireLib)
00250 {
00251   unsigned long currentTime = millis();  // record current time
00252   int numAttempts = 0;
00253   int success = 0;
00254   
00255   // calibration initialisation
00256   calibration[0] = 1.0;
00257   calibration[1] = 1.0;
00258   calibration[2] = 1.0;
00259   
00260   while( success == 0 && numAttempts < 5 )
00261   {
00262       // record number of attempts at initialisation
00263           numAttempts++;
00264   
00265           // read values from the compass
00266           Read();
00267           delay(10);
00268 
00269           // calibrate
00270           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)
00271           {
00272                   calibration[0] = fabs(715.0 / Mag_X);
00273                   calibration[1] = fabs(715.0 / Mag_Y);
00274                   calibration[2] = fabs(715.0 / Mag_Z);
00275                   
00276                   // mark success
00277                   success = 1;
00278           }
00279   }
00280   return(success);
00281 }
00282 
00283 // Read Sensor data
00284 void APM_Compass_HIL_Class::Read()
00285 {
00286     // values set by setHIL function
00287 }
00288 
00289 void APM_Compass_HIL_Class::Calculate(float roll, float pitch)
00290 {
00291   float Head_X;
00292   float Head_Y;
00293   float cos_roll;
00294   float sin_roll;
00295   float cos_pitch;
00296   float sin_pitch;
00297   Vector3f rotMagVec;
00298   
00299   cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
00300   sin_roll = sin(roll);
00301   cos_pitch = cos(pitch);
00302   sin_pitch = sin(pitch);
00303   
00304   // rotate the magnetometer values depending upon orientation
00305   if( orientation == 0 )
00306       rotMagVec = Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]);  
00307   else
00308       rotMagVec = orientationMatrix*Vector3f(Mag_X+offset[0],Mag_Y+offset[1],Mag_Z+offset[2]); 
00309   
00310   // Tilt compensated Magnetic field X component:
00311   Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00312   // Tilt compensated Magnetic field Y component:
00313   Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00314   // Magnetic Heading
00315   Heading = atan2(-Head_Y,Head_X);
00316   
00317   // Declination correction (if supplied)
00318   if( declination != 0.0 ) 
00319   {
00320       Heading = Heading + declination;
00321       if (Heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
00322           Heading -= (2.0 * M_PI);
00323       else if (Heading < -M_PI)
00324           Heading += (2.0 * M_PI);
00325   }
00326         
00327   // Optimization for external DCM use. Calculate normalized components
00328   Heading_X = cos(Heading);
00329   Heading_Y = sin(Heading);
00330 }
00331 
00332 void APM_Compass_HIL_Class::SetOrientation(const Matrix3f &rotationMatrix)
00333 {
00334     orientationMatrix = rotationMatrix;
00335         if( orientationMatrix == ROTATION_NONE )
00336             orientation = 0;
00337         else
00338             orientation = 1;
00339 }
00340 
00341 void APM_Compass_HIL_Class::SetOffsets(int x, int y, int z)
00342 {
00343     offset[0] = x;
00344         offset[1] = y;
00345         offset[2] = z;
00346 }
00347 
00348 void APM_Compass_HIL_Class::SetDeclination(float radians)
00349 {
00350     declination = radians;
00351 }
00352 
00353 void APM_Compass_HIL_Class::setHIL(float _Mag_X, float _Mag_Y, float _Mag_Z)
00354 {
00355     // TODO: map floats to raw
00356     Mag_X = _Mag_X;
00357     Mag_Y = _Mag_Y;
00358     Mag_Z = _Mag_Z;
00359 }

Generated for ArduPilot Libraries by doxygen