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

AP_Compass_HIL.cpp

Go to the documentation of this file.
00001 /*
00002         AP_Compass_HIL.cpp - Arduino Library for HIL model of HMC5843 I2C Magnetometer
00003         Code by James Goppert. DIYDrones.com
00004 
00005         This library is free software; you can redistribute it and / or
00006                 modify it under the terms of the GNU Lesser General Public
00007                 License as published by the Free Software Foundation; either
00008                 version 2.1 of the License, or (at your option) any later version.
00009 */
00010 
00011 
00012 #include "AP_Compass_HIL.h"
00013 
00014 // Constructors ////////////////////////////////////////////////////////////////
00015 AP_Compass_HIL::AP_Compass_HIL() : orientation(0), declination(0.0)
00016 {
00017   // mag x y z offset initialisation
00018   memset(offset, 0, sizeof(offset));
00019   
00020   // initialise orientation matrix
00021   orientation_matrix = ROTATION_NONE;
00022 }
00023 
00024 // Public Methods //////////////////////////////////////////////////////////////
00025 bool AP_Compass_HIL::init(int initialise_wire_lib)
00026 {
00027   unsigned long currentTime = millis();  // record current time
00028   int numAttempts = 0;
00029   int success = 0;
00030   
00031   // calibration initialisation
00032   calibration[0] = 1.0;
00033   calibration[1] = 1.0;
00034   calibration[2] = 1.0;
00035   
00036   while( success == 0 && numAttempts < 5 )
00037   {
00038       // record number of attempts at initialisation
00039           numAttempts++;
00040   
00041           // read values from the compass
00042           read();
00043           delay(10);
00044 
00045           // calibrate
00046           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)
00047           {
00048                   calibration[0] = fabs(715.0 / mag_x);
00049                   calibration[1] = fabs(715.0 / mag_y);
00050                   calibration[2] = fabs(715.0 / mag_z);
00051                   
00052                   // mark success
00053                   success = 1;
00054           }
00055   }
00056   return(success);
00057 }
00058 
00059 // Read Sensor data
00060 void AP_Compass_HIL::read()
00061 {
00062     // values set by setHIL function
00063 }
00064 
00065 void AP_Compass_HIL::calculate(float roll, float pitch)
00066 {
00067   float headX;
00068   float headY;
00069   float cos_roll;
00070   float sin_roll;
00071   float cos_pitch;
00072   float sin_pitch;
00073   Vector3f rotMagVec;
00074   
00075   cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
00076   sin_roll = sin(roll);
00077   cos_pitch = cos(pitch);
00078   sin_pitch = sin(pitch);
00079   
00080   // rotate the magnetometer values depending upon orientation
00081   if( orientation == 0 )
00082       rotMagVec = Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]);  
00083   else
00084       rotMagVec = orientation_matrix*Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]); 
00085   
00086   // Tilt compensated Magnetic field X component:
00087   headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00088   // Tilt compensated Magnetic field Y component:
00089   headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00090   // Magnetic heading
00091   heading = atan2(-headY,headX);
00092   
00093   // Declination correction (if supplied)
00094   if( declination != 0.0 ) 
00095   {
00096       heading = heading + declination;
00097       if (heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
00098           heading -= (2.0 * M_PI);
00099       else if (heading < -M_PI)
00100           heading += (2.0 * M_PI);
00101   }
00102         
00103   // Optimization for external DCM use. Calculate normalized components
00104   heading_x = cos(heading);
00105   heading_y = sin(heading);
00106 }
00107 
00108 void AP_Compass_HIL::set_orientation(const Matrix3f &rotation_matrix)
00109 {
00110     orientation_matrix = rotation_matrix;
00111         if( orientation_matrix == ROTATION_NONE )
00112             orientation = 0;
00113         else
00114             orientation = 1;
00115 }
00116 
00117 void AP_Compass_HIL::set_offsets(int x, int y, int z)
00118 {
00119     offset[0] = x;
00120         offset[1] = y;
00121         offset[2] = z;
00122 }
00123 
00124 void AP_Compass_HIL::set_declination(float radians)
00125 {
00126     declination = radians;
00127 }
00128 
00129 void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
00130 {
00131     // TODO: map floats to raw
00132     mag_x = _mag_x;
00133     mag_y = _mag_y;
00134     mag_z = _mag_z;
00135 }

Generated for ArduPilot Libraries by doxygen