Gurvan PRIEM / Mbed 2 deprecated RaptorControl

An incomplete quadcopter control programme.

Dependencies:   mbed

MPU6050/I2Cdev.cpp@0:9cb9445a11f0, 2013-07-17 (annotated)

Committer:
Gurvan
Date:
Wed Jul 17 15:58:25 2013 +0000
Revision:
0:9cb9445a11f0
Pour Zobson, fi(r)st commit.

Who changed what in which revision?

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