Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

adapt/compass.cpp@66:5d43988d100c, 2014-05-05 (annotated)

Committer:
dylanembed123
Date:
Mon May 05 13:20:35 2014 +0000
Revision:
66:5d43988d100c
Parent:
10:c4745ddaaf6a 
Final Project;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
krobertson 10:c4745ddaaf6a 1 #include <compass.h>
krobertson 10:c4745ddaaf6a 2 #include <math.h>
krobertson 10:c4745ddaaf6a 3
krobertson 10:c4745ddaaf6a 4 SPI spi(p5, p6, p7); // mosi, miso, sclk
krobertson 10:c4745ddaaf6a 5 DigitalOut cs(p8);
krobertson 10:c4745ddaaf6a 6
krobertson 10:c4745ddaaf6a 7 // Defines ////////////////////////////////////////////////////////////////
krobertson 10:c4745ddaaf6a 8
krobertson 10:c4745ddaaf6a 9 #define D_WHO_ID 0x49
krobertson 10:c4745ddaaf6a 10 #define DLM_WHO_ID 0x3C
krobertson 10:c4745ddaaf6a 11
krobertson 10:c4745ddaaf6a 12 // Constructors ////////////////////////////////////////////////////////////////
krobertson 10:c4745ddaaf6a 13
krobertson 10:c4745ddaaf6a 14 Compass::Compass(void){
krobertson 10:c4745ddaaf6a 15 //These values lead to an assumed magnetometer bias of 0.
krobertson 10:c4745ddaaf6a 16 m_min = (Compass::vector<int16_t>){-32767, -32767, -32767};
krobertson 10:c4745ddaaf6a 17 m_max = (Compass::vector<int16_t>){+32767, +32767, +32767};
krobertson 10:c4745ddaaf6a 18 //automatically determine which device is being used
krobertson 10:c4745ddaaf6a 19 _device = device_auto;
krobertson 10:c4745ddaaf6a 20 //initialize chip select to high (not active)
krobertson 10:c4745ddaaf6a 21 cs=1;
krobertson 10:c4745ddaaf6a 22 }
krobertson 10:c4745ddaaf6a 23
krobertson 10:c4745ddaaf6a 24 bool Compass::init(deviceType device){
krobertson 10:c4745ddaaf6a 25 // Setup the spi for 8 bit data, low steady state clock,
krobertson 10:c4745ddaaf6a 26 // rising edge capture, with a 1MHz clock rate
krobertson 10:c4745ddaaf6a 27 spi.format(8,0);
krobertson 10:c4745ddaaf6a 28 spi.frequency(1000000);
krobertson 10:c4745ddaaf6a 29
krobertson 10:c4745ddaaf6a 30 // determine device type if necessary
krobertson 10:c4745ddaaf6a 31 if (device == device_auto){
krobertson 10:c4745ddaaf6a 32 if (testReg(WHO_AM_I) == D_WHO_ID){
krobertson 10:c4745ddaaf6a 33 // device responds with D ID; it's a D.
krobertson 10:c4745ddaaf6a 34 device = device_D;
krobertson 10:c4745ddaaf6a 35 }else{
krobertson 10:c4745ddaaf6a 36 // device hasn't responded meaningfully, so give up
krobertson 10:c4745ddaaf6a 37 return false;
krobertson 10:c4745ddaaf6a 38 }
krobertson 10:c4745ddaaf6a 39 }
krobertson 10:c4745ddaaf6a 40
krobertson 10:c4745ddaaf6a 41 _device = device;
krobertson 10:c4745ddaaf6a 42
krobertson 10:c4745ddaaf6a 43 // set device addresses and translated register addresses
krobertson 10:c4745ddaaf6a 44 switch (device){
krobertson 10:c4745ddaaf6a 45 case device_D:
krobertson 10:c4745ddaaf6a 46 translated_regs[-OUT_X_L_M] = D_OUT_X_L_M;
krobertson 10:c4745ddaaf6a 47 translated_regs[-OUT_X_H_M] = D_OUT_X_H_M;
krobertson 10:c4745ddaaf6a 48 translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M;
krobertson 10:c4745ddaaf6a 49 translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M;
krobertson 10:c4745ddaaf6a 50 translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M;
krobertson 10:c4745ddaaf6a 51 translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M;
krobertson 10:c4745ddaaf6a 52 break;
krobertson 10:c4745ddaaf6a 53 }
krobertson 10:c4745ddaaf6a 54 return true;
krobertson 10:c4745ddaaf6a 55 }
krobertson 10:c4745ddaaf6a 56
krobertson 10:c4745ddaaf6a 57 /*
krobertson 10:c4745ddaaf6a 58 Enables the Compass's accelerometer and magnetometer. Also:
krobertson 10:c4745ddaaf6a 59 - Sets sensor full scales (gain) to default power-on values, which are
krobertson 10:c4745ddaaf6a 60 +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer
krobertson 10:c4745ddaaf6a 61 (+/- 4 gauss on LSM303D).
krobertson 10:c4745ddaaf6a 62 - Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz
krobertson 10:c4745ddaaf6a 63 ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR
krobertson 10:c4745ddaaf6a 64 settings for which the electrical characteristics are specified in
krobertson 10:c4745ddaaf6a 65 the datasheets.)
krobertson 10:c4745ddaaf6a 66 - Enables high resolution modes (if available).
krobertson 10:c4745ddaaf6a 67 Note that this function will also reset other settings controlled by
krobertson 10:c4745ddaaf6a 68 the registers it writes to.
krobertson 10:c4745ddaaf6a 69 */
krobertson 10:c4745ddaaf6a 70 void Compass::enableDefault(void){
krobertson 10:c4745ddaaf6a 71 if (_device == device_D){
krobertson 10:c4745ddaaf6a 72 // Accelerometer
krobertson 10:c4745ddaaf6a 73 writeReg(CTRL0, 0x40);
krobertson 10:c4745ddaaf6a 74 writeReg(CTRL2, 0x00);
krobertson 10:c4745ddaaf6a 75
krobertson 10:c4745ddaaf6a 76 // 0x57 = 0b01010111
krobertson 10:c4745ddaaf6a 77 // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled)
krobertson 10:c4745ddaaf6a 78 writeReg(CTRL1, 0xA7);
krobertson 10:c4745ddaaf6a 79
krobertson 10:c4745ddaaf6a 80 // Magnetometer
krobertson 10:c4745ddaaf6a 81
krobertson 10:c4745ddaaf6a 82 // 0x64 = 0b01100100
krobertson 10:c4745ddaaf6a 83 // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR)
krobertson 10:c4745ddaaf6a 84 writeReg(CTRL5, 0x74);
krobertson 10:c4745ddaaf6a 85
krobertson 10:c4745ddaaf6a 86 // 0x20 = 0b00100000
krobertson 10:c4745ddaaf6a 87 // MFS = 01 (+/- 4 gauss full scale)
krobertson 10:c4745ddaaf6a 88 writeReg(CTRL6, 0x20);
krobertson 10:c4745ddaaf6a 89
krobertson 10:c4745ddaaf6a 90 // 0x00 = 0b00000000
krobertson 10:c4745ddaaf6a 91 // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode)
krobertson 10:c4745ddaaf6a 92 writeReg(CTRL7, 0x00);
krobertson 10:c4745ddaaf6a 93 }
krobertson 10:c4745ddaaf6a 94 }
krobertson 10:c4745ddaaf6a 95
krobertson 10:c4745ddaaf6a 96 // Writes an accelerometer register
krobertson 10:c4745ddaaf6a 97 void Compass::writeAccReg(regAddr reg, int value){
krobertson 10:c4745ddaaf6a 98 cs = 0;
krobertson 10:c4745ddaaf6a 99 spi.write(reg);
krobertson 10:c4745ddaaf6a 100 spi.write(value);
krobertson 10:c4745ddaaf6a 101 cs = 1;
krobertson 10:c4745ddaaf6a 102 }
krobertson 10:c4745ddaaf6a 103
krobertson 10:c4745ddaaf6a 104 // Reads an accelerometer register
krobertson 10:c4745ddaaf6a 105 int Compass::readAccReg(regAddr reg){
krobertson 10:c4745ddaaf6a 106 int value;
krobertson 10:c4745ddaaf6a 107 cs = 0;
krobertson 10:c4745ddaaf6a 108 spi.write(reg | 0x80);
krobertson 10:c4745ddaaf6a 109 value = spi.write(0x00);
krobertson 10:c4745ddaaf6a 110 cs = 1;
krobertson 10:c4745ddaaf6a 111 return value;
krobertson 10:c4745ddaaf6a 112 }
krobertson 10:c4745ddaaf6a 113
krobertson 10:c4745ddaaf6a 114 // Writes a magnetometer register
krobertson 10:c4745ddaaf6a 115 void Compass::writeMagReg(regAddr reg, int value){
krobertson 10:c4745ddaaf6a 116 cs = 0;
krobertson 10:c4745ddaaf6a 117 spi.write(reg);
krobertson 10:c4745ddaaf6a 118 spi.write(value);
krobertson 10:c4745ddaaf6a 119 cs = 1;
krobertson 10:c4745ddaaf6a 120 }
krobertson 10:c4745ddaaf6a 121
krobertson 10:c4745ddaaf6a 122 // Reads a magnetometer register
krobertson 10:c4745ddaaf6a 123 int Compass::readMagReg(regAddr reg){
krobertson 10:c4745ddaaf6a 124 int value;
krobertson 10:c4745ddaaf6a 125
krobertson 10:c4745ddaaf6a 126 // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type)
krobertson 10:c4745ddaaf6a 127 if (reg < 0){
krobertson 10:c4745ddaaf6a 128 reg = translated_regs[-reg];
krobertson 10:c4745ddaaf6a 129 }
krobertson 10:c4745ddaaf6a 130 cs = 0;
krobertson 10:c4745ddaaf6a 131 spi.write(reg | (0x80));
krobertson 10:c4745ddaaf6a 132 value = spi.write(0x00);
krobertson 10:c4745ddaaf6a 133 cs = 1;
krobertson 10:c4745ddaaf6a 134
krobertson 10:c4745ddaaf6a 135 return value;
krobertson 10:c4745ddaaf6a 136 }
krobertson 10:c4745ddaaf6a 137
krobertson 10:c4745ddaaf6a 138 void Compass::writeReg(regAddr reg, int value){
krobertson 10:c4745ddaaf6a 139 // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M
krobertson 10:c4745ddaaf6a 140 if (_device == device_D || reg < CTRL_REG1_A){
krobertson 10:c4745ddaaf6a 141 writeMagReg(reg, value);
krobertson 10:c4745ddaaf6a 142 }
krobertson 10:c4745ddaaf6a 143 else{
krobertson 10:c4745ddaaf6a 144 writeAccReg(reg, value);
krobertson 10:c4745ddaaf6a 145 }
krobertson 10:c4745ddaaf6a 146 }
krobertson 10:c4745ddaaf6a 147
krobertson 10:c4745ddaaf6a 148 // Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC.
krobertson 10:c4745ddaaf6a 149 // To read those two registers, use readMagReg() instead.
krobertson 10:c4745ddaaf6a 150 int Compass::readReg(regAddr reg){
krobertson 10:c4745ddaaf6a 151 // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M
krobertson 10:c4745ddaaf6a 152 if (_device == device_D || reg < CTRL_REG1_A){
krobertson 10:c4745ddaaf6a 153 return readMagReg(reg);
krobertson 10:c4745ddaaf6a 154 }
krobertson 10:c4745ddaaf6a 155 else{
krobertson 10:c4745ddaaf6a 156 return readAccReg(reg);
krobertson 10:c4745ddaaf6a 157 }
krobertson 10:c4745ddaaf6a 158 }
krobertson 10:c4745ddaaf6a 159
krobertson 10:c4745ddaaf6a 160 // Reads the 3 accelerometer channels and stores them in vector a
krobertson 10:c4745ddaaf6a 161 void Compass::readAcc(void){
krobertson 10:c4745ddaaf6a 162 char reg = OUT_X_L_A | (3 << 6);
krobertson 10:c4745ddaaf6a 163 char valuesAcc[6];
krobertson 10:c4745ddaaf6a 164 cs = 0;
krobertson 10:c4745ddaaf6a 165 spi.write(reg);
krobertson 10:c4745ddaaf6a 166 for(int i=0;i<6;i++){
krobertson 10:c4745ddaaf6a 167 valuesAcc[i] = spi.write(0x00);
krobertson 10:c4745ddaaf6a 168 }
krobertson 10:c4745ddaaf6a 169 cs = 1;
krobertson 10:c4745ddaaf6a 170
krobertson 10:c4745ddaaf6a 171 // combine high and low bytes
krobertson 10:c4745ddaaf6a 172 // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0
krobertson 10:c4745ddaaf6a 173 // (12-bit resolution, left-aligned). The D has 16-bit resolution
krobertson 10:c4745ddaaf6a 174 a.x = (int16_t)(valuesAcc[1] << 8 | valuesAcc[0]);
krobertson 10:c4745ddaaf6a 175 a.y = (int16_t)(valuesAcc[3] << 8 | valuesAcc[2]);
krobertson 10:c4745ddaaf6a 176 a.z = (int16_t)(valuesAcc[5] << 8 | valuesAcc[4]);
krobertson 10:c4745ddaaf6a 177 }
krobertson 10:c4745ddaaf6a 178
krobertson 10:c4745ddaaf6a 179 // Reads the 3 magnetometer channels and stores them in vector m
krobertson 10:c4745ddaaf6a 180 void Compass::readMag(void){
krobertson 10:c4745ddaaf6a 181 int reg;
krobertson 10:c4745ddaaf6a 182 int values[6];
krobertson 10:c4745ddaaf6a 183
krobertson 10:c4745ddaaf6a 184 reg = 0xC8;
krobertson 10:c4745ddaaf6a 185 cs = 0;
krobertson 10:c4745ddaaf6a 186 spi.write(reg);
krobertson 10:c4745ddaaf6a 187 for(int i=0;i<6;i++){
krobertson 10:c4745ddaaf6a 188 values[i] = spi.write(0x00);
krobertson 10:c4745ddaaf6a 189 }
krobertson 10:c4745ddaaf6a 190 cs = 1;
krobertson 10:c4745ddaaf6a 191
krobertson 10:c4745ddaaf6a 192 char xlm, xhm, ylm, yhm, zlm, zhm;
krobertson 10:c4745ddaaf6a 193
krobertson 10:c4745ddaaf6a 194 //if (_device == device_D){
krobertson 10:c4745ddaaf6a 195 /// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H
krobertson 10:c4745ddaaf6a 196 xlm = values[0];
krobertson 10:c4745ddaaf6a 197 xhm = values[1];
krobertson 10:c4745ddaaf6a 198 ylm = values[2];
krobertson 10:c4745ddaaf6a 199 yhm = values[3];
krobertson 10:c4745ddaaf6a 200 zlm = values[4];
krobertson 10:c4745ddaaf6a 201 zhm = values[5];
krobertson 10:c4745ddaaf6a 202 // }
krobertson 10:c4745ddaaf6a 203 // combine high and low bytes
krobertson 10:c4745ddaaf6a 204 m.x = (int16_t)(xhm << 8 | xlm);
krobertson 10:c4745ddaaf6a 205 m.y = (int16_t)(yhm << 8 | ylm);
krobertson 10:c4745ddaaf6a 206 m.z = (int16_t)(zhm << 8 | zlm);
krobertson 10:c4745ddaaf6a 207 }
krobertson 10:c4745ddaaf6a 208
krobertson 10:c4745ddaaf6a 209 // Reads all 6 channels of the LSM303 and stores them in the object variables
krobertson 10:c4745ddaaf6a 210 void Compass::read(void){
krobertson 10:c4745ddaaf6a 211 readAcc();
krobertson 10:c4745ddaaf6a 212 readMag();
krobertson 10:c4745ddaaf6a 213 }
krobertson 10:c4745ddaaf6a 214
krobertson 10:c4745ddaaf6a 215 /*
krobertson 10:c4745ddaaf6a 216 Returns the angular difference in the horizontal plane between a
krobertson 10:c4745ddaaf6a 217 default vector and north, in degrees.
krobertson 10:c4745ddaaf6a 218
krobertson 10:c4745ddaaf6a 219 The default vector here is chosen to point along the surface of the
krobertson 10:c4745ddaaf6a 220 PCB, in the direction of the top of the text on the silkscreen.
krobertson 10:c4745ddaaf6a 221 This is the +X axis on the Pololu LSM303D carrier and the -Y axis on
krobertson 10:c4745ddaaf6a 222 the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers.
krobertson 10:c4745ddaaf6a 223 */
krobertson 10:c4745ddaaf6a 224 float Compass::get_heading(void)
krobertson 10:c4745ddaaf6a 225 {
krobertson 10:c4745ddaaf6a 226 if (_device == device_D){
krobertson 10:c4745ddaaf6a 227 vector<int> params = {1,0,0};
krobertson 10:c4745ddaaf6a 228 return get_heading(params);
krobertson 10:c4745ddaaf6a 229 }else{
krobertson 10:c4745ddaaf6a 230 return get_heading((vector<int>){0, -1, 0});
krobertson 10:c4745ddaaf6a 231 }
krobertson 10:c4745ddaaf6a 232 }
krobertson 10:c4745ddaaf6a 233
krobertson 10:c4745ddaaf6a 234 /*
krobertson 10:c4745ddaaf6a 235 Returns the angular difference in the horizontal plane between the
krobertson 10:c4745ddaaf6a 236 "from" vector and north, in degrees.
krobertson 10:c4745ddaaf6a 237
krobertson 10:c4745ddaaf6a 238 Description of heading algorithm:
krobertson 10:c4745ddaaf6a 239 Shift and scale the magnetic reading based on calibration data to find
krobertson 10:c4745ddaaf6a 240 the North vector. Use the acceleration readings to determine the Up
krobertson 10:c4745ddaaf6a 241 vector (gravity is measured as an upward acceleration). The cross
krobertson 10:c4745ddaaf6a 242 product of North and Up vectors is East. The vectors East and North
krobertson 10:c4745ddaaf6a 243 form a basis for the horizontal plane. The From vector is projected
krobertson 10:c4745ddaaf6a 244 into the horizontal plane and the angle between the projected vector
krobertson 10:c4745ddaaf6a 245 and horizontal north is returned.
krobertson 10:c4745ddaaf6a 246 */
krobertson 10:c4745ddaaf6a 247 template <typename T> float Compass::get_heading(vector<T> from){
krobertson 10:c4745ddaaf6a 248 vector<int32_t> temp_m = {m.x, m.y, m.z};
krobertson 10:c4745ddaaf6a 249
krobertson 10:c4745ddaaf6a 250 // subtract offset (average of min and max) from magnetometer readings
krobertson 10:c4745ddaaf6a 251 temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2;
krobertson 10:c4745ddaaf6a 252 temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2;
krobertson 10:c4745ddaaf6a 253 temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2;
krobertson 10:c4745ddaaf6a 254
krobertson 10:c4745ddaaf6a 255 // compute E and N
krobertson 10:c4745ddaaf6a 256 vector<float> E;
krobertson 10:c4745ddaaf6a 257 vector<float> N;
krobertson 10:c4745ddaaf6a 258 vector_cross(&temp_m, &a, &E);
krobertson 10:c4745ddaaf6a 259 vector_normalize(&E);
krobertson 10:c4745ddaaf6a 260 vector_cross(&a, &E, &N);
krobertson 10:c4745ddaaf6a 261 vector_normalize(&N);
krobertson 10:c4745ddaaf6a 262
krobertson 10:c4745ddaaf6a 263 // compute heading
krobertson 10:c4745ddaaf6a 264 float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / 3.14159265359;
krobertson 10:c4745ddaaf6a 265 if (heading < 0) heading += 360;
krobertson 10:c4745ddaaf6a 266 return heading;
krobertson 10:c4745ddaaf6a 267 }
krobertson 10:c4745ddaaf6a 268
krobertson 10:c4745ddaaf6a 269 template <typename Ta, typename Tb, typename To> void Compass::vector_cross(const vector<Ta> *a,const vector<Tb> *b, vector<To> *out){
krobertson 10:c4745ddaaf6a 270 out->x = (a->y * b->z) - (a->z * b->y);
krobertson 10:c4745ddaaf6a 271 out->y = (a->z * b->x) - (a->x * b->z);
krobertson 10:c4745ddaaf6a 272 out->z = (a->x * b->y) - (a->y * b->x);
krobertson 10:c4745ddaaf6a 273 }
krobertson 10:c4745ddaaf6a 274
krobertson 10:c4745ddaaf6a 275 template <typename Ta, typename Tb> float Compass::vector_dot(const vector<Ta> *a, const vector<Tb> *b){
krobertson 10:c4745ddaaf6a 276 return (a->x * b->x) + (a->y * b->y) + (a->z * b->z);
krobertson 10:c4745ddaaf6a 277 }
krobertson 10:c4745ddaaf6a 278
krobertson 10:c4745ddaaf6a 279 void Compass::vector_normalize(vector<float> *a){
krobertson 10:c4745ddaaf6a 280 float mag = sqrt(vector_dot(a, a));
krobertson 10:c4745ddaaf6a 281 a->x /= mag;
krobertson 10:c4745ddaaf6a 282 a->y /= mag;
krobertson 10:c4745ddaaf6a 283 a->z /= mag;
krobertson 10:c4745ddaaf6a 284 }
krobertson 10:c4745ddaaf6a 285
krobertson 10:c4745ddaaf6a 286 // Private Methods //////////////////////////////////////////////////////////////
krobertson 10:c4745ddaaf6a 287
krobertson 10:c4745ddaaf6a 288 int Compass::testReg(regAddr reg){
krobertson 10:c4745ddaaf6a 289 // Select the device by seting chip select low
krobertson 10:c4745ddaaf6a 290 cs = 0;
krobertson 10:c4745ddaaf6a 291 // Send 0x8f, the command to read the WHOAMI register
krobertson 10:c4745ddaaf6a 292 spi.write(reg | 0x80);
krobertson 10:c4745ddaaf6a 293 // Send a dummy byte to receive the contents of the WHOAMI register
krobertson 10:c4745ddaaf6a 294 int whoami = spi.write(0x00);
krobertson 10:c4745ddaaf6a 295 // Deselect the device
krobertson 10:c4745ddaaf6a 296 cs = 1;
krobertson 10:c4745ddaaf6a 297 return whoami;
krobertson 10:c4745ddaaf6a 298 }