jajn HA / Mbed OS 3Gall

hello

Dependencies:   C027_SupportMMA7455

MMA7455.cpp@0:c6e1fe84dc9a, 2017-12-12 (annotated)

Committer:
david8251
Date:
Tue Dec 12 12:32:16 2017 +0000
Revision:
0:c6e1fe84dc9a
hello

Who changed what in which revision?

UserRevisionLine numberNew contents of line
david8251 0:c6e1fe84dc9a 1 /******************************************************************************
david8251 0:c6e1fe84dc9a 2 * Includes
david8251 0:c6e1fe84dc9a 3 *****************************************************************************/
david8251 0:c6e1fe84dc9a 4
david8251 0:c6e1fe84dc9a 5 #include "mbed.h"
david8251 0:c6e1fe84dc9a 6 #include "mbed_debug.h"
david8251 0:c6e1fe84dc9a 7
david8251 0:c6e1fe84dc9a 8 #include "MMA7455.h"
david8251 0:c6e1fe84dc9a 9
david8251 0:c6e1fe84dc9a 10 /******************************************************************************
david8251 0:c6e1fe84dc9a 11 * Defines and typedefs
david8251 0:c6e1fe84dc9a 12 *****************************************************************************/
david8251 0:c6e1fe84dc9a 13
david8251 0:c6e1fe84dc9a 14 #define MMA7455_I2C_ADDR (0x1D << 1)
david8251 0:c6e1fe84dc9a 15
david8251 0:c6e1fe84dc9a 16 #define MMA7455_ADDR_XOUTL 0x00
david8251 0:c6e1fe84dc9a 17 #define MMA7455_ADDR_XOUTH 0x01
david8251 0:c6e1fe84dc9a 18 #define MMA7455_ADDR_YOUTL 0x02
david8251 0:c6e1fe84dc9a 19 #define MMA7455_ADDR_YOUTH 0x03
david8251 0:c6e1fe84dc9a 20 #define MMA7455_ADDR_ZOUTL 0x04
david8251 0:c6e1fe84dc9a 21 #define MMA7455_ADDR_ZOUTH 0x05
david8251 0:c6e1fe84dc9a 22 #define MMA7455_ADDR_XOUT8 0x06
david8251 0:c6e1fe84dc9a 23 #define MMA7455_ADDR_YOUT8 0x07
david8251 0:c6e1fe84dc9a 24 #define MMA7455_ADDR_ZOUT8 0x08
david8251 0:c6e1fe84dc9a 25 #define MMA7455_ADDR_STATUS 0x09
david8251 0:c6e1fe84dc9a 26 #define MMA7455_ADDR_DETSRC 0x0A
david8251 0:c6e1fe84dc9a 27 #define MMA7455_ADDR_TOUT 0x0B
david8251 0:c6e1fe84dc9a 28 #define MMA7455_ADDR_I2CAD 0x0D
david8251 0:c6e1fe84dc9a 29 #define MMA7455_ADDR_USRINF 0x0E
david8251 0:c6e1fe84dc9a 30 #define MMA7455_ADDR_WHOAMI 0x0F
david8251 0:c6e1fe84dc9a 31 #define MMA7455_ADDR_XOFFL 0x10
david8251 0:c6e1fe84dc9a 32 #define MMA7455_ADDR_XOFFH 0x11
david8251 0:c6e1fe84dc9a 33 #define MMA7455_ADDR_YOFFL 0x12
david8251 0:c6e1fe84dc9a 34 #define MMA7455_ADDR_YOFFH 0x13
david8251 0:c6e1fe84dc9a 35 #define MMA7455_ADDR_ZOFFL 0x14
david8251 0:c6e1fe84dc9a 36 #define MMA7455_ADDR_ZOFFH 0x15
david8251 0:c6e1fe84dc9a 37 #define MMA7455_ADDR_MCTL 0x16
david8251 0:c6e1fe84dc9a 38 #define MMA7455_ADDR_INTRST 0x17
david8251 0:c6e1fe84dc9a 39 #define MMA7455_ADDR_CTL1 0x18
david8251 0:c6e1fe84dc9a 40 #define MMA7455_ADDR_CTL2 0x19
david8251 0:c6e1fe84dc9a 41 #define MMA7455_ADDR_LDTH 0x1A
david8251 0:c6e1fe84dc9a 42 #define MMA7455_ADDR_PDTH 0x1B
david8251 0:c6e1fe84dc9a 43 #define MMA7455_ADDR_PW 0x1C
david8251 0:c6e1fe84dc9a 44 #define MMA7455_ADDR_LT 0x1D
david8251 0:c6e1fe84dc9a 45 #define MMA7455_ADDR_TW 0x1E
david8251 0:c6e1fe84dc9a 46
david8251 0:c6e1fe84dc9a 47 #define MMA7455_MCTL_MODE(m) ((m) << 0)
david8251 0:c6e1fe84dc9a 48 #define MMA7455_MCTL_GLVL(g) ((g) << 2)
david8251 0:c6e1fe84dc9a 49
david8251 0:c6e1fe84dc9a 50 #define MMA7455_STATUS_DRDY (1 << 0)
david8251 0:c6e1fe84dc9a 51 #define MMA7455_STATUS_DOVR (1 << 1)
david8251 0:c6e1fe84dc9a 52 #define MMA7455_STATUS_PERR (1 << 2)
david8251 0:c6e1fe84dc9a 53
david8251 0:c6e1fe84dc9a 54
david8251 0:c6e1fe84dc9a 55 MMA7455::MMA7455(PinName sda, PinName scl) : _i2c(sda, scl)
david8251 0:c6e1fe84dc9a 56 {
david8251 0:c6e1fe84dc9a 57 _mode = ModeStandby;
david8251 0:c6e1fe84dc9a 58 _range = Range_8g;
david8251 0:c6e1fe84dc9a 59
david8251 0:c6e1fe84dc9a 60 _xOff = 0;
david8251 0:c6e1fe84dc9a 61 _yOff = 0;
david8251 0:c6e1fe84dc9a 62 _zOff = 0;
david8251 0:c6e1fe84dc9a 63 }
david8251 0:c6e1fe84dc9a 64
david8251 0:c6e1fe84dc9a 65 bool MMA7455::setMode(Mode mode) {
david8251 0:c6e1fe84dc9a 66 bool result = false;
david8251 0:c6e1fe84dc9a 67 int mCtrl = 0;
david8251 0:c6e1fe84dc9a 68
david8251 0:c6e1fe84dc9a 69 do {
david8251 0:c6e1fe84dc9a 70 mCtrl = getModeControl();
david8251 0:c6e1fe84dc9a 71 if (mCtrl < 0) break;
david8251 0:c6e1fe84dc9a 72
david8251 0:c6e1fe84dc9a 73 mCtrl &= ~(0x03 << 0);
david8251 0:c6e1fe84dc9a 74 mCtrl |= MMA7455_MCTL_MODE(mode);
david8251 0:c6e1fe84dc9a 75
david8251 0:c6e1fe84dc9a 76 if (setModeControl((uint8_t)mCtrl) < 0) {
david8251 0:c6e1fe84dc9a 77 break;
david8251 0:c6e1fe84dc9a 78 }
david8251 0:c6e1fe84dc9a 79
david8251 0:c6e1fe84dc9a 80 _mode = mode;
david8251 0:c6e1fe84dc9a 81 result = true;
david8251 0:c6e1fe84dc9a 82 } while(0);
david8251 0:c6e1fe84dc9a 83
david8251 0:c6e1fe84dc9a 84
david8251 0:c6e1fe84dc9a 85
david8251 0:c6e1fe84dc9a 86 return result;
david8251 0:c6e1fe84dc9a 87 }
david8251 0:c6e1fe84dc9a 88
david8251 0:c6e1fe84dc9a 89 bool MMA7455::setRange(Range range) {
david8251 0:c6e1fe84dc9a 90 bool result = false;
david8251 0:c6e1fe84dc9a 91 int mCtrl = 0;
david8251 0:c6e1fe84dc9a 92
david8251 0:c6e1fe84dc9a 93 do {
david8251 0:c6e1fe84dc9a 94 mCtrl = getModeControl();
david8251 0:c6e1fe84dc9a 95 if (mCtrl < 0) break;
david8251 0:c6e1fe84dc9a 96
david8251 0:c6e1fe84dc9a 97 mCtrl &= ~(0x03 << 2);
david8251 0:c6e1fe84dc9a 98 mCtrl |= MMA7455_MCTL_GLVL(range);
david8251 0:c6e1fe84dc9a 99
david8251 0:c6e1fe84dc9a 100 if (setModeControl((uint8_t)mCtrl) < 0) {
david8251 0:c6e1fe84dc9a 101 break;
david8251 0:c6e1fe84dc9a 102 }
david8251 0:c6e1fe84dc9a 103
david8251 0:c6e1fe84dc9a 104 _range = range;
david8251 0:c6e1fe84dc9a 105 result = true;
david8251 0:c6e1fe84dc9a 106 } while(0);
david8251 0:c6e1fe84dc9a 107
david8251 0:c6e1fe84dc9a 108
david8251 0:c6e1fe84dc9a 109
david8251 0:c6e1fe84dc9a 110 return result;
david8251 0:c6e1fe84dc9a 111
david8251 0:c6e1fe84dc9a 112 }
david8251 0:c6e1fe84dc9a 113
david8251 0:c6e1fe84dc9a 114 bool MMA7455::read(int32_t& x, int32_t& y, int32_t& z) {
david8251 0:c6e1fe84dc9a 115 bool result = false;
david8251 0:c6e1fe84dc9a 116
david8251 0:c6e1fe84dc9a 117
david8251 0:c6e1fe84dc9a 118 // nothing to read in standby mode
david8251 0:c6e1fe84dc9a 119 if (_mode == ModeStandby) return false;
david8251 0:c6e1fe84dc9a 120
david8251 0:c6e1fe84dc9a 121 // wait for ready flag
david8251 0:c6e1fe84dc9a 122 int status = 0;
david8251 0:c6e1fe84dc9a 123 do {
david8251 0:c6e1fe84dc9a 124 status = getStatus();
david8251 0:c6e1fe84dc9a 125 } while (status >= 0 && (status & MMA7455_STATUS_DRDY) == 0);
david8251 0:c6e1fe84dc9a 126
david8251 0:c6e1fe84dc9a 127
david8251 0:c6e1fe84dc9a 128 do {
david8251 0:c6e1fe84dc9a 129 if (status < 0) break;
david8251 0:c6e1fe84dc9a 130
david8251 0:c6e1fe84dc9a 131
david8251 0:c6e1fe84dc9a 132 char buf[6];
david8251 0:c6e1fe84dc9a 133 buf[0] = MMA7455_ADDR_XOUTL;
david8251 0:c6e1fe84dc9a 134 if (_i2c.write(MMA7455_I2C_ADDR, buf, 1) != 0) break;
david8251 0:c6e1fe84dc9a 135 if (_i2c.read(MMA7455_I2C_ADDR, buf, 6) != 0) break;
david8251 0:c6e1fe84dc9a 136
david8251 0:c6e1fe84dc9a 137 // check if second bit is set in high byte -> negative value
david8251 0:c6e1fe84dc9a 138 // expand negative value to full byte
david8251 0:c6e1fe84dc9a 139 if (buf[1] & 0x02) buf[1] |= 0xFC;
david8251 0:c6e1fe84dc9a 140 if (buf[3] & 0x02) buf[3] |= 0xFC;
david8251 0:c6e1fe84dc9a 141 if (buf[5] & 0x02) buf[5] |= 0xFC;
david8251 0:c6e1fe84dc9a 142
david8251 0:c6e1fe84dc9a 143 x = (int16_t)((buf[1] << 8) | buf[0]) + _xOff;
david8251 0:c6e1fe84dc9a 144 y = (int16_t)((buf[3] << 8) | buf[2]) + _yOff;
david8251 0:c6e1fe84dc9a 145 z = (int16_t)((buf[5] << 8) | buf[4]) + _zOff;
david8251 0:c6e1fe84dc9a 146
david8251 0:c6e1fe84dc9a 147
david8251 0:c6e1fe84dc9a 148 result = true;
david8251 0:c6e1fe84dc9a 149
david8251 0:c6e1fe84dc9a 150 } while(0);
david8251 0:c6e1fe84dc9a 151
david8251 0:c6e1fe84dc9a 152
david8251 0:c6e1fe84dc9a 153 return result;
david8251 0:c6e1fe84dc9a 154 }
david8251 0:c6e1fe84dc9a 155
david8251 0:c6e1fe84dc9a 156 bool MMA7455::calibrate() {
david8251 0:c6e1fe84dc9a 157 bool result = false;
david8251 0:c6e1fe84dc9a 158 bool failed = false;
david8251 0:c6e1fe84dc9a 159
david8251 0:c6e1fe84dc9a 160 int32_t x = 0;
david8251 0:c6e1fe84dc9a 161 int32_t y = 0;
david8251 0:c6e1fe84dc9a 162 int32_t z = 0;
david8251 0:c6e1fe84dc9a 163
david8251 0:c6e1fe84dc9a 164 int32_t xr = 0;
david8251 0:c6e1fe84dc9a 165 int32_t yr = 0;
david8251 0:c6e1fe84dc9a 166 int32_t zr = 0;
david8251 0:c6e1fe84dc9a 167
david8251 0:c6e1fe84dc9a 168 int xOff = 0;
david8251 0:c6e1fe84dc9a 169 int yOff = 0;
david8251 0:c6e1fe84dc9a 170 int zOff = 16;
david8251 0:c6e1fe84dc9a 171 if (_range == Range_2g) {
david8251 0:c6e1fe84dc9a 172 zOff = 64;
david8251 0:c6e1fe84dc9a 173 }
david8251 0:c6e1fe84dc9a 174 if (_range == Range_4g) {
david8251 0:c6e1fe84dc9a 175 zOff = 32;
david8251 0:c6e1fe84dc9a 176 }
david8251 0:c6e1fe84dc9a 177
david8251 0:c6e1fe84dc9a 178 do {
david8251 0:c6e1fe84dc9a 179
david8251 0:c6e1fe84dc9a 180 // get an average of 6 values
david8251 0:c6e1fe84dc9a 181 for (int i = 0; i < 6; i++) {
david8251 0:c6e1fe84dc9a 182 if (!read(xr, yr, zr)) {
david8251 0:c6e1fe84dc9a 183 failed = true;
david8251 0:c6e1fe84dc9a 184 break;
david8251 0:c6e1fe84dc9a 185 }
david8251 0:c6e1fe84dc9a 186 x += xr;
david8251 0:c6e1fe84dc9a 187 y += yr;
david8251 0:c6e1fe84dc9a 188 z += zr;
david8251 0:c6e1fe84dc9a 189
david8251 0:c6e1fe84dc9a 190 wait_ms(100);
david8251 0:c6e1fe84dc9a 191 }
david8251 0:c6e1fe84dc9a 192
david8251 0:c6e1fe84dc9a 193 if (failed) break;
david8251 0:c6e1fe84dc9a 194 x /= 6;
david8251 0:c6e1fe84dc9a 195 y /= 6;
david8251 0:c6e1fe84dc9a 196 z /= 6;
david8251 0:c6e1fe84dc9a 197
david8251 0:c6e1fe84dc9a 198 xOff -= x;
david8251 0:c6e1fe84dc9a 199 yOff -= y;
david8251 0:c6e1fe84dc9a 200 zOff -= z;
david8251 0:c6e1fe84dc9a 201
david8251 0:c6e1fe84dc9a 202 /*
david8251 0:c6e1fe84dc9a 203 * For some reason we have not got correct/reliable calibration
david8251 0:c6e1fe84dc9a 204 * by using the offset drift registers. Instead we are
david8251 0:c6e1fe84dc9a 205 * calculating the offsets and store them in member variables.
david8251 0:c6e1fe84dc9a 206 *
david8251 0:c6e1fe84dc9a 207 * These member variables are then used in the read() method
david8251 0:c6e1fe84dc9a 208 */
david8251 0:c6e1fe84dc9a 209
david8251 0:c6e1fe84dc9a 210 _xOff = xOff;
david8251 0:c6e1fe84dc9a 211 _yOff = yOff;
david8251 0:c6e1fe84dc9a 212 _zOff = zOff;
david8251 0:c6e1fe84dc9a 213
david8251 0:c6e1fe84dc9a 214
david8251 0:c6e1fe84dc9a 215 result = true;
david8251 0:c6e1fe84dc9a 216
david8251 0:c6e1fe84dc9a 217 } while (0);
david8251 0:c6e1fe84dc9a 218
david8251 0:c6e1fe84dc9a 219
david8251 0:c6e1fe84dc9a 220
david8251 0:c6e1fe84dc9a 221 return result;
david8251 0:c6e1fe84dc9a 222 }
david8251 0:c6e1fe84dc9a 223
david8251 0:c6e1fe84dc9a 224 bool MMA7455::setCalibrationOffsets(int32_t xOff, int32_t yOff, int32_t zOff) {
david8251 0:c6e1fe84dc9a 225 _xOff = xOff;
david8251 0:c6e1fe84dc9a 226 _yOff = yOff;
david8251 0:c6e1fe84dc9a 227 _zOff = zOff;
david8251 0:c6e1fe84dc9a 228
david8251 0:c6e1fe84dc9a 229 return true;
david8251 0:c6e1fe84dc9a 230 }
david8251 0:c6e1fe84dc9a 231
david8251 0:c6e1fe84dc9a 232 bool MMA7455::getCalibrationOffsets(int32_t& xOff, int32_t& yOff, int32_t& zOff) {
david8251 0:c6e1fe84dc9a 233 xOff = _xOff;
david8251 0:c6e1fe84dc9a 234 yOff = _yOff;
david8251 0:c6e1fe84dc9a 235 zOff = _zOff;
david8251 0:c6e1fe84dc9a 236
david8251 0:c6e1fe84dc9a 237 return true;
david8251 0:c6e1fe84dc9a 238 }
david8251 0:c6e1fe84dc9a 239
david8251 0:c6e1fe84dc9a 240 int MMA7455::getStatus() {
david8251 0:c6e1fe84dc9a 241 int result = -1;
david8251 0:c6e1fe84dc9a 242 char data[1];
david8251 0:c6e1fe84dc9a 243
david8251 0:c6e1fe84dc9a 244 do {
david8251 0:c6e1fe84dc9a 245 data[0] = MMA7455_ADDR_STATUS;
david8251 0:c6e1fe84dc9a 246 if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break;
david8251 0:c6e1fe84dc9a 247
david8251 0:c6e1fe84dc9a 248 if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break;
david8251 0:c6e1fe84dc9a 249
david8251 0:c6e1fe84dc9a 250 result = data[0];
david8251 0:c6e1fe84dc9a 251
david8251 0:c6e1fe84dc9a 252 } while (0);
david8251 0:c6e1fe84dc9a 253
david8251 0:c6e1fe84dc9a 254
david8251 0:c6e1fe84dc9a 255
david8251 0:c6e1fe84dc9a 256 return result;
david8251 0:c6e1fe84dc9a 257 }
david8251 0:c6e1fe84dc9a 258
david8251 0:c6e1fe84dc9a 259 int MMA7455::getModeControl() {
david8251 0:c6e1fe84dc9a 260
david8251 0:c6e1fe84dc9a 261 int result = -1;
david8251 0:c6e1fe84dc9a 262 char data[1];
david8251 0:c6e1fe84dc9a 263
david8251 0:c6e1fe84dc9a 264 do {
david8251 0:c6e1fe84dc9a 265 data[0] = MMA7455_ADDR_MCTL;
david8251 0:c6e1fe84dc9a 266 if (_i2c.write(MMA7455_I2C_ADDR, data, 1) != 0) break;
david8251 0:c6e1fe84dc9a 267
david8251 0:c6e1fe84dc9a 268 if (_i2c.read(MMA7455_I2C_ADDR, data, 1) != 0) break;
david8251 0:c6e1fe84dc9a 269
david8251 0:c6e1fe84dc9a 270 result = data[0];
david8251 0:c6e1fe84dc9a 271
david8251 0:c6e1fe84dc9a 272 } while (0);
david8251 0:c6e1fe84dc9a 273
david8251 0:c6e1fe84dc9a 274
david8251 0:c6e1fe84dc9a 275
david8251 0:c6e1fe84dc9a 276 return result;
david8251 0:c6e1fe84dc9a 277 }
david8251 0:c6e1fe84dc9a 278
david8251 0:c6e1fe84dc9a 279 int MMA7455::setModeControl(uint8_t mctl) {
david8251 0:c6e1fe84dc9a 280 int result = -1;
david8251 0:c6e1fe84dc9a 281 char data[2];
david8251 0:c6e1fe84dc9a 282
david8251 0:c6e1fe84dc9a 283 do {
david8251 0:c6e1fe84dc9a 284 data[0] = MMA7455_ADDR_MCTL;
david8251 0:c6e1fe84dc9a 285 data[1] = (char)mctl;
david8251 0:c6e1fe84dc9a 286 if (_i2c.write(MMA7455_I2C_ADDR, data, 2) != 0) break;
david8251 0:c6e1fe84dc9a 287
david8251 0:c6e1fe84dc9a 288 result = 0;
david8251 0:c6e1fe84dc9a 289
david8251 0:c6e1fe84dc9a 290 } while (0);
david8251 0:c6e1fe84dc9a 291
david8251 0:c6e1fe84dc9a 292
david8251 0:c6e1fe84dc9a 293
david8251 0:c6e1fe84dc9a 294 return result;
david8251 0:c6e1fe84dc9a 295 }