Colin Stearns
/
qcControl
QC Control software
Fork of dgps by
Diff: adapt/compass.cpp
- Revision:
- 17:323fc40376d5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/adapt/compass.cpp Thu Apr 03 17:18:58 2014 +0000 @@ -0,0 +1,303 @@ +#include <compass.h> +#include <math.h> + +SPI spi(p5, p6, p7); // mosi, miso, sclk +DigitalOut cs(p8); + +// Defines //////////////////////////////////////////////////////////////// + +#define D_WHO_ID 0x49 +#define DLM_WHO_ID 0x3C + +// Constructors //////////////////////////////////////////////////////////////// + +Compass::Compass(void){ + //These values lead to an assumed magnetometer bias of 0. + m_min = (Compass::vector<int16_t>){-32767, -32767, -32767}; + m_max = (Compass::vector<int16_t>){+32767, +32767, +32767}; + //automatically determine which device is being used + _device = device_auto; + //initialize chip select to high (not active) + cs=1; +} + +bool Compass::init(deviceType device){ + // Setup the spi for 8 bit data, low steady state clock, + // rising edge capture, with a 1MHz clock rate + spi.format(8,0); + spi.frequency(1000000); + + // determine device type if necessary + if (device == device_auto){ + if (testReg(WHO_AM_I) == D_WHO_ID){ + // device responds with D ID; it's a D. + device = device_D; + device_id = 1; + }else{ + // device hasn't responded meaningfully, so give up + return false; + } + } + + _device = device; + + // set device addresses and translated register addresses + switch (device){ + case device_D: + translated_regs[-OUT_X_L_M] = D_OUT_X_L_M; + translated_regs[-OUT_X_H_M] = D_OUT_X_H_M; + translated_regs[-OUT_Y_L_M] = D_OUT_Y_L_M; + translated_regs[-OUT_Y_H_M] = D_OUT_Y_H_M; + translated_regs[-OUT_Z_L_M] = D_OUT_Z_L_M; + translated_regs[-OUT_Z_H_M] = D_OUT_Z_H_M; + break; + } + return true; +} + +/* +Enables the Compass's accelerometer and magnetometer. Also: +- Sets sensor full scales (gain) to default power-on values, which are + +/- 2 g for accelerometer and +/- 1.3 gauss for magnetometer + (+/- 4 gauss on LSM303D). +- Selects 50 Hz ODR (output data rate) for accelerometer and 7.5 Hz + ODR for magnetometer (6.25 Hz on LSM303D). (These are the ODR + settings for which the electrical characteristics are specified in + the datasheets.) +- Enables high resolution modes (if available). +Note that this function will also reset other settings controlled by +the registers it writes to. +*/ +void Compass::enableDefault(void){ + if (_device == device_D){ + // Accelerometer + writeReg(CTRL0, 0x40); + writeReg(CTRL2, 0x00); + + // 0x57 = 0b01010111 + // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) + writeReg(CTRL1, 0xA7); + + // Magnetometer + + // 0x64 = 0b01100100 + // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) + writeReg(CTRL5, 0x74); + + // 0x20 = 0b00100000 + // MFS = 01 (+/- 4 gauss full scale) + writeReg(CTRL6, 0x20); + + // 0x00 = 0b00000000 + // MLP = 0 (low power mode off); MD = 00 (continuous-conversion mode) + writeReg(CTRL7, 0x00); + } +} + +// Writes an accelerometer register +void Compass::writeAccReg(regAddr reg, int value){ + cs = 0; + spi.write(reg); + spi.write(value); + cs = 1; +} + +// Reads an accelerometer register +int Compass::readAccReg(regAddr reg){ + int value; + cs = 0; + spi.write(reg | 0x80); + value = spi.write(0x00); + cs = 1; + return value; +} + +// Writes a magnetometer register +void Compass::writeMagReg(regAddr reg, int value){ + cs = 0; + spi.write(reg); + spi.write(value); + cs = 1; +} + +// Reads a magnetometer register +int Compass::readMagReg(regAddr reg){ + int value; + + // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type) + if (reg < 0){ + reg = translated_regs[-reg]; + } + cs = 0; + spi.write(reg | (0x80)); + value = spi.write(0x00); + cs = 1; + + return value; +} + +void Compass::writeReg(regAddr reg, int value){ + // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M + if (_device == device_D || reg < CTRL_REG1_A){ + writeMagReg(reg, value); + } + else{ + writeAccReg(reg, value); + } +} + +// Note that this function will not work for reading TEMP_OUT_H_M and TEMP_OUT_L_M on the DLHC. +// To read those two registers, use readMagReg() instead. +int Compass::readReg(regAddr reg){ + // Use writeMagReg so it can translate OUT_[XYZ]_[HL]_M + if (_device == device_D || reg < CTRL_REG1_A){ + return readMagReg(reg); + } + else{ + return readAccReg(reg); + } +} + +// Reads the 3 accelerometer channels and stores them in vector a +void Compass::readAcc(void){ + char reg = OUT_X_L_A | (3 << 6); + char valuesAcc[6]; + cs = 0; + spi.write(reg); + for(int i=0;i<6;i++){ + valuesAcc[i] = spi.write(0x00); + } + cs = 1; + + // combine high and low bytes + // This no longer drops the lowest 4 bits of the readings from the DLH/DLM/DLHC, which are always 0 + // (12-bit resolution, left-aligned). The D has 16-bit resolution + a.x = (int16_t)(valuesAcc[1] << 8 | valuesAcc[0]); + a.y = (int16_t)(valuesAcc[3] << 8 | valuesAcc[2]); + a.z = (int16_t)(valuesAcc[5] << 8 | valuesAcc[4]); +} + +// Reads the 3 magnetometer channels and stores them in vector m +void Compass::readMag(void){ + int reg; + int values[6]; + + reg = 0xC8; + + device_id = readReg(CTRL5); + wait_ms(50); + + cs = 0; + spi.write(reg); + for(int i=0;i<6;i++){ + values[i] = spi.write(0x00); + } + cs = 1; + + char xlm, xhm, ylm, yhm, zlm, zhm; + + //if (_device == device_D){ + /// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H + xlm = values[0]; + xhm = values[1]; + ylm = values[2]; + yhm = values[3]; + zlm = values[4]; + zhm = values[5]; + // } + // combine high and low bytes + m.x = (int16_t)(xhm << 8 | xlm); + m.y = (int16_t)(yhm << 8 | ylm); + m.z = (int16_t)(zhm << 8 | zlm); +} + +// Reads all 6 channels of the LSM303 and stores them in the object variables +void Compass::read(void){ + readAcc(); + readMag(); +} + +/* +Returns the angular difference in the horizontal plane between a +default vector and north, in degrees. + +The default vector here is chosen to point along the surface of the +PCB, in the direction of the top of the text on the silkscreen. +This is the +X axis on the Pololu LSM303D carrier and the -Y axis on +the Pololu LSM303DLHC, LSM303DLM, and LSM303DLH carriers. +*/ +float Compass::get_heading(void) +{ + if (_device == device_D){ + vector<int> params = {1,0,0}; + return get_heading(params); + }else{ + return get_heading((vector<int>){0, -1, 0}); + } +} + +/* +Returns the angular difference in the horizontal plane between the +"from" vector and north, in degrees. + +Description of heading algorithm: +Shift and scale the magnetic reading based on calibration data to find +the North vector. Use the acceleration readings to determine the Up +vector (gravity is measured as an upward acceleration). The cross +product of North and Up vectors is East. The vectors East and North +form a basis for the horizontal plane. The From vector is projected +into the horizontal plane and the angle between the projected vector +and horizontal north is returned. +*/ +template <typename T> float Compass::get_heading(vector<T> from){ + vector<int32_t> temp_m = {m.x, m.y, m.z}; + + // subtract offset (average of min and max) from magnetometer readings + temp_m.x -= ((int32_t)m_min.x + m_max.x) / 2; + temp_m.y -= ((int32_t)m_min.y + m_max.y) / 2; + temp_m.z -= ((int32_t)m_min.z + m_max.z) / 2; + + // compute E and N + vector<float> E; + vector<float> N; + vector_cross(&temp_m, &a, &E); + vector_normalize(&E); + vector_cross(&a, &E, &N); + vector_normalize(&N); + + // compute heading + float heading = atan2(vector_dot(&E, &from), vector_dot(&N, &from)) * 180 / 3.14159265359; + if (heading < 0) heading += 360; + return heading; +} + +template <typename Ta, typename Tb, typename To> void Compass::vector_cross(const vector<Ta> *a,const vector<Tb> *b, vector<To> *out){ + out->x = (a->y * b->z) - (a->z * b->y); + out->y = (a->z * b->x) - (a->x * b->z); + out->z = (a->x * b->y) - (a->y * b->x); +} + +template <typename Ta, typename Tb> float Compass::vector_dot(const vector<Ta> *a, const vector<Tb> *b){ + return (a->x * b->x) + (a->y * b->y) + (a->z * b->z); +} + +void Compass::vector_normalize(vector<float> *a){ + float mag = sqrt(vector_dot(a, a)); + a->x /= mag; + a->y /= mag; + a->z /= mag; +} + +// Private Methods ////////////////////////////////////////////////////////////// + +int Compass::testReg(regAddr reg){ + // Select the device by seting chip select low + cs = 0; + // Send 0x8f, the command to read the WHOAMI register + spi.write(reg | 0x80); + // Send a dummy byte to receive the contents of the WHOAMI register + int whoami = spi.write(0x00); + // Deselect the device + cs = 1; + return whoami; +} \ No newline at end of file