sensors
Dependencies: allsensors TEMDHT FastAnalogIn SDFileSystem
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 }
Generated on Fri Aug 19 2022 17:44:32 by 1.7.2