Rearranged original code port/fork to: * Make library compatible with TiltyQuad IMU; * Prevent multiple definition, and added inclusion guard; * Cleaner access to library functions and file structure; and * "Broke out" code to control Sampling Rate and FIFO buffer update rate. By Trung Tin Ian HUA 2014. Credit to Jeff Rowberg for his original code, the best DMP implementation thus far; and szymon gaertig for porting the arduino library to mbed.
Fork of MPU6050 by
I2C/I2Cdev.cpp
- Committer:
- pHysiX
- Date:
- 2014-04-29
- Revision:
- 7:4619a083f289
- Parent:
- 6:2dc23167c8d8
File content as of revision 7:4619a083f289:
// ported from arduino library: https://github.com/jrowberg/i2cdevlib // written by szymon gaertig (email: szymon@gaertig.com.pl, website: szymongaertig.pl) // Changelog: // 2013-01-08 - first release #include "I2Cdev.h" #define useDebugSerial I2Cdev::I2Cdev(): i2c(I2C_SDA,I2C_SCL), debugSerial(USBTX, USBRX) { i2c.frequency(400000); debugSerial.baud(115200); } I2Cdev::I2Cdev(PinName i2cSda, PinName i2cScl): i2c(i2cSda,i2cScl), debugSerial(USBTX, USBRX) { i2c.frequency(400000); debugSerial.baud(115200); } /** Read a single bit from an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitNum Bit position to read (0-7) * @param data Container for single bit value * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { uint8_t b; uint8_t count = readByte(devAddr, regAddr, &b, timeout); *data = b & (1 << bitNum); return count; } /** Read a single bit from a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitNum Bit position to read (0-15) * @param data Container for single bit value * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { uint16_t b = 0x0000; uint8_t count = readWord(devAddr, regAddr, &b, timeout); *data = b & (1 << bitNum); return count; } /** Read multiple bits from an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitStart First bit position to read (0-7) * @param length Number of bits to read (not more than 8) * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { // 01101001 read byte // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 010 masked // -> 010 shifted uint8_t count, b; if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); b &= mask; b >>= (bitStart - length + 1); *data = b; } return count; } /** Read multiple bits from a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param bitStart First bit position to read (0-15) * @param length Number of bits to read (not more than 16) * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) */ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { // 1101011001101001 read byte // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 // 010 masked // -> 010 shifted uint8_t count; uint16_t w; if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); w &= mask; w >>= (bitStart - length + 1); *data = w; } return count; } /** Read single byte from an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param data Container for byte value read from device * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { return readBytes(devAddr, regAddr, 1, data, timeout); } /** Read single word from a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to read from * @param data Container for word value read from device * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { return readWords(devAddr, regAddr, 1, data, timeout); } /** Read multiple bytes from an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr First register regAddr to read from * @param length Number of bytes to read * @param data Buffer to store read data in * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Number of bytes read (-1 indicates failure) */ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { char command[1]; command[0] = regAddr; char *redData = (char*)malloc(length); i2c.write(devAddr<<1, command, 1, true); i2c.read(devAddr<<1, redData, length); for(int i =0; i < length; i++) { data[i] = redData[i]; } free (redData); return length; } int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { return 0; } /** write a single bit in an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitNum Bit position to write (0-7) * @param value New bit value to write * @return Status of operation (true = success) */ bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { uint8_t b; readByte(devAddr, regAddr, &b); b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); return writeByte(devAddr, regAddr, b); } /** write a single bit in a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitNum Bit position to write (0-15) * @param value New bit value to write * @return Status of operation (true = success) */ bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { uint16_t w; readWord(devAddr, regAddr, &w); w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); return writeWord(devAddr, regAddr, w); } /** Write multiple bits in an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitStart First bit position to write (0-7) * @param length Number of bits to write (not more than 8) * @param data Right-aligned value to write * @return Status of operation (true = success) */ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 00011100 mask byte // 10101111 original value (sample) // 10100011 original & ~mask // 10101011 masked | value uint8_t b; if (readByte(devAddr, regAddr, &b) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data b &= ~(mask); // zero all important bits in existing byte b |= data; // combine data with existing byte return writeByte(devAddr, regAddr, b); } else { return false; } } /** Write multiple bits in a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register regAddr to write to * @param bitStart First bit position to write (0-15) * @param length Number of bits to write (not more than 16) * @param data Right-aligned value to write * @return Status of operation (true = success) */ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { // 010 value to write // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 // 0001110000000000 mask byte // 1010111110010110 original value (sample) // 1010001110010110 original & ~mask // 1010101110010110 masked | value uint16_t w; if (readWord(devAddr, regAddr, &w) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data w &= ~(mask); // zero all important bits in existing word w |= data; // combine data with existing word return writeWord(devAddr, regAddr, w); } else { return false; } } /** Write single byte to an 8-bit device register. * @param devAddr I2C slave device address * @param regAddr Register address to write to * @param data New byte value to write * @return Status of operation (true = success) */ bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { return writeBytes(devAddr, regAddr, 1, &data); } /** Write single word to a 16-bit device register. * @param devAddr I2C slave device address * @param regAddr Register address to write to * @param data New word value to write * @return Status of operation (true = success) */ bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { return writeWords(devAddr, regAddr, 1, &data); } bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) { i2c.start(); i2c.write(devAddr<<1); i2c.write(regAddr); for(int i = 0; i < length; i++) { i2c.write(data[i]); } i2c.stop(); return true; } bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) { return true; } uint16_t I2Cdev::readTimeout(void) { return 0; }