Arnaud VALLEY / Mbed 2 deprecated Pinscape_Controller_V2_arnoz

Mirror with some correction

Dependencies:   mbed FastIO FastPWM USBDevice

MMA8451Q/MMA8451Q.cpp@116:7a67265d7c19, 2021-10-01 (annotated)

Committer:
arnoz
Date:
Fri Oct 01 08:19:46 2021 +0000
Revision:
116:7a67265d7c19
Parent:
77:0b96f6867312 
- Correct information regarding your last merge

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mjr 1:d913e0afb2ac 1 /* Copyright (c) 2010-2011 mbed.org, MIT License
mjr 1:d913e0afb2ac 2 *
mjr 1:d913e0afb2ac 3 * Permission is hereby granted, free of charge, to any person obtaining a copy of this software
mjr 1:d913e0afb2ac 4 * and associated documentation files (the "Software"), to deal in the Software without
mjr 1:d913e0afb2ac 5 * restriction, including without limitation the rights to use, copy, modify, merge, publish,
mjr 1:d913e0afb2ac 6 * distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
mjr 1:d913e0afb2ac 7 * Software is furnished to do so, subject to the following conditions:
mjr 1:d913e0afb2ac 8 *
mjr 1:d913e0afb2ac 9 * The above copyright notice and this permission notice shall be included in all copies or
mjr 1:d913e0afb2ac 10 * substantial portions of the Software.
mjr 1:d913e0afb2ac 11 *
mjr 1:d913e0afb2ac 12 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
mjr 1:d913e0afb2ac 13 * BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
mjr 1:d913e0afb2ac 14 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
mjr 1:d913e0afb2ac 15 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
mjr 1:d913e0afb2ac 16 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
mjr 1:d913e0afb2ac 17 */
mjr 1:d913e0afb2ac 18
mjr 1:d913e0afb2ac 19 #include "MMA8451Q.h"
mjr 1:d913e0afb2ac 20
mjr 77:0b96f6867312 21 #define REG_F_STATUS 0x00
mjr 77:0b96f6867312 22 #define REG_F_SETUP 0x09
mjr 1:d913e0afb2ac 23 #define REG_WHO_AM_I 0x0D
mjr 77:0b96f6867312 24 #define REG_CTRL_REG1 0x2A
mjr 77:0b96f6867312 25 #define REG_CTRL_REG2 0x2B
mjr 77:0b96f6867312 26 #define REG_CTRL_REG3 0x2c
mjr 77:0b96f6867312 27 #define REG_CTRL_REG4 0x2D
mjr 77:0b96f6867312 28 #define REG_CTRL_REG5 0x2E
mjr 1:d913e0afb2ac 29 #define REG_OFF_X 0x2F
mjr 1:d913e0afb2ac 30 #define REG_OFF_Y 0x30
mjr 1:d913e0afb2ac 31 #define REG_OFF_Z 0x31
mjr 1:d913e0afb2ac 32 #define XYZ_DATA_CFG_REG 0x0E
mjr 1:d913e0afb2ac 33 #define REG_OUT_X_MSB 0x01
mjr 1:d913e0afb2ac 34 #define REG_OUT_Y_MSB 0x03
mjr 1:d913e0afb2ac 35 #define REG_OUT_Z_MSB 0x05
mjr 1:d913e0afb2ac 36
mjr 1:d913e0afb2ac 37 #define UINT14_MAX 16383
mjr 1:d913e0afb2ac 38
mjr 1:d913e0afb2ac 39 #define CTL_ACTIVE 0x01
mjr 1:d913e0afb2ac 40 #define FS_MASK 0x03
mjr 1:d913e0afb2ac 41 #define FS_2G 0x00
mjr 1:d913e0afb2ac 42 #define FS_4G 0x01
mjr 1:d913e0afb2ac 43 #define FS_8G 0x02
mjr 1:d913e0afb2ac 44
mjr 77:0b96f6867312 45 #define F_STATUS_XDR_MASK 0x01 // F_STATUS - X sample ready
mjr 77:0b96f6867312 46 #define F_STATUS_YDR_MASK 0x02 // F_STATUS - Y sample ready
mjr 77:0b96f6867312 47 #define F_STATUS_ZDR_MASK 0x04 // F_STATUS - Z sample ready
mjr 77:0b96f6867312 48 #define F_STATUS_XYZDR_MASK 0x08 // F_STATUS - XYZ sample ready
mjr 77:0b96f6867312 49 #define F_STATUS_CNT_MASK 0x3F // F_STATUS register mask for FIFO count
mjr 77:0b96f6867312 50
mjr 77:0b96f6867312 51 #define F_MODE_MASK 0xC0 // F_SETUP register mask for FIFO mode
mjr 77:0b96f6867312 52 #define F_WMRK_MASK 0x3F // F_SETUP register mask for FIFO watermark
mjr 77:0b96f6867312 53
mjr 77:0b96f6867312 54 #define F_MODE_DISABLED 0x00 // FIFO disabled
mjr 77:0b96f6867312 55 #define F_MODE_CIRC 0x40 // circular FIFO
mjr 77:0b96f6867312 56 #define F_MODE_STOP 0x80 // FIFO stops when full
mjr 77:0b96f6867312 57 #define F_MODE_TRIGGER 0xC0 // FIFO triggers interrupt when watermark reached
mjr 77:0b96f6867312 58
mjr 1:d913e0afb2ac 59 #define HPF_OUT_MASK 0x10
mjr 1:d913e0afb2ac 60
mjr 1:d913e0afb2ac 61 #define SMODS_MASK 0x18
mjr 77:0b96f6867312 62
mjr 1:d913e0afb2ac 63 #define MODS_MASK 0x03
mjr 77:0b96f6867312 64 #define MODS_NORMAL 0x00 // mode 00 = normal power mode
mjr 77:0b96f6867312 65 #define MODS_LOW_NOISE 0x01 // mode 01 = low noise, low power
mjr 77:0b96f6867312 66 #define MODS_HI_RES 0x02 // mode 10 = high resolution
mjr 77:0b96f6867312 67 #define MODS_LOW_POWER 0x03 // mode 11 = low power
mjr 1:d913e0afb2ac 68
mjr 1:d913e0afb2ac 69 #define DR_MASK 0x38
mjr 1:d913e0afb2ac 70 #define DR_800_HZ 0x00
mjr 1:d913e0afb2ac 71 #define DR_400_HZ 0x08
mjr 1:d913e0afb2ac 72 #define DR_200_HZ 0x10
mjr 1:d913e0afb2ac 73 #define DR_100_HZ 0x18
mjr 1:d913e0afb2ac 74 #define DR_50_HZ 0x20
mjr 1:d913e0afb2ac 75 #define DR_12_HZ 0x28
mjr 1:d913e0afb2ac 76 #define DR_6_HZ 0x30
mjr 1:d913e0afb2ac 77 #define DR_1_HZ 0x38
mjr 1:d913e0afb2ac 78
mjr 77:0b96f6867312 79 #define F_READ_MASK 0x02 // CTRL_REG1 F_READ bit - sets data size mode:
mjr 77:0b96f6867312 80 // 1=fast read, 8-bit data; 0=14-bit data
mjr 77:0b96f6867312 81
mjr 3:3514575d4f86 82 #define CTRL_REG3_IPOL_MASK 0x02
mjr 3:3514575d4f86 83 #define CTRL_REG3_PPOD_MASK 0x01
mjr 3:3514575d4f86 84
mjr 3:3514575d4f86 85 #define INT_EN_DRDY 0x01
mjr 3:3514575d4f86 86 #define INT_CFG_DRDY 0x01
mjr 3:3514575d4f86 87
mjr 1:d913e0afb2ac 88
mjr 1:d913e0afb2ac 89 MMA8451Q::MMA8451Q(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr)
mjr 1:d913e0afb2ac 90 {
mjr 77:0b96f6867312 91 // set the I2C to fast mode
mjr 77:0b96f6867312 92 m_i2c.frequency(400000);
mjr 77:0b96f6867312 93
mjr 5:a70c0bce770d 94 // initialize parameters
mjr 5:a70c0bce770d 95 init();
mjr 5:a70c0bce770d 96 }
mjr 5:a70c0bce770d 97
mjr 5:a70c0bce770d 98 // reset the accelerometer and set our parameters
mjr 5:a70c0bce770d 99 void MMA8451Q::init()
mjr 5:a70c0bce770d 100 {
mjr 3:3514575d4f86 101 // reset all registers to power-on reset values
mjr 77:0b96f6867312 102 uint8_t d0[2] = { REG_CTRL_REG2, 0x40 };
mjr 3:3514575d4f86 103 writeRegs(d0,2 );
mjr 3:3514575d4f86 104
mjr 3:3514575d4f86 105 // wait for the reset bit to clear
mjr 3:3514575d4f86 106 do {
mjr 77:0b96f6867312 107 readRegs(REG_CTRL_REG2, d0, 1);
mjr 3:3514575d4f86 108 } while ((d0[0] & 0x40) != 0);
mjr 5:a70c0bce770d 109
mjr 5:a70c0bce770d 110 // go to standby mode
mjr 5:a70c0bce770d 111 standby();
mjr 3:3514575d4f86 112
mjr 77:0b96f6867312 113 // turn off FIFO mode - this is required before changing the F_READ bit
mjr 77:0b96f6867312 114 readRegs(REG_F_SETUP, d0, 1);
mjr 77:0b96f6867312 115 uint8_t d0a[2] = { REG_F_SETUP, 0 };
mjr 77:0b96f6867312 116 writeRegs(d0a, 2);
mjr 77:0b96f6867312 117
mjr 1:d913e0afb2ac 118 // read the curent config register
mjr 1:d913e0afb2ac 119 uint8_t d1[1];
mjr 1:d913e0afb2ac 120 readRegs(XYZ_DATA_CFG_REG, d1, 1);
mjr 1:d913e0afb2ac 121
mjr 77:0b96f6867312 122 // set 2g mode by default
mjr 1:d913e0afb2ac 123 uint8_t d2[2] = { XYZ_DATA_CFG_REG, (d1[0] & ~FS_MASK) | FS_2G };
mjr 1:d913e0afb2ac 124 writeRegs(d2, 2);
mjr 1:d913e0afb2ac 125
mjr 1:d913e0afb2ac 126 // read the ctl2 register
mjr 1:d913e0afb2ac 127 uint8_t d3[1];
mjr 77:0b96f6867312 128 readRegs(REG_CTRL_REG2, d3, 1);
mjr 1:d913e0afb2ac 129
mjr 1:d913e0afb2ac 130 // set the high resolution mode
mjr 77:0b96f6867312 131 uint8_t d4[2] = {REG_CTRL_REG2, (d3[0] & ~MODS_MASK) | MODS_HI_RES};
mjr 1:d913e0afb2ac 132 writeRegs(d4, 2);
mjr 1:d913e0afb2ac 133
mjr 77:0b96f6867312 134 // set 800 Hz mode, 14-bit data (clear the F_READ bit)
mjr 1:d913e0afb2ac 135 uint8_t d5[1];
mjr 77:0b96f6867312 136 readRegs(REG_CTRL_REG1, d5, 1);
mjr 77:0b96f6867312 137 uint8_t d6[2] = {REG_CTRL_REG1, (d5[0] & ~(DR_MASK | F_READ_MASK)) | DR_800_HZ};
mjr 1:d913e0afb2ac 138 writeRegs(d6, 2);
mjr 1:d913e0afb2ac 139
mjr 77:0b96f6867312 140 // set circular FIFO mode
mjr 77:0b96f6867312 141 uint8_t d7[1];
mjr 77:0b96f6867312 142 readRegs(REG_F_SETUP, d7, 1);
mjr 77:0b96f6867312 143 uint8_t d8[2] = {REG_F_SETUP, (d7[0] & ~F_MODE_MASK) | F_MODE_CIRC};
mjr 77:0b96f6867312 144 writeRegs(d8, 2);
mjr 77:0b96f6867312 145
mjr 1:d913e0afb2ac 146 // enter active mode
mjr 1:d913e0afb2ac 147 active();
mjr 1:d913e0afb2ac 148 }
mjr 1:d913e0afb2ac 149
mjr 1:d913e0afb2ac 150 MMA8451Q::~MMA8451Q() { }
mjr 1:d913e0afb2ac 151
mjr 77:0b96f6867312 152 bool MMA8451Q::sampleReady()
mjr 77:0b96f6867312 153 {
mjr 77:0b96f6867312 154 uint8_t d[1];
mjr 77:0b96f6867312 155 readRegs(REG_F_STATUS, d, 1);
mjr 77:0b96f6867312 156 return (d[0] & F_STATUS_XYZDR_MASK) == F_STATUS_XYZDR_MASK;
mjr 77:0b96f6867312 157 }
mjr 77:0b96f6867312 158
mjr 77:0b96f6867312 159 int MMA8451Q::getFIFOCount()
mjr 77:0b96f6867312 160 {
mjr 77:0b96f6867312 161 uint8_t d[1];
mjr 77:0b96f6867312 162 readRegs(REG_F_STATUS, d, 1);
mjr 77:0b96f6867312 163 return d[0] & F_STATUS_CNT_MASK;
mjr 77:0b96f6867312 164 }
mjr 77:0b96f6867312 165
mjr 3:3514575d4f86 166 void MMA8451Q::setInterruptMode(int pin)
mjr 3:3514575d4f86 167 {
mjr 3:3514575d4f86 168 // go to standby mode
mjr 3:3514575d4f86 169 standby();
mjr 3:3514575d4f86 170
mjr 3:3514575d4f86 171 // set IRQ push/pull and active high
mjr 3:3514575d4f86 172 uint8_t d1[1];
mjr 77:0b96f6867312 173 readRegs(REG_CTRL_REG3, d1, 1);
mjr 3:3514575d4f86 174 uint8_t d2[2] = {
mjr 77:0b96f6867312 175 REG_CTRL_REG3,
mjr 3:3514575d4f86 176 (d1[0] & ~CTRL_REG3_PPOD_MASK) | CTRL_REG3_IPOL_MASK
mjr 3:3514575d4f86 177 };
mjr 3:3514575d4f86 178 writeRegs(d2, 2);
mjr 3:3514575d4f86 179
mjr 3:3514575d4f86 180 // set pin 2 or pin 1
mjr 77:0b96f6867312 181 readRegs(REG_CTRL_REG5, d1, 1);
mjr 3:3514575d4f86 182 uint8_t d3[2] = {
mjr 77:0b96f6867312 183 REG_CTRL_REG5,
mjr 3:3514575d4f86 184 (d1[0] & ~INT_CFG_DRDY) | (pin == 1 ? INT_CFG_DRDY : 0)
mjr 3:3514575d4f86 185 };
mjr 3:3514575d4f86 186 writeRegs(d3, 2);
mjr 3:3514575d4f86 187
mjr 3:3514575d4f86 188 // enable data ready interrupt
mjr 77:0b96f6867312 189 readRegs(REG_CTRL_REG4, d1, 1);
mjr 77:0b96f6867312 190 uint8_t d4[2] = { REG_CTRL_REG4, d1[0] | INT_EN_DRDY };
mjr 3:3514575d4f86 191 writeRegs(d4, 2);
mjr 3:3514575d4f86 192
mjr 3:3514575d4f86 193 // enter active mode
mjr 3:3514575d4f86 194 active();
mjr 3:3514575d4f86 195 }
mjr 3:3514575d4f86 196
mjr 76:7f5912b6340e 197 void MMA8451Q::clearInterruptMode()
mjr 76:7f5912b6340e 198 {
mjr 76:7f5912b6340e 199 // go to standby mode
mjr 76:7f5912b6340e 200 standby();
mjr 76:7f5912b6340e 201
mjr 76:7f5912b6340e 202 // clear the interrupt register
mjr 77:0b96f6867312 203 uint8_t d1[2] = { REG_CTRL_REG4, 0 };
mjr 76:7f5912b6340e 204 writeRegs(d1, 2);
mjr 76:7f5912b6340e 205
mjr 76:7f5912b6340e 206 // enter active mode
mjr 76:7f5912b6340e 207 active();
mjr 76:7f5912b6340e 208 }
mjr 76:7f5912b6340e 209
mjr 77:0b96f6867312 210 void MMA8451Q::setRange(int g)
mjr 77:0b96f6867312 211 {
mjr 77:0b96f6867312 212 // go to standby mode
mjr 77:0b96f6867312 213 standby();
mjr 77:0b96f6867312 214
mjr 77:0b96f6867312 215 // read the curent config register
mjr 77:0b96f6867312 216 uint8_t d1[1];
mjr 77:0b96f6867312 217 readRegs(XYZ_DATA_CFG_REG, d1, 1);
mjr 77:0b96f6867312 218
mjr 77:0b96f6867312 219 // figure the mode flag for the desired G setting
mjr 77:0b96f6867312 220 uint8_t mode = (g == 8 ? FS_8G : g == 4 ? FS_4G : FS_2G);
mjr 77:0b96f6867312 221
mjr 77:0b96f6867312 222 // set new mode
mjr 77:0b96f6867312 223 uint8_t d2[2] = { XYZ_DATA_CFG_REG, (d1[0] & ~FS_MASK) | mode };
mjr 77:0b96f6867312 224 writeRegs(d2, 2);
mjr 77:0b96f6867312 225
mjr 77:0b96f6867312 226 // enter active mode
mjr 77:0b96f6867312 227 active();
mjr 77:0b96f6867312 228 }
mjr 77:0b96f6867312 229
mjr 1:d913e0afb2ac 230 void MMA8451Q::standby()
mjr 1:d913e0afb2ac 231 {
mjr 1:d913e0afb2ac 232 // read the current control register
mjr 1:d913e0afb2ac 233 uint8_t d1[1];
mjr 77:0b96f6867312 234 readRegs(REG_CTRL_REG1, d1, 1);
mjr 1:d913e0afb2ac 235
mjr 5:a70c0bce770d 236 // wait for standby mode
mjr 5:a70c0bce770d 237 do {
mjr 5:a70c0bce770d 238 // write it back with the Active bit cleared
mjr 77:0b96f6867312 239 uint8_t d2[2] = { REG_CTRL_REG1, d1[0] & ~CTL_ACTIVE };
mjr 5:a70c0bce770d 240 writeRegs(d2, 2);
mjr 5:a70c0bce770d 241
mjr 77:0b96f6867312 242 readRegs(REG_CTRL_REG1, d1, 1);
mjr 5:a70c0bce770d 243 } while (d1[0] & CTL_ACTIVE);
mjr 1:d913e0afb2ac 244 }
mjr 1:d913e0afb2ac 245
mjr 1:d913e0afb2ac 246 void MMA8451Q::active()
mjr 1:d913e0afb2ac 247 {
mjr 1:d913e0afb2ac 248 // read the current control register
mjr 1:d913e0afb2ac 249 uint8_t d1[1];
mjr 77:0b96f6867312 250 readRegs(REG_CTRL_REG1, d1, 1);
mjr 1:d913e0afb2ac 251
mjr 1:d913e0afb2ac 252 // write it back out with the Active bit set
mjr 77:0b96f6867312 253 uint8_t d2[2] = { REG_CTRL_REG1, d1[0] | CTL_ACTIVE };
mjr 1:d913e0afb2ac 254 writeRegs(d2, 2);
mjr 1:d913e0afb2ac 255 }
mjr 1:d913e0afb2ac 256
mjr 1:d913e0afb2ac 257 uint8_t MMA8451Q::getWhoAmI() {
mjr 1:d913e0afb2ac 258 uint8_t who_am_i = 0;
mjr 1:d913e0afb2ac 259 readRegs(REG_WHO_AM_I, &who_am_i, 1);
mjr 1:d913e0afb2ac 260 return who_am_i;
mjr 1:d913e0afb2ac 261 }
mjr 1:d913e0afb2ac 262
mjr 1:d913e0afb2ac 263 float MMA8451Q::getAccX() {
mjr 1:d913e0afb2ac 264 return (float(getAccAxis(REG_OUT_X_MSB))/4096.0);
mjr 1:d913e0afb2ac 265 }
mjr 1:d913e0afb2ac 266
mjr 1:d913e0afb2ac 267 void MMA8451Q::getAccXY(float &x, float &y)
mjr 1:d913e0afb2ac 268 {
mjr 1:d913e0afb2ac 269 // read the X and Y output registers
mjr 1:d913e0afb2ac 270 uint8_t res[4];
mjr 1:d913e0afb2ac 271 readRegs(REG_OUT_X_MSB, res, 4);
mjr 1:d913e0afb2ac 272
mjr 1:d913e0afb2ac 273 // translate the x value
mjr 1:d913e0afb2ac 274 uint16_t acc = (res[0] << 8) | (res[1]);
mjr 1:d913e0afb2ac 275 x = int16_t(acc)/(4*4096.0);
mjr 1:d913e0afb2ac 276
mjr 1:d913e0afb2ac 277 // translate the y value
mjr 1:d913e0afb2ac 278 acc = (res[2] << 9) | (res[3]);
mjr 1:d913e0afb2ac 279 y = int16_t(acc)/(4*4096.0);
mjr 1:d913e0afb2ac 280 }
mjr 1:d913e0afb2ac 281
mjr 3:3514575d4f86 282 void MMA8451Q::getAccXYZ(float &x, float &y, float &z)
mjr 3:3514575d4f86 283 {
mjr 3:3514575d4f86 284 // read the X, Y, and Z output registers
mjr 3:3514575d4f86 285 uint8_t res[6];
mjr 3:3514575d4f86 286 readRegs(REG_OUT_X_MSB, res, 6);
mjr 3:3514575d4f86 287
mjr 3:3514575d4f86 288 // translate the x value
mjr 3:3514575d4f86 289 uint16_t acc = (res[0] << 8) | (res[1]);
mjr 3:3514575d4f86 290 x = int16_t(acc)/(4*4096.0);
mjr 3:3514575d4f86 291
mjr 3:3514575d4f86 292 // translate the y value
mjr 3:3514575d4f86 293 acc = (res[2] << 8) | (res[3]);
mjr 3:3514575d4f86 294 y = int16_t(acc)/(4*4096.0);
mjr 3:3514575d4f86 295
mjr 3:3514575d4f86 296 // translate the z value
mjr 3:3514575d4f86 297 acc = (res[4] << 8) | (res[5]);
mjr 3:3514575d4f86 298 z = int16_t(acc)/(4*4096.0);
mjr 3:3514575d4f86 299 }
mjr 3:3514575d4f86 300
mjr 77:0b96f6867312 301 void MMA8451Q::getAccXYZ(int &x, int &y, int &z)
mjr 77:0b96f6867312 302 {
mjr 77:0b96f6867312 303 // read the X, Y, and Z output registers
mjr 77:0b96f6867312 304 uint8_t res[6];
mjr 77:0b96f6867312 305 readRegs(REG_OUT_X_MSB, res, 6);
mjr 77:0b96f6867312 306
mjr 77:0b96f6867312 307 // translate the register values
mjr 77:0b96f6867312 308 x = xlat14(&res[0]);
mjr 77:0b96f6867312 309 y = xlat14(&res[2]);
mjr 77:0b96f6867312 310 z = xlat14(&res[4]);
mjr 77:0b96f6867312 311 }
mjr 77:0b96f6867312 312
mjr 1:d913e0afb2ac 313 float MMA8451Q::getAccY() {
mjr 1:d913e0afb2ac 314 return (float(getAccAxis(REG_OUT_Y_MSB))/4096.0);
mjr 1:d913e0afb2ac 315 }
mjr 1:d913e0afb2ac 316
mjr 1:d913e0afb2ac 317 float MMA8451Q::getAccZ() {
mjr 1:d913e0afb2ac 318 return (float(getAccAxis(REG_OUT_Z_MSB))/4096.0);
mjr 1:d913e0afb2ac 319 }
mjr 1:d913e0afb2ac 320
mjr 1:d913e0afb2ac 321 void MMA8451Q::getAccAllAxis(float * res) {
mjr 1:d913e0afb2ac 322 res[0] = getAccX();
mjr 1:d913e0afb2ac 323 res[1] = getAccY();
mjr 1:d913e0afb2ac 324 res[2] = getAccZ();
mjr 1:d913e0afb2ac 325 }
mjr 1:d913e0afb2ac 326
mjr 1:d913e0afb2ac 327 int16_t MMA8451Q::getAccAxis(uint8_t addr) {
mjr 1:d913e0afb2ac 328 int16_t acc;
mjr 1:d913e0afb2ac 329 uint8_t res[2];
mjr 1:d913e0afb2ac 330 readRegs(addr, res, 2);
mjr 1:d913e0afb2ac 331
mjr 1:d913e0afb2ac 332 acc = (res[0] << 6) | (res[1] >> 2);
mjr 1:d913e0afb2ac 333 if (acc > UINT14_MAX/2)
mjr 1:d913e0afb2ac 334 acc -= UINT14_MAX;
mjr 1:d913e0afb2ac 335
mjr 1:d913e0afb2ac 336 return acc;
mjr 1:d913e0afb2ac 337 }
mjr 1:d913e0afb2ac 338
mjr 1:d913e0afb2ac 339 void MMA8451Q::readRegs(int addr, uint8_t * data, int len) {
mjr 1:d913e0afb2ac 340 char t[1] = {addr};
mjr 1:d913e0afb2ac 341 m_i2c.write(m_addr, t, 1, true);
mjr 1:d913e0afb2ac 342 m_i2c.read(m_addr, (char *)data, len);
mjr 1:d913e0afb2ac 343 }
mjr 1:d913e0afb2ac 344
mjr 1:d913e0afb2ac 345 void MMA8451Q::writeRegs(uint8_t * data, int len) {
mjr 1:d913e0afb2ac 346 m_i2c.write(m_addr, (char *)data, len);
mjr 1:d913e0afb2ac 347 }