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

/home/jgoppert/Projects/ap/libraries/AP_Compass/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   offset[0] = 0;
00019   offset[1] = 0;
00020   offset[2] = 0;
00021   
00022   // initialise orientation matrix
00023   orientation_matrix = ROTATION_NONE;
00024 }
00025 
00026 // Public Methods //////////////////////////////////////////////////////////////
00027 bool AP_Compass_HIL::init(int initialise_wire_lib)
00028 {
00029   unsigned long currentTime = millis();  // record current time
00030   int numAttempts = 0;
00031   int success = 0;
00032   
00033   // calibration initialisation
00034   calibration[0] = 1.0;
00035   calibration[1] = 1.0;
00036   calibration[2] = 1.0;
00037   
00038   while( success == 0 && numAttempts < 5 )
00039   {
00040       // record number of attempts at initialisation
00041           numAttempts++;
00042   
00043           // read values from the compass
00044           read();
00045           delay(10);
00046 
00047           // calibrate
00048           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)
00049           {
00050                   calibration[0] = fabs(715.0 / mag_x);
00051                   calibration[1] = fabs(715.0 / mag_y);
00052                   calibration[2] = fabs(715.0 / mag_z);
00053                   
00054                   // mark success
00055                   success = 1;
00056           }
00057   }
00058   return(success);
00059 }
00060 
00061 // Read Sensor data
00062 void AP_Compass_HIL::read()
00063 {
00064     // values set by setHIL function
00065 }
00066 
00067 void AP_Compass_HIL::calculate(float roll, float pitch)
00068 {
00069   float headX;
00070   float headY;
00071   float cos_roll;
00072   float sin_roll;
00073   float cos_pitch;
00074   float sin_pitch;
00075   Vector3f rotMagVec;
00076   
00077   cos_roll = cos(roll);  // Optimizacion, se puede sacar esto de la matriz DCM?
00078   sin_roll = sin(roll);
00079   cos_pitch = cos(pitch);
00080   sin_pitch = sin(pitch);
00081   
00082   // rotate the magnetometer values depending upon orientation
00083   if( orientation == 0 )
00084       rotMagVec = Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]);  
00085   else
00086       rotMagVec = orientation_matrix*Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]); 
00087   
00088   // Tilt compensated Magnetic field X component:
00089   headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00090   // Tilt compensated Magnetic field Y component:
00091   headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00092   // Magnetic heading
00093   heading = atan2(-headY,headX);
00094   
00095   // Declination correction (if supplied)
00096   if( declination != 0.0 ) 
00097   {
00098       heading = heading + declination;
00099       if (heading > M_PI)    // Angle normalization (-180 deg, 180 deg)
00100           heading -= (2.0 * M_PI);
00101       else if (heading < -M_PI)
00102           heading += (2.0 * M_PI);
00103   }
00104         
00105   // Optimization for external DCM use. Calculate normalized components
00106   heading_x = cos(heading);
00107   heading_y = sin(heading);
00108 }
00109 
00110 void AP_Compass_HIL::set_orientation(const Matrix3f &rotation_matrix)
00111 {
00112     orientation_matrix = rotation_matrix;
00113         if( orientation_matrix == ROTATION_NONE )
00114             orientation = 0;
00115         else
00116             orientation = 1;
00117 }
00118 
00119 void AP_Compass_HIL::set_offsets(int x, int y, int z)
00120 {
00121     offset[0] = x;
00122         offset[1] = y;
00123         offset[2] = z;
00124 }
00125 
00126 void AP_Compass_HIL::set_declination(float radians)
00127 {
00128     declination = radians;
00129 }
00130 
00131 void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
00132 {
00133     // TODO: map floats to raw
00134     mag_x = _mag_x;
00135     mag_y = _mag_y;
00136     mag_z = _mag_z;
00137 }

Generated for ArduPilot Libraries by doxygen