Prosper Van / MotionDriverv512

InvenSense Motion Driver v5.1.2 ported

Dependents:   Sensorv2

I2Cdev/I2Cdev.cpp@0:4dd021344a6b, 2014-06-03 (annotated)

Committer:
oprospero
Date:
Tue Jun 03 07:54:12 2014 +0000
Revision:
0:4dd021344a6b
Child:
4:2cb380415dc7
Initial MotionDriver v5.1.2 ported from InvenSense

Who changed what in which revision?

UserRevisionLine numberNew contents of line
oprospero 0:4dd021344a6b 1 // ported from arduino library: https://github.com/jrowberg/i2cdevlib
oprospero 0:4dd021344a6b 2 // written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl)
oprospero 0:4dd021344a6b 3 // Changelog:
oprospero 0:4dd021344a6b 4 // 2013-01-08 - first release
oprospero 0:4dd021344a6b 5
oprospero 0:4dd021344a6b 6 #include "I2Cdev.h"
oprospero 0:4dd021344a6b 7
oprospero 0:4dd021344a6b 8 //#define useDebugSerial
oprospero 0:4dd021344a6b 9 #ifdef useDebugSerial
oprospero 0:4dd021344a6b 10 Serial debugSerial(USBTX, USBRX);
oprospero 0:4dd021344a6b 11 #endif
oprospero 0:4dd021344a6b 12
oprospero 0:4dd021344a6b 13
oprospero 0:4dd021344a6b 14 I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL)
oprospero 0:4dd021344a6b 15 {
oprospero 0:4dd021344a6b 16 // i2c.frequency(400000);
oprospero 0:4dd021344a6b 17 }
oprospero 0:4dd021344a6b 18
oprospero 0:4dd021344a6b 19 I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl)
oprospero 0:4dd021344a6b 20 {
oprospero 0:4dd021344a6b 21 // i2c.frequency(400000);
oprospero 0:4dd021344a6b 22 }
oprospero 0:4dd021344a6b 23
oprospero 0:4dd021344a6b 24 void I2Cdev::frequency(int hz)
oprospero 0:4dd021344a6b 25 {
oprospero 0:4dd021344a6b 26 i2c.frequency(hz);
oprospero 0:4dd021344a6b 27 }
oprospero 0:4dd021344a6b 28
oprospero 0:4dd021344a6b 29
oprospero 0:4dd021344a6b 30 /** Read a single bit from an 8-bit device register.
oprospero 0:4dd021344a6b 31 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 32 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 33 * @param bitNum Bit position to read (0-7)
oprospero 0:4dd021344a6b 34 * @param data Container for single bit value
oprospero 0:4dd021344a6b 35 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 36 * @return Status of read operation (true = success)
oprospero 0:4dd021344a6b 37 */
oprospero 0:4dd021344a6b 38 int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 39 uint8_t b;
oprospero 0:4dd021344a6b 40 uint8_t count = readByte(devAddr, regAddr, &b, timeout);
oprospero 0:4dd021344a6b 41 *data = b & (1 << bitNum);
oprospero 0:4dd021344a6b 42 return count;
oprospero 0:4dd021344a6b 43 }
oprospero 0:4dd021344a6b 44
oprospero 0:4dd021344a6b 45 /** Read a single bit from a 16-bit device register.
oprospero 0:4dd021344a6b 46 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 47 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 48 * @param bitNum Bit position to read (0-15)
oprospero 0:4dd021344a6b 49 * @param data Container for single bit value
oprospero 0:4dd021344a6b 50 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 51 * @return Status of read operation (true = success)
oprospero 0:4dd021344a6b 52 */
oprospero 0:4dd021344a6b 53 int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 54 uint16_t b;
oprospero 0:4dd021344a6b 55 uint8_t count = readWord(devAddr, regAddr, &b, timeout);
oprospero 0:4dd021344a6b 56 *data = b & (1 << bitNum);
oprospero 0:4dd021344a6b 57 return count;
oprospero 0:4dd021344a6b 58 }
oprospero 0:4dd021344a6b 59
oprospero 0:4dd021344a6b 60 /** Read multiple bits from an 8-bit device register.
oprospero 0:4dd021344a6b 61 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 62 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 63 * @param bitStart First bit position to read (0-7)
oprospero 0:4dd021344a6b 64 * @param length Number of bits to read (not more than 8)
oprospero 0:4dd021344a6b 65 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
oprospero 0:4dd021344a6b 66 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 67 * @return Status of read operation (true = success)
oprospero 0:4dd021344a6b 68 */
oprospero 0:4dd021344a6b 69 int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 70 // 01101001 read byte
oprospero 0:4dd021344a6b 71 // 76543210 bit numbers
oprospero 0:4dd021344a6b 72 // xxx args: bitStart=4, length=3
oprospero 0:4dd021344a6b 73 // 010 masked
oprospero 0:4dd021344a6b 74 // -> 010 shifted
oprospero 0:4dd021344a6b 75 uint8_t count = readByte(devAddr, regAddr, data, timeout);
oprospero 0:4dd021344a6b 76 if ( !count )
oprospero 0:4dd021344a6b 77 {
oprospero 0:4dd021344a6b 78 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
oprospero 0:4dd021344a6b 79 *data &= mask;
oprospero 0:4dd021344a6b 80 *data >>= (bitStart - length + 1);
oprospero 0:4dd021344a6b 81 }
oprospero 0:4dd021344a6b 82 return count;
oprospero 0:4dd021344a6b 83 }
oprospero 0:4dd021344a6b 84
oprospero 0:4dd021344a6b 85 /** Read multiple bits from a 16-bit device register.
oprospero 0:4dd021344a6b 86 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 87 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 88 * @param bitStart First bit position to read (0-15)
oprospero 0:4dd021344a6b 89 * @param length Number of bits to read (not more than 16)
oprospero 0:4dd021344a6b 90 * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
oprospero 0:4dd021344a6b 91 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 92 * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
oprospero 0:4dd021344a6b 93 */
oprospero 0:4dd021344a6b 94 int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 95 // 1101011001101001 read byte
oprospero 0:4dd021344a6b 96 // fedcba9876543210 bit numbers
oprospero 0:4dd021344a6b 97 // xxx args: bitStart=12, length=3
oprospero 0:4dd021344a6b 98 // 010 masked
oprospero 0:4dd021344a6b 99 // -> 010 shifted
oprospero 0:4dd021344a6b 100 uint8_t count;
oprospero 0:4dd021344a6b 101 uint16_t w;
oprospero 0:4dd021344a6b 102 if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
oprospero 0:4dd021344a6b 103 uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
oprospero 0:4dd021344a6b 104 w &= mask;
oprospero 0:4dd021344a6b 105 w >>= (bitStart - length + 1);
oprospero 0:4dd021344a6b 106 *data = w;
oprospero 0:4dd021344a6b 107 }
oprospero 0:4dd021344a6b 108 return count;
oprospero 0:4dd021344a6b 109 }
oprospero 0:4dd021344a6b 110 /** Read single byte from an 8-bit device register.
oprospero 0:4dd021344a6b 111 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 112 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 113 * @param data Container for byte value read from device
oprospero 0:4dd021344a6b 114 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 115 * @return Status of read operation (true = success)
oprospero 0:4dd021344a6b 116 */
oprospero 0:4dd021344a6b 117 int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 118 return readBytes(devAddr, regAddr, 1, data, timeout);
oprospero 0:4dd021344a6b 119 }
oprospero 0:4dd021344a6b 120
oprospero 0:4dd021344a6b 121 /** Read single word from a 16-bit device register.
oprospero 0:4dd021344a6b 122 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 123 * @param regAddr Register regAddr to read from
oprospero 0:4dd021344a6b 124 * @param data Container for word value read from device
oprospero 0:4dd021344a6b 125 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 126 * @return Status of read operation (true = success)
oprospero 0:4dd021344a6b 127 */
oprospero 0:4dd021344a6b 128 int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
oprospero 0:4dd021344a6b 129 return readWords(devAddr, regAddr, 1, data, timeout);
oprospero 0:4dd021344a6b 130 }
oprospero 0:4dd021344a6b 131
oprospero 0:4dd021344a6b 132 /** Read multiple bytes from an 8-bit device register.
oprospero 0:4dd021344a6b 133 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 134 * @param regAddr First register regAddr to read from
oprospero 0:4dd021344a6b 135 * @param length Number of bytes to read
oprospero 0:4dd021344a6b 136 * @param data Buffer to store read data in
oprospero 0:4dd021344a6b 137 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
oprospero 0:4dd021344a6b 138 * @return Number of bytes read (-1 indicates failure)
oprospero 0:4dd021344a6b 139 */
oprospero 0:4dd021344a6b 140
oprospero 0:4dd021344a6b 141 int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
oprospero 0:4dd021344a6b 142 {
oprospero 0:4dd021344a6b 143 char command[1];
oprospero 0:4dd021344a6b 144 command[0] = regAddr;
oprospero 0:4dd021344a6b 145 // char *redData = (char*)malloc(length);
oprospero 0:4dd021344a6b 146 // i2c.write(devAddr<<1, command, 1, true);
oprospero 0:4dd021344a6b 147 // i2c.read(devAddr<<1, redData, length);
oprospero 0:4dd021344a6b 148 // for(int i =0; i < length; i++) {
oprospero 0:4dd021344a6b 149 // data[i] = redData[i];
oprospero 0:4dd021344a6b 150 // }
oprospero 0:4dd021344a6b 151 // free (redData);
oprospero 0:4dd021344a6b 152 i2c.write(devAddr<<1, command, 1, true);
oprospero 0:4dd021344a6b 153 i2c.read(devAddr<<1, (char *) data, length);
oprospero 0:4dd021344a6b 154 return 0;
oprospero 0:4dd021344a6b 155 }
oprospero 0:4dd021344a6b 156
oprospero 0:4dd021344a6b 157 int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
oprospero 0:4dd021344a6b 158 {
oprospero 0:4dd021344a6b 159 return 0;
oprospero 0:4dd021344a6b 160 }
oprospero 0:4dd021344a6b 161
oprospero 0:4dd021344a6b 162 /** write a single bit in an 8-bit device register.
oprospero 0:4dd021344a6b 163 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 164 * @param regAddr Register regAddr to write to
oprospero 0:4dd021344a6b 165 * @param bitNum Bit position to write (0-7)
oprospero 0:4dd021344a6b 166 * @param value New bit value to write
oprospero 0:4dd021344a6b 167 * @return Status of operation (true = success)
oprospero 0:4dd021344a6b 168 */
oprospero 0:4dd021344a6b 169 bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
oprospero 0:4dd021344a6b 170 uint8_t b;
oprospero 0:4dd021344a6b 171 readByte(devAddr, regAddr, &b);
oprospero 0:4dd021344a6b 172 b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
oprospero 0:4dd021344a6b 173 return writeByte(devAddr, regAddr, b);
oprospero 0:4dd021344a6b 174 }
oprospero 0:4dd021344a6b 175
oprospero 0:4dd021344a6b 176 /** write a single bit in a 16-bit device register.
oprospero 0:4dd021344a6b 177 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 178 * @param regAddr Register regAddr to write to
oprospero 0:4dd021344a6b 179 * @param bitNum Bit position to write (0-15)
oprospero 0:4dd021344a6b 180 * @param value New bit value to write
oprospero 0:4dd021344a6b 181 * @return Status of operation (true = success)
oprospero 0:4dd021344a6b 182 */
oprospero 0:4dd021344a6b 183 bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
oprospero 0:4dd021344a6b 184 uint16_t w;
oprospero 0:4dd021344a6b 185 readWord(devAddr, regAddr, &w);
oprospero 0:4dd021344a6b 186 w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
oprospero 0:4dd021344a6b 187 return writeWord(devAddr, regAddr, w);
oprospero 0:4dd021344a6b 188 }
oprospero 0:4dd021344a6b 189
oprospero 0:4dd021344a6b 190 /** Write multiple bits in an 8-bit device register.
oprospero 0:4dd021344a6b 191 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 192 * @param regAddr Register regAddr to write to
oprospero 0:4dd021344a6b 193 * @param bitStart First bit position to write (0-7)
oprospero 0:4dd021344a6b 194 * @param length Number of bits to write (not more than 8)
oprospero 0:4dd021344a6b 195 * @param data Right-aligned value to write
oprospero 0:4dd021344a6b 196 * @return Status of operation (true = success)
oprospero 0:4dd021344a6b 197 */
oprospero 0:4dd021344a6b 198 bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
oprospero 0:4dd021344a6b 199 // 010 value to write
oprospero 0:4dd021344a6b 200 // 76543210 bit numbers
oprospero 0:4dd021344a6b 201 // xxx args: bitStart=4, length=3
oprospero 0:4dd021344a6b 202 // 00011100 mask byte
oprospero 0:4dd021344a6b 203 // 10101111 original value (sample)
oprospero 0:4dd021344a6b 204 // 10100011 original & ~mask
oprospero 0:4dd021344a6b 205 // 10101011 masked | value
oprospero 0:4dd021344a6b 206 uint8_t b;
oprospero 0:4dd021344a6b 207 if (readByte(devAddr, regAddr, &b) != 0) {
oprospero 0:4dd021344a6b 208 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
oprospero 0:4dd021344a6b 209 data <<= (bitStart - length + 1); // shift data into correct position
oprospero 0:4dd021344a6b 210 data &= mask; // zero all non-important bits in data
oprospero 0:4dd021344a6b 211 b &= ~(mask); // zero all important bits in existing byte
oprospero 0:4dd021344a6b 212 b |= data; // combine data with existing byte
oprospero 0:4dd021344a6b 213 return writeByte(devAddr, regAddr, b);
oprospero 0:4dd021344a6b 214 } else {
oprospero 0:4dd021344a6b 215 return false;
oprospero 0:4dd021344a6b 216 }
oprospero 0:4dd021344a6b 217 }
oprospero 0:4dd021344a6b 218
oprospero 0:4dd021344a6b 219 /** Write multiple bits in a 16-bit device register.
oprospero 0:4dd021344a6b 220 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 221 * @param regAddr Register regAddr to write to
oprospero 0:4dd021344a6b 222 * @param bitStart First bit position to write (0-15)
oprospero 0:4dd021344a6b 223 * @param length Number of bits to write (not more than 16)
oprospero 0:4dd021344a6b 224 * @param data Right-aligned value to write
oprospero 0:4dd021344a6b 225 * @return Status of operation (true = success)
oprospero 0:4dd021344a6b 226 */
oprospero 0:4dd021344a6b 227 bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
oprospero 0:4dd021344a6b 228 // 010 value to write
oprospero 0:4dd021344a6b 229 // fedcba9876543210 bit numbers
oprospero 0:4dd021344a6b 230 // xxx args: bitStart=12, length=3
oprospero 0:4dd021344a6b 231 // 0001110000000000 mask byte
oprospero 0:4dd021344a6b 232 // 1010111110010110 original value (sample)
oprospero 0:4dd021344a6b 233 // 1010001110010110 original & ~mask
oprospero 0:4dd021344a6b 234 // 1010101110010110 masked | value
oprospero 0:4dd021344a6b 235 uint16_t w;
oprospero 0:4dd021344a6b 236 if (readWord(devAddr, regAddr, &w) != 0) {
oprospero 0:4dd021344a6b 237 uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
oprospero 0:4dd021344a6b 238 data <<= (bitStart - length + 1); // shift data into correct position
oprospero 0:4dd021344a6b 239 data &= mask; // zero all non-important bits in data
oprospero 0:4dd021344a6b 240 w &= ~(mask); // zero all important bits in existing word
oprospero 0:4dd021344a6b 241 w |= data; // combine data with existing word
oprospero 0:4dd021344a6b 242 return writeWord(devAddr, regAddr, w);
oprospero 0:4dd021344a6b 243 } else {
oprospero 0:4dd021344a6b 244 return false;
oprospero 0:4dd021344a6b 245 }
oprospero 0:4dd021344a6b 246 }
oprospero 0:4dd021344a6b 247
oprospero 0:4dd021344a6b 248 /** Write single byte to an 8-bit device register.
oprospero 0:4dd021344a6b 249 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 250 * @param regAddr Register address to write to
oprospero 0:4dd021344a6b 251 * @param data New byte value to write
oprospero 0:4dd021344a6b 252 * @return Status of operation (non-zero = success)
oprospero 0:4dd021344a6b 253 */
oprospero 0:4dd021344a6b 254 int I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
oprospero 0:4dd021344a6b 255 return writeBytes(devAddr, regAddr, 1, &data);
oprospero 0:4dd021344a6b 256 }
oprospero 0:4dd021344a6b 257
oprospero 0:4dd021344a6b 258 /** Write single word to a 16-bit device register.
oprospero 0:4dd021344a6b 259 * @param devAddr I2C slave device address
oprospero 0:4dd021344a6b 260 * @param regAddr Register address to write to
oprospero 0:4dd021344a6b 261 * @param data New word value to write
oprospero 0:4dd021344a6b 262 * @return Status of operation (non-zero = success)
oprospero 0:4dd021344a6b 263 */
oprospero 0:4dd021344a6b 264 bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
oprospero 0:4dd021344a6b 265 return writeWords(devAddr, regAddr, 1, &data);
oprospero 0:4dd021344a6b 266 }
oprospero 0:4dd021344a6b 267
oprospero 0:4dd021344a6b 268 int I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
oprospero 0:4dd021344a6b 269 {
oprospero 0:4dd021344a6b 270 int result = length;
oprospero 0:4dd021344a6b 271 // i2c.start();
oprospero 0:4dd021344a6b 272 // i2c.write(devAddr<<1);
oprospero 0:4dd021344a6b 273 // i2c.write(regAddr);
oprospero 0:4dd021344a6b 274 i2c.write(devAddr<<1,(char *) &regAddr,1,true);
oprospero 0:4dd021344a6b 275 for(int i = 0; i < length; i++) {
oprospero 0:4dd021344a6b 276 result -= i2c.write(data[i]);
oprospero 0:4dd021344a6b 277 }
oprospero 0:4dd021344a6b 278 i2c.stop();
oprospero 0:4dd021344a6b 279 return result;
oprospero 0:4dd021344a6b 280 }
oprospero 0:4dd021344a6b 281
oprospero 0:4dd021344a6b 282 bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
oprospero 0:4dd021344a6b 283 {
oprospero 0:4dd021344a6b 284 return true;
oprospero 0:4dd021344a6b 285 }
oprospero 0:4dd021344a6b 286
oprospero 0:4dd021344a6b 287 uint16_t I2Cdev::readTimeout(void)
oprospero 0:4dd021344a6b 288 {
oprospero 0:4dd021344a6b 289 return 0;
oprospero 0:4dd021344a6b 290 }