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 memset(offset, 0, sizeof(offset));
00019
00020
00021 orientation_matrix = ROTATION_NONE;
00022 }
00023
00024
00025 bool AP_Compass_HIL::init(int initialise_wire_lib)
00026 {
00027 unsigned long currentTime = millis();
00028 int numAttempts = 0;
00029 int success = 0;
00030
00031
00032 calibration[0] = 1.0;
00033 calibration[1] = 1.0;
00034 calibration[2] = 1.0;
00035
00036 while( success == 0 && numAttempts < 5 )
00037 {
00038
00039 numAttempts++;
00040
00041
00042 read();
00043 delay(10);
00044
00045
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
00053 success = 1;
00054 }
00055 }
00056 return(success);
00057 }
00058
00059
00060 void AP_Compass_HIL::read()
00061 {
00062
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);
00076 sin_roll = sin(roll);
00077 cos_pitch = cos(pitch);
00078 sin_pitch = sin(pitch);
00079
00080
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
00087 headX = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00088
00089 headY = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00090
00091 heading = atan2(-headY,headX);
00092
00093
00094 if( declination != 0.0 )
00095 {
00096 heading = heading + declination;
00097 if (heading > M_PI)
00098 heading -= (2.0 * M_PI);
00099 else if (heading < -M_PI)
00100 heading += (2.0 * M_PI);
00101 }
00102
00103
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
00132 mag_x = _mag_x;
00133 mag_y = _mag_y;
00134 mag_z = _mag_z;
00135 }