From d7f6cef6e131efbea8c17c95f0761d5f2336b9ad Mon Sep 17 00:00:00 2001 From: Anthony Good Date: Tue, 2 Dec 2014 23:04:48 -0500 Subject: [PATCH] new file: LSM303.cpp new file: LSM303.h --- LSM303.cpp | 570 +++++++++++++++++++++++++++++++++++++++++++++++++++++ LSM303.h | 218 ++++++++++++++++++++ 2 files changed, 788 insertions(+) create mode 100644 LSM303.cpp create mode 100644 LSM303.h diff --git a/LSM303.cpp b/LSM303.cpp new file mode 100644 index 0000000..3333b1f --- /dev/null +++ b/LSM303.cpp @@ -0,0 +1,570 @@ +#include +#include +#include + +// Defines //////////////////////////////////////////////////////////////// + +// The Arduino two-wire interface uses a 7-bit number for the address, +// and sets the last bit correctly based on reads and writes +#define D_SA0_HIGH_ADDRESS 0b0011101 // D with SA0 high +#define D_SA0_LOW_ADDRESS 0b0011110 // D with SA0 low or non-D magnetometer +#define NON_D_MAG_ADDRESS 0b0011110 // D with SA0 low or non-D magnetometer +#define NON_D_ACC_SA0_LOW_ADDRESS 0b0011000 // non-D accelerometer with SA0 low +#define NON_D_ACC_SA0_HIGH_ADDRESS 0b0011001 // non-D accelerometer with SA0 high + +#define TEST_REG_NACK -1 + +#define D_WHO_ID 0x49 +#define DLM_WHO_ID 0x3C + +// Constructors //////////////////////////////////////////////////////////////// + +LSM303::LSM303(void) +{ + /* + These values lead to an assumed magnetometer bias of 0. + Use the Calibrate example program to determine appropriate values + for your particular unit. The Heading example demonstrates how to + adjust these values in your own sketch. + */ + m_min = (LSM303::vector){-32767, -32767, -32767}; + m_max = (LSM303::vector){+32767, +32767, +32767}; + + _device = device_auto; + + io_timeout = 0; // 0 = no timeout + did_timeout = false; +} + +// Public Methods ////////////////////////////////////////////////////////////// + +// Did a timeout occur in readAcc(), readMag(), or read() since the last call to timeoutOccurred()? +bool LSM303::timeoutOccurred() +{ + bool tmp = did_timeout; + did_timeout = false; + return tmp; + +} + +void LSM303::setTimeout(unsigned int timeout) +{ + io_timeout = timeout; +} + +unsigned int LSM303::getTimeout() +{ + return io_timeout; +} + +bool LSM303::init(deviceType device, sa0State sa0) +{ + // determine device type if necessary + if (device == device_auto) + { + if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID) + { + // device responds to address 0011101 with D ID; it's a D with SA0 high + device = device_D; + sa0 = sa0_high; + } + else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID) + { + // device responds to address 0011110 with D ID; it's a D with SA0 low + device = device_D; + sa0 = sa0_low; + } + // Remaining possibilities: DLHC, DLM, or DLH. DLHC seems to respond to WHO_AM_I request the + // same way as DLM, even though this register isn't documented in its datasheet, so instead, + // guess if it's a DLHC based on acc address (Pololu boards pull SA0 low on DLM and DLH; + // DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high). + else if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK) + { + // device responds to address 0011001; guess that it's a DLHC + device = device_DLHC; + sa0 = sa0_high; + } + // Remaining possibilities: DLM or DLH. Check acc with SA0 low address to make sure it's responsive + else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK) + { + // device responds to address 0011000 with DLM ID; guess that it's a DLM + sa0 = sa0_low; + + // Now check WHO_AM_I_M + if (testReg(NON_D_MAG_ADDRESS, WHO_AM_I_M) == DLM_WHO_ID) + { + device = device_DLM; + } + else + { + device = device_DLH; + } + } + else + { + // device hasn't responded meaningfully, so give up + return false; + } + } + + // determine SA0 if necessary + if (sa0 == sa0_auto) + { + if (device == device_D) + { + if (testReg(D_SA0_HIGH_ADDRESS, WHO_AM_I) == D_WHO_ID) + { + sa0 = sa0_high; + } + else if (testReg(D_SA0_LOW_ADDRESS, WHO_AM_I) == D_WHO_ID) + { + sa0 = sa0_low; + } + else + { + // no response on either possible address; give up + return false; + } + } + else if (device == device_DLM || device == device_DLH) + { + if (testReg(NON_D_ACC_SA0_HIGH_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK) + { + sa0 = sa0_high; + } + else if (testReg(NON_D_ACC_SA0_LOW_ADDRESS, CTRL_REG1_A) != TEST_REG_NACK) + { + sa0 = sa0_low; + } + else + { + // no response on either possible address; give up + return false; + } + } + } + + _device = device; + + // set device addresses and translated register addresses + switch (device) + { + case device_D: + acc_address = mag_address = (sa0 == sa0_high) ? D_SA0_HIGH_ADDRESS : D_SA0_LOW_ADDRESS; + 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; + + case device_DLHC: + acc_address = NON_D_ACC_SA0_HIGH_ADDRESS; // DLHC doesn't have SA0 but uses same acc address as DLH/DLM with SA0 high + mag_address = NON_D_MAG_ADDRESS; + translated_regs[-OUT_X_H_M] = DLHC_OUT_X_H_M; + translated_regs[-OUT_X_L_M] = DLHC_OUT_X_L_M; + translated_regs[-OUT_Y_H_M] = DLHC_OUT_Y_H_M; + translated_regs[-OUT_Y_L_M] = DLHC_OUT_Y_L_M; + translated_regs[-OUT_Z_H_M] = DLHC_OUT_Z_H_M; + translated_regs[-OUT_Z_L_M] = DLHC_OUT_Z_L_M; + break; + + case device_DLM: + acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS; + mag_address = NON_D_MAG_ADDRESS; + translated_regs[-OUT_X_H_M] = DLM_OUT_X_H_M; + translated_regs[-OUT_X_L_M] = DLM_OUT_X_L_M; + translated_regs[-OUT_Y_H_M] = DLM_OUT_Y_H_M; + translated_regs[-OUT_Y_L_M] = DLM_OUT_Y_L_M; + translated_regs[-OUT_Z_H_M] = DLM_OUT_Z_H_M; + translated_regs[-OUT_Z_L_M] = DLM_OUT_Z_L_M; + break; + + case device_DLH: + acc_address = (sa0 == sa0_high) ? NON_D_ACC_SA0_HIGH_ADDRESS : NON_D_ACC_SA0_LOW_ADDRESS; + mag_address = NON_D_MAG_ADDRESS; + translated_regs[-OUT_X_H_M] = DLH_OUT_X_H_M; + translated_regs[-OUT_X_L_M] = DLH_OUT_X_L_M; + translated_regs[-OUT_Y_H_M] = DLH_OUT_Y_H_M; + translated_regs[-OUT_Y_L_M] = DLH_OUT_Y_L_M; + translated_regs[-OUT_Z_H_M] = DLH_OUT_Z_H_M; + translated_regs[-OUT_Z_L_M] = DLH_OUT_Z_L_M; + break; + } + return true; +} + +/* +Enables the LSM303'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 LSM303::enableDefault(void) +{ + + if (_device == device_D) + { + // Accelerometer + + // 0x57 = 0b01010111 + // AFS = 0 (+/- 2 g full scale) + writeReg(CTRL2, 0x00); + + // 0x57 = 0b01010111 + // AODR = 0101 (50 Hz ODR); AZEN = AYEN = AXEN = 1 (all axes enabled) + writeReg(CTRL1, 0x57); + + // Magnetometer + + // 0x64 = 0b01100100 + // M_RES = 11 (high resolution mode); M_ODR = 001 (6.25 Hz ODR) + writeReg(CTRL5, 0x64); + + // 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); + } + else if (_device == device_DLHC) + { + // Accelerometer + + // 0x08 = 0b00001000 + // FS = 00 (+/- 2 g full scale); HR = 1 (high resolution enable) + writeAccReg(CTRL_REG4_A, 0x08); + + // 0x47 = 0b01000111 + // ODR = 0100 (50 Hz ODR); LPen = 0 (normal mode); Zen = Yen = Xen = 1 (all axes enabled) + writeAccReg(CTRL_REG1_A, 0x47); + + // Magnetometer + + // 0x0C = 0b00001100 + // DO = 011 (7.5 Hz ODR) + writeMagReg(CRA_REG_M, 0x0C); + + // 0x20 = 0b00100000 + // GN = 001 (+/- 1.3 gauss full scale) + writeMagReg(CRB_REG_M, 0x20); + + // 0x00 = 0b00000000 + // MD = 00 (continuous-conversion mode) + writeMagReg(MR_REG_M, 0x00); + } + else // DLM, DLH + { + // Accelerometer + + // 0x00 = 0b00000000 + // FS = 00 (+/- 2 g full scale) + writeAccReg(CTRL_REG4_A, 0x00); + + // 0x27 = 0b00100111 + // PM = 001 (normal mode); DR = 00 (50 Hz ODR); Zen = Yen = Xen = 1 (all axes enabled) + writeAccReg(CTRL_REG1_A, 0x27); + + // Magnetometer + + // 0x0C = 0b00001100 + // DO = 011 (7.5 Hz ODR) + writeMagReg(CRA_REG_M, 0x0C); + + // 0x20 = 0b00100000 + // GN = 001 (+/- 1.3 gauss full scale) + writeMagReg(CRB_REG_M, 0x20); + + // 0x00 = 0b00000000 + // MD = 00 (continuous-conversion mode) + writeMagReg(MR_REG_M, 0x00); + } +} + +// Writes an accelerometer register +void LSM303::writeAccReg(regAddr reg, byte value) +{ + Wire.beginTransmission(acc_address); + Wire.write((byte)reg); + Wire.write(value); + last_status = Wire.endTransmission(); +} + +// Reads an accelerometer register +byte LSM303::readAccReg(regAddr reg) +{ + byte value; + + Wire.beginTransmission(acc_address); + Wire.write((byte)reg); + last_status = Wire.endTransmission(); + Wire.requestFrom(acc_address, (byte)1); + value = Wire.read(); + Wire.endTransmission(); + + return value; +} + +// Writes a magnetometer register +void LSM303::writeMagReg(regAddr reg, byte value) +{ + Wire.beginTransmission(mag_address); + Wire.write((byte)reg); + Wire.write(value); + last_status = Wire.endTransmission(); +} + +// Reads a magnetometer register +byte LSM303::readMagReg(regAddr reg) +{ + byte value; + + // if dummy register address (magnetometer Y/Z), look up actual translated address (based on device type) + if (reg < 0) + { + reg = translated_regs[-reg]; + } + + Wire.beginTransmission(mag_address); + Wire.write((byte)reg); + last_status = Wire.endTransmission(); + Wire.requestFrom(mag_address, (byte)1); + value = Wire.read(); + Wire.endTransmission(); + + return value; +} + +void LSM303::writeReg(regAddr reg, byte value) +{ + // mag address == acc_address for LSM303D, so it doesn't really matter which one we use. + // 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. +byte LSM303::readReg(regAddr reg) +{ + // mag address == acc_address for LSM303D, so it doesn't really matter which one we use. + // 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 LSM303::readAcc(void) +{ + Wire.beginTransmission(acc_address); + // assert the MSB of the address to get the accelerometer + // to do slave-transmit subaddress updating. + Wire.write(OUT_X_L_A | (1 << 7)); + last_status = Wire.endTransmission(); + Wire.requestFrom(acc_address, (byte)6); + + unsigned int millis_start = millis(); + while (Wire.available() < 6) { + if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) + { + did_timeout = true; + return; + } + } + + byte xla = Wire.read(); + byte xha = Wire.read(); + byte yla = Wire.read(); + byte yha = Wire.read(); + byte zla = Wire.read(); + byte zha = Wire.read(); + + // 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)(xha << 8 | xla); + a.y = (int16_t)(yha << 8 | yla); + a.z = (int16_t)(zha << 8 | zla); +} + +// Reads the 3 magnetometer channels and stores them in vector m +void LSM303::readMag(void) +{ + Wire.beginTransmission(mag_address); + // If LSM303D, assert MSB to enable subaddress updating + // OUT_X_L_M comes first on D, OUT_X_H_M on others + Wire.write((_device == device_D) ? translated_regs[-OUT_X_L_M] | (1 << 7) : translated_regs[-OUT_X_H_M]); + last_status = Wire.endTransmission(); + Wire.requestFrom(mag_address, (byte)6); + + unsigned int millis_start = millis(); + while (Wire.available() < 6) { + if (io_timeout > 0 && ((unsigned int)millis() - millis_start) > io_timeout) + { + did_timeout = true; + return; + } + } + + byte xlm, xhm, ylm, yhm, zlm, zhm; + + if (_device == device_D) + { + /// D: X_L, X_H, Y_L, Y_H, Z_L, Z_H + xlm = Wire.read(); + xhm = Wire.read(); + ylm = Wire.read(); + yhm = Wire.read(); + zlm = Wire.read(); + zhm = Wire.read(); + } + else + { + // DLHC, DLM, DLH: X_H, X_L... + xhm = Wire.read(); + xlm = Wire.read(); + + if (_device == device_DLH) + { + // DLH: ...Y_H, Y_L, Z_H, Z_L + yhm = Wire.read(); + ylm = Wire.read(); + zhm = Wire.read(); + zlm = Wire.read(); + } + else + { + // DLM, DLHC: ...Z_H, Z_L, Y_H, Y_L + zhm = Wire.read(); + zlm = Wire.read(); + yhm = Wire.read(); + ylm = Wire.read(); + } + } + + // 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 LSM303::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 LSM303::heading(void) +{ + if (_device == device_D) + { + return heading((vector){1, 0, 0}); + } + else + { + return heading((vector){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 float LSM303::heading(vector from) +{ + vector 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 E; + vector 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 / M_PI; + if (heading < 0) heading += 360; + return heading; +} + +template void LSM303::vector_cross(const vector *a,const vector *b, vector *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 float LSM303::vector_dot(const vector *a, const vector *b) +{ + return (a->x * b->x) + (a->y * b->y) + (a->z * b->z); +} + +void LSM303::vector_normalize(vector *a) +{ + float mag = sqrt(vector_dot(a, a)); + a->x /= mag; + a->y /= mag; + a->z /= mag; +} + +// Private Methods ////////////////////////////////////////////////////////////// + +int LSM303::testReg(byte address, regAddr reg) +{ + Wire.beginTransmission(address); + Wire.write((byte)reg); + last_status = Wire.endTransmission(); + + Wire.requestFrom(address, (byte)1); + if (Wire.available()) + return Wire.read(); + else + return TEST_REG_NACK; +} \ No newline at end of file diff --git a/LSM303.h b/LSM303.h new file mode 100644 index 0000000..c50a12f --- /dev/null +++ b/LSM303.h @@ -0,0 +1,218 @@ +#ifndef LSM303_h +#define LSM303_h + +#include // for byte data type + +class LSM303 +{ + public: + template struct vector + { + T x, y, z; + }; + + enum deviceType { device_DLH, device_DLM, device_DLHC, device_D, device_auto }; + enum sa0State { sa0_low, sa0_high, sa0_auto }; + + // register addresses + enum regAddr + { + TEMP_OUT_L = 0x05, // D + TEMP_OUT_H = 0x06, // D + + STATUS_M = 0x07, // D + + INT_CTRL_M = 0x12, // D + INT_SRC_M = 0x13, // D + INT_THS_L_M = 0x14, // D + INT_THS_H_M = 0x15, // D + + OFFSET_X_L_M = 0x16, // D + OFFSET_X_H_M = 0x17, // D + OFFSET_Y_L_M = 0x18, // D + OFFSET_Y_H_M = 0x19, // D + OFFSET_Z_L_M = 0x1A, // D + OFFSET_Z_H_M = 0x1B, // D + REFERENCE_X = 0x1C, // D + REFERENCE_Y = 0x1D, // D + REFERENCE_Z = 0x1E, // D + + CTRL0 = 0x1F, // D + CTRL1 = 0x20, // D + CTRL_REG1_A = 0x20, // DLH, DLM, DLHC + CTRL2 = 0x21, // D + CTRL_REG2_A = 0x21, // DLH, DLM, DLHC + CTRL3 = 0x22, // D + CTRL_REG3_A = 0x22, // DLH, DLM, DLHC + CTRL4 = 0x23, // D + CTRL_REG4_A = 0x23, // DLH, DLM, DLHC + CTRL5 = 0x24, // D + CTRL_REG5_A = 0x24, // DLH, DLM, DLHC + CTRL6 = 0x25, // D + CTRL_REG6_A = 0x25, // DLHC + HP_FILTER_RESET_A = 0x25, // DLH, DLM + CTRL7 = 0x26, // D + REFERENCE_A = 0x26, // DLH, DLM, DLHC + STATUS_A = 0x27, // D + STATUS_REG_A = 0x27, // DLH, DLM, DLHC + + OUT_X_L_A = 0x28, + OUT_X_H_A = 0x29, + OUT_Y_L_A = 0x2A, + OUT_Y_H_A = 0x2B, + OUT_Z_L_A = 0x2C, + OUT_Z_H_A = 0x2D, + + FIFO_CTRL = 0x2E, // D + FIFO_CTRL_REG_A = 0x2E, // DLHC + FIFO_SRC = 0x2F, // D + FIFO_SRC_REG_A = 0x2F, // DLHC + + IG_CFG1 = 0x30, // D + INT1_CFG_A = 0x30, // DLH, DLM, DLHC + IG_SRC1 = 0x31, // D + INT1_SRC_A = 0x31, // DLH, DLM, DLHC + IG_THS1 = 0x32, // D + INT1_THS_A = 0x32, // DLH, DLM, DLHC + IG_DUR1 = 0x33, // D + INT1_DURATION_A = 0x33, // DLH, DLM, DLHC + IG_CFG2 = 0x34, // D + INT2_CFG_A = 0x34, // DLH, DLM, DLHC + IG_SRC2 = 0x35, // D + INT2_SRC_A = 0x35, // DLH, DLM, DLHC + IG_THS2 = 0x36, // D + INT2_THS_A = 0x36, // DLH, DLM, DLHC + IG_DUR2 = 0x37, // D + INT2_DURATION_A = 0x37, // DLH, DLM, DLHC + + CLICK_CFG = 0x38, // D + CLICK_CFG_A = 0x38, // DLHC + CLICK_SRC = 0x39, // D + CLICK_SRC_A = 0x39, // DLHC + CLICK_THS = 0x3A, // D + CLICK_THS_A = 0x3A, // DLHC + TIME_LIMIT = 0x3B, // D + TIME_LIMIT_A = 0x3B, // DLHC + TIME_LATENCY = 0x3C, // D + TIME_LATENCY_A = 0x3C, // DLHC + TIME_WINDOW = 0x3D, // D + TIME_WINDOW_A = 0x3D, // DLHC + + Act_THS = 0x3E, // D + Act_DUR = 0x3F, // D + + CRA_REG_M = 0x00, // DLH, DLM, DLHC + CRB_REG_M = 0x01, // DLH, DLM, DLHC + MR_REG_M = 0x02, // DLH, DLM, DLHC + + SR_REG_M = 0x09, // DLH, DLM, DLHC + IRA_REG_M = 0x0A, // DLH, DLM, DLHC + IRB_REG_M = 0x0B, // DLH, DLM, DLHC + IRC_REG_M = 0x0C, // DLH, DLM, DLHC + + WHO_AM_I_M = 0x0F, // DLM + WHO_AM_I = 0x0F, // D + + TEMP_OUT_H_M = 0x31, // DLHC + TEMP_OUT_L_M = 0x32, // DLHC + + + // dummy addresses for registers in different locations on different devices; + // the library translates these based on device type + // value with sign flipped is used as index into translated_regs array + + OUT_X_H_M = -1, + OUT_X_L_M = -2, + OUT_Y_H_M = -3, + OUT_Y_L_M = -4, + OUT_Z_H_M = -5, + OUT_Z_L_M = -6, + // update dummy_reg_count if registers are added here! + + // device-specific register addresses + + DLH_OUT_X_H_M = 0x03, + DLH_OUT_X_L_M = 0x04, + DLH_OUT_Y_H_M = 0x05, + DLH_OUT_Y_L_M = 0x06, + DLH_OUT_Z_H_M = 0x07, + DLH_OUT_Z_L_M = 0x08, + + DLM_OUT_X_H_M = 0x03, + DLM_OUT_X_L_M = 0x04, + DLM_OUT_Z_H_M = 0x05, + DLM_OUT_Z_L_M = 0x06, + DLM_OUT_Y_H_M = 0x07, + DLM_OUT_Y_L_M = 0x08, + + DLHC_OUT_X_H_M = 0x03, + DLHC_OUT_X_L_M = 0x04, + DLHC_OUT_Z_H_M = 0x05, + DLHC_OUT_Z_L_M = 0x06, + DLHC_OUT_Y_H_M = 0x07, + DLHC_OUT_Y_L_M = 0x08, + + D_OUT_X_L_M = 0x08, + D_OUT_X_H_M = 0x09, + D_OUT_Y_L_M = 0x0A, + D_OUT_Y_H_M = 0x0B, + D_OUT_Z_L_M = 0x0C, + D_OUT_Z_H_M = 0x0D + }; + + vector a; // accelerometer readings + vector m; // magnetometer readings + vector m_max; // maximum magnetometer values, used for calibration + vector m_min; // minimum magnetometer values, used for calibration + + byte last_status; // status of last I2C transmission + + LSM303(void); + + bool init(deviceType device = device_auto, sa0State sa0 = sa0_auto); + byte getDeviceType(void) { return _device; } + + void enableDefault(void); + + void writeAccReg(regAddr reg, byte value); + byte readAccReg(regAddr reg); + void writeMagReg(regAddr reg, byte value); + byte readMagReg(regAddr reg); + + void writeReg(regAddr reg, byte value); + byte readReg(regAddr reg); + + void readAcc(void); + void readMag(void); + void read(void); + + void setTimeout(unsigned int timeout); + unsigned int getTimeout(void); + bool timeoutOccurred(void); + + float heading(void); + template float heading(vector from); + + // vector functions + template static void vector_cross(const vector *a, const vector *b, vector *out); + template static float vector_dot(const vector *a,const vector *b); + static void vector_normalize(vector *a); + + private: + deviceType _device; // chip type (DLH, DLM, or DLHC) + byte acc_address; + byte mag_address; + + static const int dummy_reg_count = 6; + regAddr translated_regs[dummy_reg_count + 1]; // index 0 not used + + unsigned int io_timeout; + bool did_timeout; + + int testReg(byte address, regAddr reg); +}; + +#endif + + +