MMA7455.cpp

00001 /******************************************************************************
00002  * Includes
00003  *****************************************************************************/
00004 
00005 #include "mbed.h"
00006 #include "mbed_debug.h"
00007 
00008 #include "MMA7455.h"
00009 
00010 /******************************************************************************
00011  * Defines and typedefs
00012  *****************************************************************************/
00013 
00014 #define MMA7455_I2C_ADDR (0x1D << 1)
00015 
00016 #define MMA7455_ADDR_XOUTL  0x00
00017 #define MMA7455_ADDR_XOUTH  0x01
00018 #define MMA7455_ADDR_YOUTL  0x02
00019 #define MMA7455_ADDR_YOUTH  0x03
00020 #define MMA7455_ADDR_ZOUTL  0x04
00021 #define MMA7455_ADDR_ZOUTH  0x05
00022 #define MMA7455_ADDR_XOUT8  0x06
00023 #define MMA7455_ADDR_YOUT8  0x07
00024 #define MMA7455_ADDR_ZOUT8  0x08
00025 #define MMA7455_ADDR_STATUS 0x09
00026 #define MMA7455_ADDR_DETSRC 0x0A
00027 #define MMA7455_ADDR_TOUT   0x0B
00028 #define MMA7455_ADDR_I2CAD  0x0D
00029 #define MMA7455_ADDR_USRINF 0x0E
00030 #define MMA7455_ADDR_WHOAMI 0x0F
00031 #define MMA7455_ADDR_XOFFL  0x10
00032 #define MMA7455_ADDR_XOFFH  0x11
00033 #define MMA7455_ADDR_YOFFL  0x12
00034 #define MMA7455_ADDR_YOFFH  0x13
00035 #define MMA7455_ADDR_ZOFFL  0x14
00036 #define MMA7455_ADDR_ZOFFH  0x15
00037 #define MMA7455_ADDR_MCTL   0x16
00038 #define MMA7455_ADDR_INTRST 0x17
00039 #define MMA7455_ADDR_CTL1   0x18
00040 #define MMA7455_ADDR_CTL2   0x19
00041 #define MMA7455_ADDR_LDTH   0x1A
00042 #define MMA7455_ADDR_PDTH   0x1B
00043 #define MMA7455_ADDR_PW     0x1C
00044 #define MMA7455_ADDR_LT     0x1D
00045 #define MMA7455_ADDR_TW     0x1E
00046 
00047 #define MMA7455_MCTL_MODE(m) ((m) << 0)
00048 #define MMA7455_MCTL_GLVL(g) ((g) << 2)
00049 
00050 #define MMA7455_STATUS_DRDY (1 << 0)
00051 #define MMA7455_STATUS_DOVR (1 << 1)
00052 #define MMA7455_STATUS_PERR (1 << 2)
00053 
00054 
00055 MMA7455::MMA7455(PinName sda, PinName scl) : _i2c(sda, scl)
00056 {
00057     _mode = ModeStandby;
00058     _range = Range_8g;
00059 
00060     _xOff = 0;
00061     _yOff = 0;
00062     _zOff = 0;
00063 }
00064 
00065 bool MMA7455::setMode(Mode mode) {
00066     bool result = false;
00067     int mCtrl = 0;
00068 
00069     do {
00070         mCtrl = getModeControl();
00071         if (mCtrl < 0) break;
00072 
00073         mCtrl &= ~(0x03 << 0);
00074         mCtrl |= MMA7455_MCTL_MODE(mode);
00075 
00076         if (setModeControl((uint8_t)mCtrl) < 0) {
00077             break;
00078         }
00079 
00080         _mode = mode;
00081         result = true;
00082     } while(0);
00083 
00084 
00085 
00086     return result;
00087 }
00088 
00089 bool MMA7455::setRange(Range range) {
00090     bool result = false;
00091     int mCtrl = 0;
00092 
00093     do {
00094         mCtrl = getModeControl();
00095         if (mCtrl < 0) break;
00096 
00097         mCtrl &= ~(0x03 << 2);
00098         mCtrl |= MMA7455_MCTL_GLVL(range);
00099 
00100         if (setModeControl((uint8_t)mCtrl) < 0) {
00101             break;
00102         }
00103 
00104         _range = range;
00105         result = true;
00106     } while(0);
00107 
00108 
00109 
00110     return result;
00111 
00112 }
00113 
00114 bool MMA7455::read(int32_t& x, int32_t& y, int32_t& z) {
00115     bool result = false;
00116 
00117 
00118     // nothing to read in standby mode
00119     if (_mode == ModeStandby) return false;
00120 
00121     // wait for ready flag
00122     int status = 0;
00123     do {
00124       status = getStatus();
00125     } while (status >= 0 && (status & MMA7455_STATUS_DRDY) == 0);
00126 
00127 
00128     do {
00129         if (status < 0) break;
00130 
00131 
00132         char buf[6];
00133         buf[0] = MMA7455_ADDR_XOUTL;
00134         if (_i2c.write(MMA7455_I2C_ADDR, buf, 1) != 0) break;
00135         if (_i2c.read(MMA7455_I2C_ADDR, buf, 6) != 0) break;
00136 
00137         // check if second bit is set in high byte -> negative value
00138         // expand negative value to full byte
00139         if (buf[1] & 0x02) buf[1] |= 0xFC;
00140         if (buf[3] & 0x02) buf[3] |= 0xFC;
00141         if (buf[5] & 0x02) buf[5] |= 0xFC;
00142 
00143         x = (int16_t)((buf[1] << 8) | buf[0]) + _xOff;
00144         y = (int16_t)((buf[3] << 8) | buf[2]) + _yOff;
00145         z = (int16_t)((buf[5] << 8) | buf[4]) + _zOff;
00146 
00147 
00148         result = true;
00149 
00150     } while(0);
00151 
00152 
00153     return result;
00154 }
00155 
00156 bool MMA7455::calibrate() {
00157     bool result = false;
00158     bool failed = false;
00159 
00160     int32_t x = 0;
00161     int32_t y = 0;
00162     int32_t z = 0;
00163 
00164     int32_t xr = 0;
00165     int32_t yr = 0;
00166     int32_t zr = 0;
00167 
00168     int xOff = 0;
00169     int yOff = 0;
00170     int zOff = 16;
00171     if (_range == Range_2g) {
00172         zOff = 64;
00173     }
00174     if (_range == Range_4g) {
00175         zOff = 32;
00176     }
00177 
00178     do {
00179 
00180         // get an average of 6 values
00181         for (int i = 0; i < 6; i++) {
00182             if (!read(xr, yr, zr)) {
00183                 failed = true;
00184                 break;
00185             }
00186             x += xr;
00187             y += yr;
00188             z += zr;
00189 
00190             wait_ms(100);
00191         }
00192 
00193         if (failed) break;
00194         x /= 6;
00195         y /= 6;
00196         z /= 6;
00197 
00198         xOff -= x;
00199         yOff -= y;
00200         zOff -= z;
00201 
00202         /*
00203          * For some reason we have not got correct/reliable calibration
00204          * by using the offset drift registers. Instead we are
00205          * calculating the offsets and store them in member variables.
00206          *
00207          * These member variables are then used in the read() method
00208          */
00209 
00210         _xOff = xOff;
00211         _yOff = yOff;
00212         _zOff = zOff;
00213 
00214 
00215         result = true;
00216 
00217     } while (0);
00218 
00219 
00220 
00221     return result;
00222 }
00223 
00224 bool MMA7455::setCalibrationOffsets(int32_t xOff, int32_t yOff, int32_t zOff) {
00225     _xOff = xOff;
00226     _yOff = yOff;
00227     _zOff = zOff;
00228 
00229     return true;
00230 }
00231 
00232 bool MMA7455::getCalibrationOffsets(int32_t& xOff, int32_t& yOff, int32_t& zOff) {
00233     xOff = _xOff;
00234     yOff = _yOff;
00235     zOff = _zOff;
00236 
00237     return true;
00238 }
00239 
00240 int MMA7455::getStatus() {
00241     int result = -1;
00242     char data[1];
00243 
00244     do {
00245         data[0] = MMA7455_ADDR_STATUS;
00246         if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break;
00247 
00248         if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break;
00249 
00250         result = data[0];
00251 
00252     } while (0);
00253 
00254 
00255 
00256     return result;
00257 }
00258 
00259 int MMA7455::getModeControl() {
00260 
00261     int result = -1;
00262     char data[1];
00263 
00264     do {
00265         data[0] = MMA7455_ADDR_MCTL;
00266         if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break;
00267 
00268         if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break;
00269 
00270         result = data[0];
00271 
00272     } while (0);
00273 
00274 
00275 
00276     return result;
00277 }
00278 
00279 int MMA7455::setModeControl(uint8_t mctl) {
00280     int result = -1;
00281     char data[2];
00282 
00283     do {
00284         data[0] = MMA7455_ADDR_MCTL;
00285         data[1] = (char)mctl;
00286         if (_i2c.write(MMA7455_I2C_ADDR, data, 2) != 0) break;
00287 
00288         result = 0;
00289 
00290     } while (0);
00291 
00292 
00293 
00294     return result;
00295 }