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