Small changes
Dependencies: FATFileSystem
Fork of EALib by
Diff: MMA7455.cpp
- Revision:
- 4:b32cf4ef45c5
- Child:
- 12:15597e45eea0
diff -r 9d31a3c5013e -r b32cf4ef45c5 MMA7455.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MMA7455.cpp Fri Oct 18 12:48:58 2013 +0200 @@ -0,0 +1,298 @@ + +/****************************************************************************** + * Includes + *****************************************************************************/ + +#include "mbed.h" +#include "mbed_debug.h" + +#include "MMA7455.h" + +/****************************************************************************** + * Defines and typedefs + *****************************************************************************/ + +#define MMA7455_I2C_ADDR (0x1D << 1) + +#define MMA7455_ADDR_XOUTL 0x00 +#define MMA7455_ADDR_XOUTH 0x01 +#define MMA7455_ADDR_YOUTL 0x02 +#define MMA7455_ADDR_YOUTH 0x03 +#define MMA7455_ADDR_ZOUTL 0x04 +#define MMA7455_ADDR_ZOUTH 0x05 +#define MMA7455_ADDR_XOUT8 0x06 +#define MMA7455_ADDR_YOUT8 0x07 +#define MMA7455_ADDR_ZOUT8 0x08 +#define MMA7455_ADDR_STATUS 0x09 +#define MMA7455_ADDR_DETSRC 0x0A +#define MMA7455_ADDR_TOUT 0x0B +#define MMA7455_ADDR_I2CAD 0x0D +#define MMA7455_ADDR_USRINF 0x0E +#define MMA7455_ADDR_WHOAMI 0x0F +#define MMA7455_ADDR_XOFFL 0x10 +#define MMA7455_ADDR_XOFFH 0x11 +#define MMA7455_ADDR_YOFFL 0x12 +#define MMA7455_ADDR_YOFFH 0x13 +#define MMA7455_ADDR_ZOFFL 0x14 +#define MMA7455_ADDR_ZOFFH 0x15 +#define MMA7455_ADDR_MCTL 0x16 +#define MMA7455_ADDR_INTRST 0x17 +#define MMA7455_ADDR_CTL1 0x18 +#define MMA7455_ADDR_CTL2 0x19 +#define MMA7455_ADDR_LDTH 0x1A +#define MMA7455_ADDR_PDTH 0x1B +#define MMA7455_ADDR_PW 0x1C +#define MMA7455_ADDR_LT 0x1D +#define MMA7455_ADDR_TW 0x1E + +#define MMA7455_MCTL_MODE(m) ((m) << 0) +#define MMA7455_MCTL_GLVL(g) ((g) << 2) + +#define MMA7455_STATUS_DRDY (1 << 0) +#define MMA7455_STATUS_DOVR (1 << 1) +#define MMA7455_STATUS_PERR (1 << 2) + + +MMA7455::MMA7455(PinName sda, PinName scl) : _i2c(sda, scl) +{ + _mode = ModeStandby; + _range = Range_8g; + + _xOff = 0; + _yOff = 0; + _zOff = 0; +} + +bool MMA7455::setMode(Mode mode) { + bool result = false; + int mCtrl = 0; + + do { + mCtrl = getModeControl(); + if (mCtrl < 0) break; + + mCtrl &= ~(0x03 << 0); + mCtrl |= MMA7455_MCTL_MODE(mode); + + if (setModeControl((uint8_t)mCtrl) < 0) { + break; + } + + _mode = mode; + result = true; + } while(0); + + + + return result; +} + +bool MMA7455::setRange(Range range) { + bool result = false; + int mCtrl = 0; + + do { + mCtrl = getModeControl(); + if (mCtrl < 0) break; + + mCtrl &= ~(0x03 << 2); + mCtrl |= MMA7455_MCTL_GLVL(range); + + if (setModeControl((uint8_t)mCtrl) < 0) { + break; + } + + _range = range; + result = true; + } while(0); + + + + return result; + +} + +bool MMA7455::read(int32_t& x, int32_t& y, int32_t& z) { + bool result = false; + + + // nothing to read in standby mode + if (_mode == ModeStandby) return false; + + // wait for ready flag + int status = 0; + do { + status = getStatus(); + } while (status >= 0 && (status & MMA7455_STATUS_DRDY) == 0); + + + do { + if (status < 0) break; + + + char buf[6]; + buf[0] = MMA7455_ADDR_XOUTL; + if (_i2c.write(MMA7455_I2C_ADDR, buf, 1) != 0) break; + if (_i2c.read(MMA7455_I2C_ADDR, buf, 6) != 0) break; + + // check if second bit is set in high byte -> negative value + // expand negative value to full byte + if (buf[1] & 0x02) buf[1] |= 0xFC; + if (buf[3] & 0x02) buf[3] |= 0xFC; + if (buf[5] & 0x02) buf[5] |= 0xFC; + + x = (int16_t)((buf[1] << 8) | buf[0]) + _xOff; + y = (int16_t)((buf[3] << 8) | buf[2]) + _yOff; + z = (int16_t)((buf[5] << 8) | buf[4]) + _zOff; + + + result = true; + + } while(0); + + + return result; +} + +bool MMA7455::calibrate() { + bool result = false; + bool failed = false; + + int32_t x = 0; + int32_t y = 0; + int32_t z = 0; + + int32_t xr = 0; + int32_t yr = 0; + int32_t zr = 0; + + int xOff = 0; + int yOff = 0; + int zOff = 16; + if (_range == Range_2g) { + zOff = 64; + } + if (_range == Range_4g) { + zOff = 32; + } + + do { + + // get an average of 6 values + for (int i = 0; i < 6; i++) { + if (!read(xr, yr, zr)) { + failed = true; + break; + } + x += xr; + y += yr; + z += zr; + + wait_ms(100); + } + + if (failed) break; + x /= 6; + y /= 6; + z /= 6; + + xOff -= x; + yOff -= y; + zOff -= z; + + /* + * For some reason we have not got correct/reliable calibration + * by using the offset drift registers. Instead we are + * calculating the offsets and store them in member variables. + * + * These member variables are then used in the read() method + */ + + _xOff = xOff; + _yOff = yOff; + _zOff = zOff; + + + result = true; + + } while (0); + + + + return result; +} + +bool MMA7455::setCalibrationOffsets(int32_t xOff, int32_t yOff, int32_t zOff) { + _xOff = xOff; + _yOff = yOff; + _zOff = zOff; + + return true; +} + +bool MMA7455::getCalibrationOffsets(int32_t& xOff, int32_t& yOff, int32_t& zOff) { + xOff = _xOff; + yOff = _yOff; + zOff = _zOff; + + return true; +} + +int MMA7455::getStatus() { + int result = -1; + char data[1]; + + do { + data[0] = MMA7455_ADDR_STATUS; + if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break; + + if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break; + + result = data[0]; + + } while (0); + + + + return result; +} + +int MMA7455::getModeControl() { + + int result = -1; + char data[1]; + + do { + data[0] = MMA7455_ADDR_MCTL; + if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break; + + if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break; + + result = data[0]; + + } while (0); + + + + return result; +} + +int MMA7455::setModeControl(uint8_t mctl) { + int result = -1; + char data[2]; + + do { + data[0] = MMA7455_ADDR_MCTL; + data[1] = (char)mctl; + if (_i2c.write(MMA7455_I2C_ADDR, data, 2) != 0) break; + + result = 0; + + } while (0); + + + + return result; +} + +