Amr El-Gendy / Mbed 2 deprecated QUAD_PROJECT

a quadcopter code

Dependencies:   Pulse RangeFinder mbed

I2Cdev.cpp@0:4a55d0a21ea9, 2015-11-24 (annotated)

Committer:
Gendy
Date:
Tue Nov 24 19:57:06 2015 +0000
Revision:
0:4a55d0a21ea9
Quad_project PID on angle;

Who changed what in which revision?

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