Colin Stearns / Mbed 2 deprecated qcControl

QC Control software

Dependencies:   mbed

Fork of dgps by Colin Stearns

adapt/compass.cpp@17:323fc40376d5, 2014-04-03 (annotated)

Committer:
krobertson
Date:
Thu Apr 03 17:18:58 2014 +0000
Revision:
17:323fc40376d5
added compass to project

Who changed what in which revision?

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