EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

Drivers/MPU6050/I2Cdev.cpp@1:1da89c13dfa1, 2014-10-11 (annotated)

Committer:
Throwbot
Date:
Sat Oct 11 03:53:27 2014 +0000
Revision:
1:1da89c13dfa1
Ultrasonic seperate class

Who changed what in which revision?

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