diff --git a/lib/i2cdev/I2Cdev.cpp b/lib/i2cdev/I2Cdev.cpp index 63653cc..219de05 100644 --- a/lib/i2cdev/I2Cdev.cpp +++ b/lib/i2cdev/I2Cdev.cpp @@ -1,25 +1,30 @@ // I2Cdev library collection - Main I2C device class // Abstracts bit and byte I2C R/W functions into a convenient class -// 6/9/2012 by Jeff Rowberg +// 2013-06-05 by Jeff Rowberg // // Changelog: -// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire -// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation -// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) -// 2011-10-03 - added automatic Arduino version detection for ease of use -// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications -// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) -// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default -// 2011-08-02 - added support for 16-bit registers -// - fixed incorrect Doxygen comments on some methods -// - added timeout value for read operations (thanks mem @ Arduino forums) -// 2011-07-30 - changed read/write function structures to return success or byte counts -// - made all methods static for multi-device memory savings -// 2011-07-28 - initial release +// 2021-09-28 - allow custom Wire object as transaction function argument +// 2020-01-20 - hardija : complete support for Teensy 3.x +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release /* ============================================ I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg +Copyright (c) 2013 Jeff Rowberg Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -43,34 +48,32 @@ THE SOFTWARE. #include "I2Cdev.h" -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE #ifdef I2CDEV_IMPLEMENTATION_WARNINGS #if ARDUINO < 100 #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. #warning This I2Cdev implementation does not support: #warning - Repeated starts conditions #warning - Timeout detection (some Wire requests block forever) #elif ARDUINO == 100 #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. #warning This I2Cdev implementation does not support: #warning - Repeated starts conditions #warning - Timeout detection (some Wire requests block forever) #elif ARDUINO > 100 - /* - #warning Using current Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + /*#warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. #warning This I2Cdev implementation does not support: - #warning - Timeout detection (some Wire requests block forever) - */ + #warning - Timeout detection (some Wire requests block forever)*/ #endif #endif #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - #error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE @@ -87,8 +90,6 @@ THE SOFTWARE. #endif -#define min(a, b) a > b ? b : a - /** Default constructor. */ I2Cdev::I2Cdev() { @@ -102,9 +103,9 @@ I2Cdev::I2Cdev() { * @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) { +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout, void *wireObj) { uint8_t b; - uint8_t count = readByte(devAddr, regAddr, &b, timeout); + uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj); *data = b & (1 << bitNum); return count; } @@ -117,9 +118,9 @@ int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t * @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) { +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout, void *wireObj) { uint16_t b; - uint8_t count = readWord(devAddr, regAddr, &b, timeout); + uint8_t count = readWord(devAddr, regAddr, &b, timeout, wireObj); *data = b & (1 << bitNum); return count; } @@ -133,14 +134,14 @@ int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16 * @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) { +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) { // 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) { + if ((count = readByte(devAddr, regAddr, &b, timeout, wireObj)) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); b &= mask; b >>= (bitStart - length + 1); @@ -158,7 +159,7 @@ int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint * @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) { +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) { // 1101011001101001 read byte // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 @@ -166,7 +167,7 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin // -> 010 shifted uint8_t count; uint16_t w; - if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + if ((count = readWord(devAddr, regAddr, &w, timeout, wireObj)) != 0) { uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); w &= mask; w >>= (bitStart - length + 1); @@ -182,8 +183,8 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin * @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); +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout, void *wireObj) { + return readBytes(devAddr, regAddr, 1, data, timeout, wireObj); } /** Read single word from a 16-bit device register. @@ -193,8 +194,8 @@ int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_ * @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); +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout, void *wireObj) { + return readWords(devAddr, regAddr, 1, data, timeout, wireObj); } /** Read multiple bytes from an 8-bit device register. @@ -205,7 +206,7 @@ int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16 * @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) { +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -216,90 +217,85 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 Serial.print("..."); #endif - uint8_t status = 0; int8_t count = 0; uint32_t t1 = millis(); - #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; #if (ARDUINO < 100) // Arduino v00xx (before v1.0), Wire library // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.receive(); + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->receive(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); #endif } - - Wire.endTransmission(); } #elif (ARDUINO == 100) // Arduino v1.0.0, Wire library // Adds standardized write() and read() stream methods instead of send() and receive() // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); #endif - } - - Wire.endTransmission(); + } } #elif (ARDUINO > 100) // Arduino v1.0.1+, Wire library // Adds official support for repeated start condition, yay! // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(false); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); #endif } - - status = Wire.endTransmission(); } #endif #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - // Fastwire library (STILL UNDER DEVELOPMENT, NON-FUNCTIONAL!) + // Fastwire library // no loop required for fastwire - uint8_t status = Fastwire::readBuf(devAddr, regAddr, data, length); + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); if (status == 0) { count = length; // success } else { @@ -312,11 +308,9 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(". Done, status "); - Serial.print(status); - Serial.print(", "); + Serial.print(". Done ("); Serial.print(count, DEC); - Serial.println(" read."); + Serial.println(" read)."); #endif return count; @@ -328,9 +322,9 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 * @param length Number of words 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 words read (0 indicates failure) + * @return Number of words read (-1 indicates failure) */ -int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -344,29 +338,31 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 int8_t count = 0; uint32_t t1 = millis(); - #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; #if (ARDUINO < 100) // Arduino v00xx (before v1.0), Wire library // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.receive() << 8; + data[count] = useWire->receive() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.receive(); + data[count] |= useWire->receive(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -375,31 +371,29 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 } msb = !msb; } - - Wire.endTransmission(); } #elif (ARDUINO == 100) // Arduino v1.0.0, Wire library // Adds standardized write() and read() stream methods instead of send() and receive() - + // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; + data[count] = useWire->read() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); + data[count] |= useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -407,32 +401,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 count++; } msb = !msb; - } - - Wire.endTransmission(); + } } #elif (ARDUINO > 100) // Arduino v1.0.1+, Wire library // Adds official support for repeated start condition, yay! // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes - + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(false); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; + data[count] = useWire->read() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); + data[count] |= useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -440,18 +432,16 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 count++; } msb = !msb; - } - - Wire.endTransmission(); + } } #endif #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - // Fastwire library (STILL UNDER DEVELOPMENT, NON-FUNCTIONAL!) + // Fastwire library // no loop required for fastwire - uint16_t intermediate[(uint8_t)length]; - uint8_t status = Fastwire::readBuf(devAddr, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + uint8_t intermediate[(uint8_t)length*2]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, intermediate, (uint8_t)(length * 2)); if (status == 0) { count = length; // success for (uint8_t i = 0; i < length; i++) { @@ -470,7 +460,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 Serial.print(count, DEC); Serial.println(" read)."); #endif - + return count; } @@ -481,11 +471,11 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 * @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) { +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj) { uint8_t b; - readByte(devAddr, regAddr, &b); + readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj); b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); - return writeByte(devAddr, regAddr, b); + return writeByte(devAddr, regAddr, b, wireObj); } /** write a single bit in a 16-bit device register. @@ -495,11 +485,11 @@ bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t * @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) { +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj) { uint16_t w; - readWord(devAddr, regAddr, &w); + readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj); w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); - return writeWord(devAddr, regAddr, w); + return writeWord(devAddr, regAddr, w, wireObj); } /** Write multiple bits in an 8-bit device register. @@ -510,7 +500,7 @@ bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_ * @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) { +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj) { // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 @@ -519,13 +509,13 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8 // 10100011 original & ~mask // 10101011 masked | value uint8_t b; - if (readByte(devAddr, regAddr, &b) != 0) { + if (readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj) != 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); + return writeByte(devAddr, regAddr, b, wireObj); } else { return false; } @@ -539,22 +529,22 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8 * @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) { +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj) { // 010 value to write // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 - // 0001110000000000 mask byte + // 0001110000000000 mask word // 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); + if (readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj) != 0) { + uint16_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); + return writeWord(devAddr, regAddr, w, wireObj); } else { return false; } @@ -566,8 +556,8 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint * @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); +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj) { + return writeBytes(devAddr, regAddr, 1, &data, wireObj); } /** Write single word to a 16-bit device register. @@ -576,8 +566,8 @@ bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { * @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::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj) { + return writeWords(devAddr, regAddr, 1, &data, wireObj); } /** Write multiple bytes to an 8-bit device register. @@ -587,7 +577,7 @@ bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { * @param data Buffer to copy new data from * @return Status of operation (true = success) */ -bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -598,35 +588,51 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_ Serial.print("..."); #endif uint8_t status = 0; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; +#endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send((uint8_t) regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write((uint8_t) regAddr); // send address + useWire->beginTransmission(devAddr); + useWire->send((uint8_t) regAddr); // send address + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->beginTransmission(devAddr); + useWire->write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); #endif for (uint8_t i = 0; i < length; i++) { - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t) data[i]); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.write((uint8_t) data[i]); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - status = Fastwire::write(devAddr, regAddr, data[i]); - Serial.println(status); - #endif #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[i], HEX); if (i + 1 < length) Serial.print(" "); #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->send((uint8_t) data[i]); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif } #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - status = Wire.endTransmission(); + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); #endif #ifdef I2CDEV_SERIAL_DEBUG - Serial.print(". Done, status "); - Serial.println(status); + Serial.println(". Done."); #endif return status == 0; } @@ -638,7 +644,7 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_ * @param data Buffer to copy new data from * @return Status of operation (true = success) */ -bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -649,33 +655,52 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16 Serial.print("..."); #endif uint8_t status = 0; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; +#endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send(regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write(regAddr); // send address + useWire->beginTransmission(devAddr); + useWire->send(regAddr); // send address + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->beginTransmission(devAddr); + useWire->write(regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); #endif - for (uint8_t i = 0; i < length * 2; i++) { - #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t)(data[i++] >> 8)); // send MSB - Wire.send((uint8_t)data[i]); // send LSB - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.write((uint8_t)(data[i++] >> 8)); // send MSB - Wire.write((uint8_t)data[i]); // send LSB - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) - status = Fastwire::write(devAddr, regAddr, (uint8_t)(data[i++] >> 8)); - status = Fastwire::write(devAddr, regAddr + 1, (uint8_t)data[i]); - #endif + for (uint8_t i = 0; i < length; i++) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[i], HEX); if (i + 1 < length) Serial.print(" "); #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->send((uint8_t)(data[i] >> 8)); // send MSB + useWire->send((uint8_t)data[i]); // send LSB + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t)(data[i] >> 8)); // send MSB + useWire->write((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i]); // send LSB + if (status != 0) break; + #endif } #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - status = Wire.endTransmission(); + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); #endif #ifdef I2CDEV_SERIAL_DEBUG Serial.println(". Done."); @@ -689,12 +714,23 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16 uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + /* - FastWire 0.2 - This is a library to help faster programs to read I2C devices. - Copyright(C) 2011 Francesco Ferrara - occhiobello at gmail dot com - */ + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ boolean Fastwire::waitInt() { int l = 250; @@ -721,11 +757,34 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; #endif TWSR = 0; // no prescaler => prescaler = 1 - TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWBR = F_CPU / 2000 / khz - 8; // change the I2C clock rate TWCR = 1 << TWEN; // enable twi module, no interrupt } - byte Fastwire::write(byte device, byte address, byte value) { + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { byte twst, retry; retry = 2; @@ -735,6 +794,8 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twst = TWSR & 0xF8; if (twst != TW_START && twst != TW_REP_START) return 2; + //Serial.print(device, HEX); + //Serial.print(" "); TWDR = device & 0xFE; // send device address without read bit (1) TWCR = (1 << TWINT) | (1 << TWEN); if (!waitInt()) return 3; @@ -742,21 +803,39 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } while (twst == TW_MT_SLA_NACK && retry-- > 0); if (twst != TW_MT_SLA_ACK) return 4; + //Serial.print(address, HEX); + //Serial.print(" "); TWDR = address; // send data to the previously addressed device TWCR = (1 << TWINT) | (1 << TWEN); if (!waitInt()) return 5; twst = TWSR & 0xF8; if (twst != TW_MT_DATA_ACK) return 6; - TWDR = value; // send data to the previously addressed device - TWCR = (1 << TWINT) | (1 << TWEN); - if (!waitInt()) return 7; - twst = TWSR & 0xF8; - if (twst != TW_MT_DATA_ACK) return 8; + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); return 0; } + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { byte twst, retry; @@ -767,6 +846,8 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twst = TWSR & 0xF8; if (twst != TW_START && twst != TW_REP_START) return 17; + //Serial.print(device, HEX); + //Serial.print(" "); TWDR = device & 0xfe; // send device address to write TWCR = (1 << TWINT) | (1 << TWEN); if (!waitInt()) return 18; @@ -774,6 +855,8 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } while (twst == TW_MT_SLA_NACK && retry-- > 0); if (twst != TW_MT_SLA_ACK) return 19; + //Serial.print(address, HEX); + //Serial.print(" "); TWDR = address; // send data to the previously addressed device TWCR = (1 << TWINT) | (1 << TWEN); if (!waitInt()) return 20; @@ -789,6 +872,8 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twst = TWSR & 0xF8; if (twst != TW_START && twst != TW_REP_START) return 23; + //Serial.print(device, HEX); + //Serial.print(" "); TWDR = device | 0x01; // send device address with the read bit (1) TWCR = (1 << TWINT) | (1 << TWEN); if (!waitInt()) return 24; @@ -796,19 +881,33 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } while (twst == TW_MR_SLA_NACK && retry-- > 0); if (twst != TW_MR_SLA_ACK) return 25; - for(uint8_t i = 0; i < num; i++) { + for (uint8_t i = 0; i < num; i++) { if (i == num - 1) - TWCR = (1 << TWINT) | (1 << TWEN); + TWCR = (1 << TWINT) | (1 << TWEN); else TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); if (!waitInt()) return 26; twst = TWSR & 0xF8; if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); } + //Serial.print("\n"); + stop(); return 0; - } + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE @@ -818,50 +917,50 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; /* call this version 1.0 - + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer length and index are set *before* the data is actually read. The problem is that these are variables local to the TwoWire object, and by the time we actually have read the - data, and know what the length actually is, we have no simple access to the object's + data, and know what the length actually is, we have no simple access to the object's variables. The actual bytes read *is* given to the callback function, though. - + The ISR code for a slave receiver is commented out. I don't have that setup, and can't verify it at this time. Save it for 2.0! - - The handling of the read and write processes here is much like in the demo sketch code: + + The handling of the read and write processes here is much like in the demo sketch code: the process is broken down into sequential functions, where each registers the next as a callback, essentially. - - For example, for the Read process, twi_read00 just returns if TWI is not yet in a + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a ready state. When there's another interrupt, and the interface *is* ready, then it sets up the read, starts it, and registers twi_read01 as the function to call after the *next* interrupt. twi_read01, then, just returns if the interface is still in a "reading" state. When the reading is done, it copies the information to the buffer, - cleans up, and calls the user-requested callback function with the actual number of + cleans up, and calls the user-requested callback function with the actual number of bytes read. - + The writing is similar. - + Questions, comments and problems can go to Gene@Telobot.com. - + Thumbs Up! Gene Knight - + */ - + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; uint8_t TwoWire::rxBufferIndex = 0; uint8_t TwoWire::rxBufferLength = 0; - + uint8_t TwoWire::txAddress = 0; uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; uint8_t TwoWire::txBufferIndex = 0; uint8_t TwoWire::txBufferLength = 0; - + //uint8_t TwoWire::transmitting = 0; void (*TwoWire::user_onRequest)(void); void (*TwoWire::user_onReceive)(int); - + static volatile uint8_t twi_transmitting; static volatile uint8_t twi_state; static uint8_t twi_slarw; @@ -876,7 +975,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; static volatile uint8_t twi_Done; void (*twi_cbendTransmissionDone)(int); void (*twi_cbreadFromDone)(int); - + void twi_init() { // initialize state twi_state = TWI_READY; @@ -898,7 +997,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register // enable twi module, acks, and twi interrupt - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA); /* TWEN - TWI Enable Bit TWIE - TWI Interrupt Enable @@ -907,7 +1006,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; TWSTA - TWI Start Condition */ } - + typedef struct { uint8_t address; uint8_t* data; @@ -928,13 +1027,13 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twi_Return_Value = bRetVal; fNextInterruptFunction = 0; } - + uint8_t twii_WaitForDone(uint16_t timeout) { uint32_t endMillis = millis() + timeout; while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; return twi_Return_Value; } - + void twii_SetState(uint8_t ucState) { twi_state = ucState; } @@ -967,7 +1066,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } void twii_SetStart() { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTA); } void twi_write01() { @@ -984,8 +1083,8 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); return; } - - + + void twi_write00() { if (TWI_READY != twi_state) return; // blocking test if (TWI_BUFFER_LENGTH < ptwv -> length) { @@ -1002,7 +1101,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; fNextInterruptFunction = twi_write01; // next routine return twi_write01(); } - + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { uint8_t i; ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); @@ -1022,7 +1121,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); return; } - + void twi_read00() { if (TWI_READY != twi_state) return; // blocking test if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return @@ -1050,34 +1149,34 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; void twi_reply(uint8_t ack) { // transmit master read ready signal, with or without ack if (ack){ - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT) | (1 << TWEA); } else { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT); } } - + void twi_stop(void) { // send stop condition - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); - + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTO); + // wait for stop condition to be exectued on bus // TWINT is not set after a stop condition! - while (TWCR & _BV(TWSTO)) { + while (TWCR & (1 << TWSTO)) { continue; } - + // update twi state twi_state = TWI_READY; } void twi_releaseBus(void) { // release bus - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); - + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT); + // update twi state twi_state = TWI_READY; } - + SIGNAL(TWI_vect) { switch (TW_STATUS) { // All Master @@ -1087,7 +1186,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; TWDR = twi_slarw; twi_reply(1); break; - + // Master Transmitter case TW_MT_SLA_ACK: // slave receiver acked address case TW_MT_DATA_ACK: // slave receiver acked data @@ -1115,7 +1214,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twi_error = TW_MT_ARB_LOST; twi_releaseBus(); break; - + // Master Receiver case TW_MR_DATA_ACK: // data received, ack sent // put byte into buffer @@ -1213,7 +1312,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twi_txBufferLength = 1; twi_txBuffer[0] = 0x00; } - + // transmit first byte from buffer, fall through case TW_ST_DATA_ACK: // byte sent, ack returned @@ -1251,26 +1350,26 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } TwoWire::TwoWire() { } - + void TwoWire::begin(void) { rxBufferIndex = 0; rxBufferLength = 0; - + txBufferIndex = 0; txBufferLength = 0; twi_init(); } - + void TwoWire::beginTransmission(uint8_t address) { //beginTransmission((uint8_t)address); // indicate that we are transmitting twi_transmitting = 1; - + // set address of targeted slave txAddress = address; - + // reset tx buffer iterator vars txBufferIndex = 0; txBufferLength = 0; @@ -1297,7 +1396,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; twi_writeTo(txAddress, txBuffer, txBufferLength, 1); return; } - + void TwoWire::send(uint8_t data) { if (twi_transmitting) { // in master transmitter mode @@ -1318,21 +1417,21 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; //twi_transmit(&data, 1); } } - + uint8_t TwoWire::receive(void) { // default to returning null char // for people using with char strings uint8_t value = 0; - + // get each successive byte on each call if (rxBufferIndex < rxBufferLength) { value = rxBuffer[rxBufferIndex]; ++rxBufferIndex; } - + return value; } - + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { // clamp to buffer length if (quantity > NBWIRE_BUFFER_LENGTH) { @@ -1347,10 +1446,10 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; // set rx buffer iterator vars rxBufferIndex = 0; rxBufferLength = read; - + return read; } - + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { // clamp to buffer length if (quantity > NBWIRE_BUFFER_LENGTH) { @@ -1359,7 +1458,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; // perform blocking read into buffer twi_cbreadFromDone = function; - twi_readFrom(address, rxBuffer, quantity); + twi_readFrom(address, rxBuffer, quantity); //uint8_t read = twii_WaitForDone(); // set rx buffer iterator vars @@ -1376,4 +1475,4 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; return rxBufferLength - rxBufferIndex; } -#endif +#endif \ No newline at end of file diff --git a/lib/i2cdev/I2Cdev.h b/lib/i2cdev/I2Cdev.h index 4473649..c4fe7f5 100644 --- a/lib/i2cdev/I2Cdev.h +++ b/lib/i2cdev/I2Cdev.h @@ -1,25 +1,30 @@ // I2Cdev library collection - Main I2C device class header file // Abstracts bit and byte I2C R/W functions into a convenient class -// 6/9/2012 by Jeff Rowberg +// 2013-06-05 by Jeff Rowberg // // Changelog: -// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire -// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation -// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) -// 2011-10-03 - added automatic Arduino version detection for ease of use -// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications -// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) -// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default -// 2011-08-02 - added support for 16-bit registers -// - fixed incorrect Doxygen comments on some methods -// - added timeout value for read operations (thanks mem @ Arduino forums) -// 2011-07-30 - changed read/write function structures to return success or byte counts -// - made all methods static for multi-device memory savings -// 2011-07-28 - initial release +// 2021-09-28 - allow custom Wire object as transaction function argument +// 2020-01-20 - hardija : complete support for Teensy 3.x +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 +// 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications +// 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) +// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire +// - add compiler warnings when using outdated or IDE or limited I2Cdev implementation +// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) +// 2011-10-03 - added automatic Arduino version detection for ease of use +// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications +// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) +// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default +// 2011-08-02 - added support for 16-bit registers +// - fixed incorrect Doxygen comments on some methods +// - added timeout value for read operations (thanks mem @ Arduino forums) +// 2011-07-30 - changed read/write function structures to return success or byte counts +// - made all methods static for multi-device memory savings +// 2011-07-28 - initial release /* ============================================ I2Cdev device library code is placed under the MIT license -Copyright (c) 2012 Jeff Rowberg +Copyright (c) 2013 Jeff Rowberg Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal @@ -44,10 +49,20 @@ THE SOFTWARE. #ifndef _I2CDEV_H_ #define _I2CDEV_H_ +// ----------------------------------------------------------------------------- +// Enable deprecated pgmspace typedefs in avr-libc +// ----------------------------------------------------------------------------- +#define __PROG_TYPES_COMPAT__ + // ----------------------------------------------------------------------------- // I2C interface implementation setting // ----------------------------------------------------------------------------- +#ifndef I2CDEV_IMPLEMENTATION #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_TEENSY_3X_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE +#endif // I2CDEV_IMPLEMENTATION // comment this out if you are using a non-optimal IDE/implementation setting // but want the compiler to shut up about it @@ -60,7 +75,9 @@ THE SOFTWARE. #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project // ^^^ NBWire implementation is still buggy w/some interrupts! #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project - // ^^^ FastWire implementation in I2Cdev is INCOMPLETE! +#define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library +#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire +#define I2CDEV_TEENSY_3X_WIRE 6 // Teensy 3.x support using i2c_t3 library // ----------------------------------------------------------------------------- // Arduino-style "Serial.print" debug constant (uncomment to enable) @@ -75,12 +92,38 @@ THE SOFTWARE. #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include - #ifndef BUFFER_LENGTH - #define BUFFER_LENGTH I2C_BUFFER_LENGTH - #endif #endif -#else - #include "ArduinoWrapper.h" + #if I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY + #include + #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + #include "SBWire.h" + #endif +#endif + +#ifdef SPARK + #include "application.h" + #define ARDUINO 101 + #define BUFFER_LENGTH 32 +#endif + +#ifndef I2CDEVLIB_WIRE_BUFFER_LENGTH + #if defined(I2C_BUFFER_LENGTH) + // Arduino ESP32 core Wire uses this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH + #elif defined(BUFFER_LENGTH) + // Arduino AVR core Wire and many others use this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH BUFFER_LENGTH + #elif defined(SERIAL_BUFFER_SIZE) + // Arduino SAMD core Wire uses this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH SERIAL_BUFFER_SIZE + #else + // should be a safe fallback, though possibly inefficient + #define I2CDEVLIB_WIRE_BUFFER_LENGTH 32 + #endif #endif // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") @@ -89,36 +132,36 @@ THE SOFTWARE. class I2Cdev { public: I2Cdev(); - - static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); - static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); - static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); - static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); - static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); - static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); - static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); - static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); + static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + + static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0); + static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0); + static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0); + static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0); + static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0); + static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0); + static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0); + static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0); static uint16_t readTimeout; }; #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE ////////////////////// - // FastWire 0.2 + // FastWire 0.24 // This is a library to help faster programs to read I2C devices. - // Copyright(C) 2011 + // Copyright(C) 2012 // Francesco Ferrara ////////////////////// - + /* Master */ #define TW_START 0x08 #define TW_REP_START 0x10 @@ -146,8 +189,12 @@ class I2Cdev { public: static void setup(int khz, boolean pullup); - static byte write(byte device, byte address, byte value); + static byte beginTransmission(byte device); + static byte write(byte value); + static byte writeBuf(byte device, byte address, byte *data, byte num); static byte readBuf(byte device, byte address, byte *data, byte num); + static void reset(); + static byte stop(); }; #endif @@ -157,24 +204,24 @@ class I2Cdev { // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html #define NBWIRE_BUFFER_LENGTH 32 - + class TwoWire { private: static uint8_t rxBuffer[]; static uint8_t rxBufferIndex; static uint8_t rxBufferLength; - + static uint8_t txAddress; static uint8_t txBuffer[]; static uint8_t txBufferIndex; static uint8_t txBufferLength; - + // static uint8_t transmitting; static void (*user_onRequest)(void); static void (*user_onReceive)(int); static void onRequestService(void); static void onReceiveService(uint8_t*, int); - + public: TwoWire(); void begin(); @@ -196,26 +243,26 @@ class I2Cdev { void onReceive(void (*)(int)); void onRequest(void (*)(void)); }; - + #define TWI_READY 0 #define TWI_MRX 1 #define TWI_MTX 2 #define TWI_SRX 3 #define TWI_STX 4 - + #define TW_WRITE 0 #define TW_READ 1 - + #define TW_MT_SLA_NACK 0x20 #define TW_MT_DATA_NACK 0x30 - + #define CPU_FREQ 16000000L #define TWI_FREQ 100000L #define TWI_BUFFER_LENGTH 32 - + /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ - - #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) + + #define TW_STATUS_MASK ((1 << TWS7)|(1 << TWS6)|(1 << TWS5)|(1 << TWS4)|(1 << TWS3)) #define TW_STATUS (TWSR & TW_STATUS_MASK) #define TW_START 0x08 #define TW_REP_START 0x10 @@ -245,20 +292,20 @@ class I2Cdev { #define TW_SR_STOP 0xA0 #define TW_NO_INFO 0xF8 #define TW_BUS_ERROR 0x00 - + //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) - + #ifndef sbi // set bit - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= (1 << bit)) #endif // sbi - + #ifndef cbi // clear bit - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit)) #endif // cbi - + extern TwoWire Wire; #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE -#endif /* _I2CDEV_H_ */ +#endif /* _I2CDEV_H_ */ \ No newline at end of file diff --git a/lib/mpu6050/MPU6050.cpp b/lib/mpu6050/MPU6050.cpp index 5e98187..9184e8f 100644 --- a/lib/mpu6050/MPU6050.cpp +++ b/lib/mpu6050/MPU6050.cpp @@ -2760,21 +2760,11 @@ int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflo return 2; } else if (fifoCounter > length) { // If FIFO counter exceeds a size of one packet - read full packets and get the latest packet uint8_t fifoBuf[192] = {0}; - uint8_t bytesCounter = 0; // Count only full packets and read them into buffer fifoCounter = length * (fifoCounter / length); - do { - uint8_t i2cBuf[BUFFER_LENGTH] = {0}; - uint8_t readBytes = min(fifoCounter, (uint16_t)BUFFER_LENGTH); - getFIFOBytes(i2cBuf, readBytes); - for (uint8_t i = 0; i < readBytes; i++) { - fifoBuf[i + bytesCounter] = i2cBuf[i]; - } - fifoCounter -= readBytes; - bytesCounter += readBytes; - } while (fifoCounter); + getFIFOBytes(fifoBuf, fifoCounter); // Read the last packet - for (uint8_t i = 0, start = bytesCounter - length; i < length; i++) { + for (uint8_t i = 0, start = fifoCounter - length; i < length; i++) { data[i] = fifoBuf[start + i]; } return 1; diff --git a/lib/mpu6050/MPU6050.h b/lib/mpu6050/MPU6050.h index c8b6362..0f2009d 100644 --- a/lib/mpu6050/MPU6050.h +++ b/lib/mpu6050/MPU6050.h @@ -46,9 +46,6 @@ THE SOFTWARE. #include #elif defined(ESP32) #include - #ifndef BUFFER_LENGTH - #define BUFFER_LENGTH 32 - #endif #else //#define PROGMEM /* empty */ //#define pgm_read_byte(x) (*(x)) diff --git a/lib/mpu9250/MPU9250.cpp b/lib/mpu9250/MPU9250.cpp index 878506c..eae99ab 100644 --- a/lib/mpu9250/MPU9250.cpp +++ b/lib/mpu9250/MPU9250.cpp @@ -3306,21 +3306,11 @@ int8_t MPU9250_Base::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // ov return 2; } else if (fifoCounter > length) { // If FIFO counter exceeds a size of one packet - read full packets and get the latest packet uint8_t fifoBuf[192] = {0}; - uint8_t bytesCounter = 0; // Count only full packets and read them into buffer fifoCounter = length * (fifoCounter / length); - do { - uint8_t i2cBuf[BUFFER_LENGTH] = {0}; - uint8_t readBytes = min(fifoCounter, (uint16_t)BUFFER_LENGTH); - getFIFOBytes(i2cBuf, readBytes); - for (uint8_t i = 0; i < readBytes; i++) { - fifoBuf[i + bytesCounter] = i2cBuf[i]; - } - fifoCounter -= readBytes; - bytesCounter += readBytes; - } while (fifoCounter); + getFIFOBytes(fifoBuf, fifoCounter); // Read the last packet - for (uint8_t i = 0, start = bytesCounter - length; i < length; i++) { + for (uint8_t i = 0, start = fifoCounter - length; i < length; i++) { data[i] = fifoBuf[start + i]; } return 1;