Dave Lu / FYDP_Final2

Only imu output

Dependencies:   Servo mbed

Fork of FYDP_Final2 by Mark Vandermeulen

drivers/MPU6050-DMP-Ian/I2C/I2Cdev2.cpp@5:d2e955a94940, 2015-03-25 (annotated)

Committer:
tntmarket
Date:
Wed Mar 25 09:58:50 2015 +0000
Revision:
5:d2e955a94940
Parent:
0:21019d94ad33 
only imu output

Who changed what in which revision?

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