Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "AP_Compass_HIL.h"
00013
00014
00015 AP_Compass_HIL::AP_Compass_HIL() : orientation(0), declination(0.0)
00016 {
00017
00018 offset[0] = 0;
00019 offset[1] = 0;
00020 offset[2] = 0;
00021
00022
00023 orientation_matrix = ROTATION_NONE;
00024 }
00025
00026
00027 bool AP_Compass_HIL::init(int initialise_wire_lib)
00028 {
00029 unsigned long currentTime = millis();
00030 int numAttempts = 0;
00031 int success = 0;
00032
00033
00034 calibration[0] = 1.0;
00035 calibration[1] = 1.0;
00036 calibration[2] = 1.0;
00037
00038 while( success == 0 && numAttempts < 5 )
00039 {
00040
00041 numAttempts++;
00042
00043
00044 read();
00045 delay(10);
00046
00047
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
00055 success = 1;
00056 }
00057 }
00058 return(success);
00059 }
00060
00061
00062 void AP_Compass_HIL::read()
00063 {
00064
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);
00078 sin_roll = sin(roll);
00079 cos_pitch = cos(pitch);
00080 sin_pitch = sin(pitch);
00081
00082
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
00089 headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00090
00091 headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00092
00093 heading = atan2(-headY,headX);
00094
00095
00096 if( declination != 0.0 )
00097 {
00098 heading = heading + declination;
00099 if (heading > M_PI)
00100 heading -= (2.0 * M_PI);
00101 else if (heading < -M_PI)
00102 heading += (2.0 * M_PI);
00103 }
00104
00105
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
00134 mag_x = _mag_x;
00135 mag_y = _mag_y;
00136 mag_z = _mag_z;
00137 }