Mark Vandermeulen / MPU6050-DMP

MPU6050 library modified so I can use 2 IMUs

Dependents:   MPU6050-DMP_sample

Fork of MPU6050 by Shundo Kishi

I2Cdev2.cpp@6:d348c3cad7c7, 2015-03-18 (annotated)

Committer:
majik
Date:
Wed Mar 18 22:18:55 2015 +0000
Revision:
6:d348c3cad7c7
This was modified so I can use 2 MPU6050 IMUs.; I just made a copy of the mpu6050 files under a different name. There is probably a better way to do this.

Who changed what in which revision?

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