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
00037
00038 #include <math.h>
00039 #include "WConstants.h"
00040
00041 #include <Wire.h>
00042 #include "AP_Compass_HMC5843.h"
00043
00044 #define COMPASS_ADDRESS 0x1E
00045 #define ConfigRegA 0x00
00046 #define ConfigRegB 0x01
00047 #define magGain 0x20
00048 #define PositiveBiasConfig 0x11
00049 #define NegativeBiasConfig 0x12
00050 #define NormalOperation 0x10
00051 #define ModeRegister 0x02
00052 #define ContinuousConversion 0x00
00053 #define SingleConversion 0x01
00054
00055
00056 AP_Compass_HMC5843::AP_Compass_HMC5843() : orientation(0), declination(0.0)
00057 {
00058
00059 offset[0] = 0;
00060 offset[1] = 0;
00061 offset[2] = 0;
00062
00063
00064 orientation_matrix = ROTATION_NONE;
00065 }
00066
00067
00068 bool AP_Compass_HMC5843::init(int initialise_wire_lib)
00069 {
00070 unsigned long currentTime = millis();
00071 int numAttempts = 0;
00072 int success = 0;
00073
00074 if( initialise_wire_lib != 0 )
00075 Wire.begin();
00076
00077 delay(10);
00078
00079
00080 calibration[0] = 1.0;
00081 calibration[1] = 1.0;
00082 calibration[2] = 1.0;
00083
00084 while( success == 0 && numAttempts < 5 )
00085 {
00086
00087 numAttempts++;
00088
00089
00090 Wire.beginTransmission(COMPASS_ADDRESS);
00091 Wire.send(ConfigRegA);
00092 Wire.send(PositiveBiasConfig);
00093 if (0 != Wire.endTransmission())
00094 continue;
00095 delay(50);
00096
00097
00098 Wire.beginTransmission(COMPASS_ADDRESS);
00099 Wire.send(ConfigRegB);
00100 Wire.send(magGain);
00101 Wire.endTransmission();
00102 delay(10);
00103
00104 Wire.beginTransmission(COMPASS_ADDRESS);
00105 Wire.send(ModeRegister);
00106 Wire.send(SingleConversion);
00107 Wire.endTransmission();
00108 delay(10);
00109
00110
00111 read();
00112 delay(10);
00113
00114
00115 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)
00116 {
00117 calibration[0] = fabs(715.0 / mag_x);
00118 calibration[1] = fabs(715.0 / mag_y);
00119 calibration[2] = fabs(715.0 / mag_z);
00120
00121
00122 success = 1;
00123 }
00124
00125
00126 Wire.beginTransmission(COMPASS_ADDRESS);
00127 Wire.send(ConfigRegA);
00128 Wire.send(NormalOperation);
00129 Wire.endTransmission();
00130 delay(50);
00131
00132 Wire.beginTransmission(COMPASS_ADDRESS);
00133 Wire.send(ModeRegister);
00134 Wire.send(ContinuousConversion);
00135 Wire.endTransmission();
00136 delay(50);
00137 }
00138 return(success);
00139 }
00140
00141
00142 void AP_Compass_HMC5843::read()
00143 {
00144 int i = 0;
00145 byte buff[6];
00146
00147 Wire.beginTransmission(COMPASS_ADDRESS);
00148 Wire.send(0x03);
00149 Wire.endTransmission();
00150
00151
00152 Wire.requestFrom(COMPASS_ADDRESS, 6);
00153 while(Wire.available())
00154 {
00155 buff[i] = Wire.receive();
00156 i++;
00157 }
00158 Wire.endTransmission();
00159
00160 if (i==6)
00161 {
00162
00163 mag_x = -((((int)buff[0]) << 8) | buff[1]) * calibration[0];
00164 mag_y = ((((int)buff[2]) << 8) | buff[3]) * calibration[1];
00165 mag_z = -((((int)buff[4]) << 8) | buff[5]) * calibration[2];
00166 last_update = millis();
00167 }
00168 }
00169
00170 void AP_Compass_HMC5843::calculate(float roll, float pitch)
00171 {
00172 float headX;
00173 float headY;
00174 float cos_roll;
00175 float sin_roll;
00176 float cos_pitch;
00177 float sin_pitch;
00178 Vector3f rotmagVec;
00179
00180 cos_roll = cos(roll);
00181 sin_roll = sin(roll);
00182 cos_pitch = cos(pitch);
00183 sin_pitch = sin(pitch);
00184
00185
00186 if( orientation == 0 )
00187 rotmagVec = Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]);
00188 else
00189 rotmagVec = orientation_matrix*Vector3f(mag_x+offset[0],mag_y+offset[1],mag_z+offset[2]);
00190
00191
00192 headX = rotmagVec.x*cos_pitch+rotmagVec.y*sin_roll*sin_pitch+rotmagVec.z*cos_roll*sin_pitch;
00193
00194 headY = rotmagVec.y*cos_roll-rotmagVec.z*sin_roll;
00195
00196 heading = atan2(-headY,headX);
00197
00198
00199 if( declination != 0.0 )
00200 {
00201 heading = heading + declination;
00202 if (heading > M_PI)
00203 heading -= (2.0 * M_PI);
00204 else if (heading < -M_PI)
00205 heading += (2.0 * M_PI);
00206 }
00207
00208
00209 heading_x = cos(heading);
00210 heading_y = sin(heading);
00211 }
00212
00213 void AP_Compass_HMC5843::set_orientation(const Matrix3f &rotation_matrix)
00214 {
00215 orientation_matrix = rotation_matrix;
00216 if( orientation_matrix == ROTATION_NONE )
00217 orientation = 0;
00218 else
00219 orientation = 1;
00220 }
00221
00222 void AP_Compass_HMC5843::set_offsets(int x, int y, int z)
00223 {
00224 offset[0] = x;
00225 offset[1] = y;
00226 offset[2] = z;
00227 }
00228
00229 void AP_Compass_HMC5843::set_declination(float radians)
00230 {
00231 declination = radians;
00232 }