Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 extern "C" {
00037
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
00057 APM_Compass_Class::APM_Compass_Class() : orientation(0), declination(0.0)
00058 {
00059
00060 offset[0] = 0;
00061 offset[1] = 0;
00062 offset[2] = 0;
00063
00064
00065 orientationMatrix = ROTATION_NONE;
00066 }
00067
00068
00069 bool APM_Compass_Class::Init(int initialiseWireLib)
00070 {
00071 unsigned long currentTime = millis();
00072 int numAttempts = 0;
00073 int success = 0;
00074
00075 if( initialiseWireLib != 0 )
00076 Wire.begin();
00077
00078 delay(10);
00079
00080
00081 calibration[0] = 1.0;
00082 calibration[1] = 1.0;
00083 calibration[2] = 1.0;
00084
00085 while( success == 0 && numAttempts < 5 )
00086 {
00087
00088 numAttempts++;
00089
00090
00091 Wire.beginTransmission(CompassAddress);
00092 Wire.send(ConfigRegA);
00093 Wire.send(PositiveBiasConfig);
00094 if (0 != Wire.endTransmission())
00095 continue;
00096 delay(50);
00097
00098
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
00112 Read();
00113 delay(10);
00114
00115
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
00123 success = 1;
00124 }
00125
00126
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);
00136 Wire.endTransmission();
00137 delay(50);
00138 }
00139 return(success);
00140 }
00141
00142
00143 void APM_Compass_Class::Read()
00144 {
00145 int i = 0;
00146 byte buff[6];
00147
00148 Wire.beginTransmission(CompassAddress);
00149 Wire.send(0x03);
00150 Wire.endTransmission();
00151
00152
00153 Wire.requestFrom(CompassAddress, 6);
00154 while(Wire.available())
00155 {
00156 buff[i] = Wire.receive();
00157 i++;
00158 }
00159 Wire.endTransmission();
00160
00161 if (i==6)
00162 {
00163
00164 Mag_X = -((((int)buff[0]) << 8) | buff[1]) * calibration[0];
00165 Mag_Y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1];
00166 Mag_Z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2];
00167 lastUpdate = millis();
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);
00182 sin_roll = sin(roll);
00183 cos_pitch = cos(pitch);
00184 sin_pitch = sin(pitch);
00185
00186
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
00193 Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00194
00195 Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00196
00197 Heading = atan2(-Head_Y,Head_X);
00198
00199
00200 if( declination != 0.0 )
00201 {
00202 Heading = Heading + declination;
00203 if (Heading > M_PI)
00204 Heading -= (2.0 * M_PI);
00205 else if (Heading < -M_PI)
00206 Heading += (2.0 * M_PI);
00207 }
00208
00209
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
00237 APM_Compass_HIL_Class::APM_Compass_HIL_Class() : orientation(0), declination(0.0)
00238 {
00239
00240 offset[0] = 0;
00241 offset[1] = 0;
00242 offset[2] = 0;
00243
00244
00245 orientationMatrix = ROTATION_NONE;
00246 }
00247
00248
00249 bool APM_Compass_HIL_Class::Init(int initialiseWireLib)
00250 {
00251 unsigned long currentTime = millis();
00252 int numAttempts = 0;
00253 int success = 0;
00254
00255
00256 calibration[0] = 1.0;
00257 calibration[1] = 1.0;
00258 calibration[2] = 1.0;
00259
00260 while( success == 0 && numAttempts < 5 )
00261 {
00262
00263 numAttempts++;
00264
00265
00266 Read();
00267 delay(10);
00268
00269
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
00277 success = 1;
00278 }
00279 }
00280 return(success);
00281 }
00282
00283
00284 void APM_Compass_HIL_Class::Read()
00285 {
00286
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);
00300 sin_roll = sin(roll);
00301 cos_pitch = cos(pitch);
00302 sin_pitch = sin(pitch);
00303
00304
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
00311 Head_X = rotMagVec.x*cos_pitch+rotMagVec.y*sin_roll*sin_pitch+rotMagVec.z*cos_roll*sin_pitch;
00312
00313 Head_Y = rotMagVec.y*cos_roll-rotMagVec.z*sin_roll;
00314
00315 Heading = atan2(-Head_Y,Head_X);
00316
00317
00318 if( declination != 0.0 )
00319 {
00320 Heading = Heading + declination;
00321 if (Heading > M_PI)
00322 Heading -= (2.0 * M_PI);
00323 else if (Heading < -M_PI)
00324 Heading += (2.0 * M_PI);
00325 }
00326
00327
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
00356 Mag_X = _Mag_X;
00357 Mag_Y = _Mag_Y;
00358 Mag_Z = _Mag_Z;
00359 }