jajn HA / Mbed OS C027_SupportTest

final

Fork of C027_SupportTest by u-blox

MMA7455.cpp@34:2d06e42fac2c, 2017-07-20 (annotated)

Committer:
david8251
Date:
Thu Jul 20 09:02:49 2017 +0000
Revision:
34:2d06e42fac2c
final

Who changed what in which revision?

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