From 29174bf1f93d0c88394e28fc5219fac87caeebed Mon Sep 17 00:00:00 2001
From: Magnus Persson <magnus@noreply.github.com>
Date: Sat, 1 Jan 2022 17:42:50 +0100
Subject: [PATCH] Swiched to own forks of libs for better version control

---
 lib/mpu6050/I2Cdev.cpp      |  264 ++++----
 lib/mpu6050/I2Cdev.h        |   62 +-
 lib/mpu6050/MPU6050.cpp     | 1152 ++++++++++++++++++-----------------
 lib/mpu6050/MPU6050.h       |  227 +------
 lib/mpu6050/helper_3dmath.h |  216 +++++++
 platformio.ini              |   22 +-
 6 files changed, 1014 insertions(+), 929 deletions(-)
 create mode 100644 lib/mpu6050/helper_3dmath.h

diff --git a/lib/mpu6050/I2Cdev.cpp b/lib/mpu6050/I2Cdev.cpp
index 5525624..892274b 100644
--- a/lib/mpu6050/I2Cdev.cpp
+++ b/lib/mpu6050/I2Cdev.cpp
@@ -3,6 +3,9 @@
 // 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
 //
 // Changelog:
+//      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
@@ -87,11 +90,6 @@ THE SOFTWARE.
 
 #endif
 
-#ifndef BUFFER_LENGTH
-// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations)
-#define BUFFER_LENGTH 32
-#endif
-
 /** Default constructor.
  */
 I2Cdev::I2Cdev() {
@@ -105,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;
 }
@@ -120,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;
 }
@@ -136,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);
@@ -161,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
@@ -169,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);
@@ -185,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.
@@ -196,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.
@@ -208,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);
@@ -223,70 +221,72 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
     uint32_t t1 = millis();
 
     #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((int)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();
+                useWire->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((int)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 (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 (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
-                    data[count] = Wire.read();
+                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();
+                useWire->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((int)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 (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 (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
-                    data[count] = Wire.read();
+                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(" ");
@@ -328,7 +328,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
  * @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 (-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);
@@ -343,28 +343,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
     uint32_t t1 = millis();
 
 #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(" ");
@@ -374,30 +376,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     msb = !msb;
                 }
 
-                Wire.endTransmission();
+                useWire->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,30 +409,30 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     msb = !msb;
                 }
         
-                Wire.endTransmission();
+                useWire->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();
+                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,7 +442,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     msb = !msb;
                 }
         
-                Wire.endTransmission();
+                useWire->endTransmission();
             }
         #endif
 
@@ -479,11 +481,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.
@@ -493,11 +495,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.
@@ -508,7 +510,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
@@ -517,13 +519,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;
     }
@@ -537,7 +539,7 @@ 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
@@ -546,13 +548,13 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint
     // 1010001110010110 original & ~mask
     // 1010101110010110 masked | value
     uint16_t w;
-    if (readWord(devAddr, regAddr, &w) != 0) {
+    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;
     }
@@ -564,8 +566,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.
@@ -574,8 +576,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.
@@ -585,7 +587,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);
@@ -596,14 +598,20 @@ 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
+        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)
-        Wire.beginTransmission(devAddr);
-        Wire.write((uint8_t) regAddr); // send address
+        useWire->beginTransmission(devAddr);
+        useWire->write((uint8_t) regAddr); // send address
     #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
         Fastwire::beginTransmission(devAddr);
         Fastwire::write(regAddr);
@@ -614,21 +622,21 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
             if (i + 1 < length) Serial.print(" ");
         #endif
         #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
-            Wire.send((uint8_t) data[i]);
+            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)
-            Wire.write((uint8_t) data[i]);
+            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();
+        useWire->endTransmission();
     #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \
             || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \
             || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE)
-        status = Wire.endTransmission();
+        status = useWire->endTransmission();
     #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
         Fastwire::stop();
         //status = Fastwire::endTransmission();
@@ -646,7 +654,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);
@@ -657,14 +665,20 @@ 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
+        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)
-        Wire.beginTransmission(devAddr);
-        Wire.write(regAddr); // send address
+        useWire->beginTransmission(devAddr);
+        useWire->write(regAddr); // send address
     #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
         Fastwire::beginTransmission(devAddr);
         Fastwire::write(regAddr);
@@ -675,13 +689,13 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
             if (i + 1 < length) Serial.print(" ");
         #endif
         #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
+            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)
-            Wire.write((uint8_t)(data[i] >> 8));    // send MSB
-            Wire.write((uint8_t)data[i]);         // send LSB
+            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
@@ -689,11 +703,11 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
         #endif
     }
     #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE)
-        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 = Wire.endTransmission();
+        status = useWire->endTransmission();
     #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE)
         Fastwire::stop();
         //status = Fastwire::endTransmission();
@@ -753,7 +767,7 @@ 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
     }
 
@@ -993,7 +1007,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
@@ -1062,7 +1076,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() {
@@ -1145,19 +1159,19 @@ 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;
         }
     
@@ -1167,7 +1181,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
 
     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;
diff --git a/lib/mpu6050/I2Cdev.h b/lib/mpu6050/I2Cdev.h
index 60c3195..5b59c56 100644
--- a/lib/mpu6050/I2Cdev.h
+++ b/lib/mpu6050/I2Cdev.h
@@ -3,6 +3,7 @@
 // 2013-06-05 by Jeff Rowberg <jeff@rowberg.net>
 //
 // Changelog:
+//      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
@@ -48,6 +49,11 @@ THE SOFTWARE.
 #ifndef _I2CDEV_H_
 #define _I2CDEV_H_
 
+// -----------------------------------------------------------------------------
+// Enable deprecated pgmspace typedefs in avr-libc
+// -----------------------------------------------------------------------------
+#define __PROG_TYPES_COMPAT__
+
 // -----------------------------------------------------------------------------
 // I2C interface implementation setting
 // -----------------------------------------------------------------------------
@@ -99,10 +105,26 @@ THE SOFTWARE.
 #endif
 
 #ifdef SPARK
-    #include <spark_wiring_i2c.h>
+    #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];")
 #define I2CDEV_DEFAULT_READ_TIMEOUT     1000
@@ -111,23 +133,23 @@ 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 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);
-        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 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;
 };
@@ -240,7 +262,7 @@ class I2Cdev {
 
     /* 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
@@ -275,11 +297,11 @@ class I2Cdev {
     //#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;
diff --git a/lib/mpu6050/MPU6050.cpp b/lib/mpu6050/MPU6050.cpp
index f8c127d..ecdfe04 100644
--- a/lib/mpu6050/MPU6050.cpp
+++ b/lib/mpu6050/MPU6050.cpp
@@ -4,6 +4,7 @@
 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
 //
 // Changelog:
+//  2021-09-27 - split implementations out of header files, finally
 //  2019-07-08 - Added Auto Calibration routine
 //     ... - ongoing debug release
 
@@ -43,7 +44,7 @@ THE SOFTWARE.
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address):devAddr(address) {
+MPU6050_Base::MPU6050_Base(uint8_t address, void *wireObj):devAddr(address), wireObj(wireObj) {
 }
 
 /** Power on and prepare for general usage.
@@ -53,7 +54,7 @@ MPU6050::MPU6050(uint8_t address):devAddr(address) {
  * the clock source to use the X Gyro for reference, which is slightly better than
  * the default internal clock source.
  */
-void MPU6050::initialize() {
+void MPU6050_Base::initialize() {
     setClockSource(MPU6050_CLOCK_PLL_XGYRO);
     setFullScaleGyroRange(MPU6050_GYRO_FS_250);
     setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
@@ -64,7 +65,7 @@ void MPU6050::initialize() {
  * Make sure the device is connected and responds as expected.
  * @return True if connection is valid, false otherwise
  */
-bool MPU6050::testConnection() {
+bool MPU6050_Base::testConnection() {
     return getDeviceID() == 0x34;
 }
 
@@ -76,8 +77,8 @@ bool MPU6050::testConnection() {
  * the MPU-6000, which does not have a VLOGIC pin.
  * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
  */
-uint8_t MPU6050::getAuxVDDIOLevel() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
+uint8_t MPU6050_Base::getAuxVDDIOLevel() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the auxiliary I2C supply voltage level.
@@ -86,8 +87,8 @@ uint8_t MPU6050::getAuxVDDIOLevel() {
  * the MPU-6000, which does not have a VLOGIC pin.
  * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
  */
-void MPU6050::setAuxVDDIOLevel(uint8_t level) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
+void MPU6050_Base::setAuxVDDIOLevel(uint8_t level) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level, wireObj);
 }
 
 // SMPLRT_DIV register
@@ -113,8 +114,8 @@ void MPU6050::setAuxVDDIOLevel(uint8_t level) {
  * @return Current sample rate
  * @see MPU6050_RA_SMPLRT_DIV
  */
-uint8_t MPU6050::getRate() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
+uint8_t MPU6050_Base::getRate() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set gyroscope sample rate divider.
@@ -122,8 +123,8 @@ uint8_t MPU6050::getRate() {
  * @see getRate()
  * @see MPU6050_RA_SMPLRT_DIV
  */
-void MPU6050::setRate(uint8_t rate) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
+void MPU6050_Base::setRate(uint8_t rate) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate, wireObj);
 }
 
 // CONFIG register
@@ -155,8 +156,8 @@ void MPU6050::setRate(uint8_t rate) {
  *
  * @return FSYNC configuration value
  */
-uint8_t MPU6050::getExternalFrameSync() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
+uint8_t MPU6050_Base::getExternalFrameSync() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set external FSYNC configuration.
@@ -164,8 +165,8 @@ uint8_t MPU6050::getExternalFrameSync() {
  * @see MPU6050_RA_CONFIG
  * @param sync New FSYNC configuration value
  */
-void MPU6050::setExternalFrameSync(uint8_t sync) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
+void MPU6050_Base::setExternalFrameSync(uint8_t sync) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync, wireObj);
 }
 /** Get digital low-pass filter configuration.
  * The DLPF_CFG parameter sets the digital low pass filter configuration. It
@@ -195,8 +196,8 @@ void MPU6050::setExternalFrameSync(uint8_t sync) {
  * @see MPU6050_CFG_DLPF_CFG_BIT
  * @see MPU6050_CFG_DLPF_CFG_LENGTH
  */
-uint8_t MPU6050::getDLPFMode() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
+uint8_t MPU6050_Base::getDLPFMode() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set digital low-pass filter configuration.
@@ -207,8 +208,8 @@ uint8_t MPU6050::getDLPFMode() {
  * @see MPU6050_CFG_DLPF_CFG_BIT
  * @see MPU6050_CFG_DLPF_CFG_LENGTH
  */
-void MPU6050::setDLPFMode(uint8_t mode) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+void MPU6050_Base::setDLPFMode(uint8_t mode) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode, wireObj);
 }
 
 // GYRO_CONFIG register
@@ -230,8 +231,8 @@ void MPU6050::setDLPFMode(uint8_t mode) {
  * @see MPU6050_GCONFIG_FS_SEL_BIT
  * @see MPU6050_GCONFIG_FS_SEL_LENGTH
  */
-uint8_t MPU6050::getFullScaleGyroRange() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer);
+uint8_t MPU6050_Base::getFullScaleGyroRange() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set full-scale gyroscope range.
@@ -242,8 +243,8 @@ uint8_t MPU6050::getFullScaleGyroRange() {
  * @see MPU6050_GCONFIG_FS_SEL_BIT
  * @see MPU6050_GCONFIG_FS_SEL_LENGTH
  */
-void MPU6050::setFullScaleGyroRange(uint8_t range) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+void MPU6050_Base::setFullScaleGyroRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range, wireObj);
 }
 
 // SELF TEST FACTORY TRIM VALUES
@@ -252,9 +253,9 @@ void MPU6050::setFullScaleGyroRange(uint8_t range) {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_X
  */
-uint8_t MPU6050::getAccelXSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]);
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);	
+uint8_t MPU6050_Base::getAccelXSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0], I2Cdev::readTimeout, wireObj);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout, wireObj);	
     return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03);
 }
 
@@ -262,9 +263,9 @@ uint8_t MPU6050::getAccelXSelfTestFactoryTrim() {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_Y
  */
-uint8_t MPU6050::getAccelYSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]);
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);	
+uint8_t MPU6050_Base::getAccelYSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0], I2Cdev::readTimeout, wireObj);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout, wireObj);	
     return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03);
 }
 
@@ -272,8 +273,8 @@ uint8_t MPU6050::getAccelYSelfTestFactoryTrim() {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_Z
  */
-uint8_t MPU6050::getAccelZSelfTestFactoryTrim() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer);	
+uint8_t MPU6050_Base::getAccelZSelfTestFactoryTrim() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer, I2Cdev::readTimeout, wireObj);	
     return (buffer[0]>>3) | (buffer[1] & 0x03);
 }
 
@@ -281,8 +282,8 @@ uint8_t MPU6050::getAccelZSelfTestFactoryTrim() {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_X
  */
-uint8_t MPU6050::getGyroXSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer);	
+uint8_t MPU6050_Base::getGyroXSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer, I2Cdev::readTimeout, wireObj);	
     return (buffer[0] & 0x1F);
 }
 
@@ -290,8 +291,8 @@ uint8_t MPU6050::getGyroXSelfTestFactoryTrim() {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_Y
  */
-uint8_t MPU6050::getGyroYSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer);	
+uint8_t MPU6050_Base::getGyroYSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer, I2Cdev::readTimeout, wireObj);	
     return (buffer[0] & 0x1F);
 }
 
@@ -299,8 +300,8 @@ uint8_t MPU6050::getGyroYSelfTestFactoryTrim() {
  * @return factory trim value
  * @see MPU6050_RA_SELF_TEST_Z
  */
-uint8_t MPU6050::getGyroZSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer);	
+uint8_t MPU6050_Base::getGyroZSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer, I2Cdev::readTimeout, wireObj);	
     return (buffer[0] & 0x1F);
 }
 
@@ -310,46 +311,46 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim() {
  * @return Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-bool MPU6050::getAccelXSelfTest() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
+bool MPU6050_Base::getAccelXSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get self-test enabled setting for accelerometer X axis.
  * @param enabled Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-void MPU6050::setAccelXSelfTest(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
+void MPU6050_Base::setAccelXSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled, wireObj);
 }
 /** Get self-test enabled value for accelerometer Y axis.
  * @return Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-bool MPU6050::getAccelYSelfTest() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
+bool MPU6050_Base::getAccelYSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get self-test enabled value for accelerometer Y axis.
  * @param enabled Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-void MPU6050::setAccelYSelfTest(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
+void MPU6050_Base::setAccelYSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled, wireObj);
 }
 /** Get self-test enabled value for accelerometer Z axis.
  * @return Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-bool MPU6050::getAccelZSelfTest() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
+bool MPU6050_Base::getAccelZSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set self-test enabled value for accelerometer Z axis.
  * @param enabled Self-test enabled value
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-void MPU6050::setAccelZSelfTest(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
+void MPU6050_Base::setAccelZSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled, wireObj);
 }
 /** Get full-scale accelerometer range.
  * The FS_SEL parameter allows setting the full-scale range of the accelerometer
@@ -368,16 +369,16 @@ void MPU6050::setAccelZSelfTest(bool enabled) {
  * @see MPU6050_ACONFIG_AFS_SEL_BIT
  * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
  */
-uint8_t MPU6050::getFullScaleAccelRange() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer);
+uint8_t MPU6050_Base::getFullScaleAccelRange() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set full-scale accelerometer range.
  * @param range New full-scale accelerometer range setting
  * @see getFullScaleAccelRange()
  */
-void MPU6050::setFullScaleAccelRange(uint8_t range) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
+void MPU6050_Base::setFullScaleAccelRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range, wireObj);
 }
 /** Get the high-pass filter configuration.
  * The DHPF is a filter module in the path leading to motion detectors (Free
@@ -414,8 +415,8 @@ void MPU6050::setFullScaleAccelRange(uint8_t range) {
  * @see MPU6050_DHPF_RESET
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-uint8_t MPU6050::getDHPFMode() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer);
+uint8_t MPU6050_Base::getDHPFMode() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the high-pass filter configuration.
@@ -424,8 +425,8 @@ uint8_t MPU6050::getDHPFMode() {
  * @see MPU6050_DHPF_RESET
  * @see MPU6050_RA_ACCEL_CONFIG
  */
-void MPU6050::setDHPFMode(uint8_t bandwidth) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
+void MPU6050_Base::setDHPFMode(uint8_t bandwidth) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth, wireObj);
 }
 
 // FF_THR register
@@ -445,8 +446,8 @@ void MPU6050::setDHPFMode(uint8_t bandwidth) {
  * @return Current free-fall acceleration threshold value (LSB = 2mg)
  * @see MPU6050_RA_FF_THR
  */
-uint8_t MPU6050::getFreefallDetectionThreshold() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer);
+uint8_t MPU6050_Base::getFreefallDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get free-fall event acceleration threshold.
@@ -454,8 +455,8 @@ uint8_t MPU6050::getFreefallDetectionThreshold() {
  * @see getFreefallDetectionThreshold()
  * @see MPU6050_RA_FF_THR
  */
-void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
+void MPU6050_Base::setFreefallDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold, wireObj);
 }
 
 // FF_DUR register
@@ -477,8 +478,8 @@ void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) {
  * @return Current free-fall duration threshold value (LSB = 1ms)
  * @see MPU6050_RA_FF_DUR
  */
-uint8_t MPU6050::getFreefallDetectionDuration() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer);
+uint8_t MPU6050_Base::getFreefallDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get free-fall event duration threshold.
@@ -486,8 +487,8 @@ uint8_t MPU6050::getFreefallDetectionDuration() {
  * @see getFreefallDetectionDuration()
  * @see MPU6050_RA_FF_DUR
  */
-void MPU6050::setFreefallDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
+void MPU6050_Base::setFreefallDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration, wireObj);
 }
 
 // MOT_THR register
@@ -511,8 +512,8 @@ void MPU6050::setFreefallDetectionDuration(uint8_t duration) {
  * @return Current motion detection acceleration threshold value (LSB = 2mg)
  * @see MPU6050_RA_MOT_THR
  */
-uint8_t MPU6050::getMotionDetectionThreshold() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer);
+uint8_t MPU6050_Base::getMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set motion detection event acceleration threshold.
@@ -520,8 +521,8 @@ uint8_t MPU6050::getMotionDetectionThreshold() {
  * @see getMotionDetectionThreshold()
  * @see MPU6050_RA_MOT_THR
  */
-void MPU6050::setMotionDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
+void MPU6050_Base::setMotionDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold, wireObj);
 }
 
 // MOT_DUR register
@@ -541,8 +542,8 @@ void MPU6050::setMotionDetectionThreshold(uint8_t threshold) {
  * @return Current motion detection duration threshold value (LSB = 1ms)
  * @see MPU6050_RA_MOT_DUR
  */
-uint8_t MPU6050::getMotionDetectionDuration() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer);
+uint8_t MPU6050_Base::getMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set motion detection event duration threshold.
@@ -550,8 +551,8 @@ uint8_t MPU6050::getMotionDetectionDuration() {
  * @see getMotionDetectionDuration()
  * @see MPU6050_RA_MOT_DUR
  */
-void MPU6050::setMotionDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
+void MPU6050_Base::setMotionDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration, wireObj);
 }
 
 // ZRMOT_THR register
@@ -581,8 +582,8 @@ void MPU6050::setMotionDetectionDuration(uint8_t duration) {
  * @return Current zero motion detection acceleration threshold value (LSB = 2mg)
  * @see MPU6050_RA_ZRMOT_THR
  */
-uint8_t MPU6050::getZeroMotionDetectionThreshold() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer);
+uint8_t MPU6050_Base::getZeroMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set zero motion detection event acceleration threshold.
@@ -590,8 +591,8 @@ uint8_t MPU6050::getZeroMotionDetectionThreshold() {
  * @see getZeroMotionDetectionThreshold()
  * @see MPU6050_RA_ZRMOT_THR
  */
-void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
+void MPU6050_Base::setZeroMotionDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold, wireObj);
 }
 
 // ZRMOT_DUR register
@@ -612,8 +613,8 @@ void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) {
  * @return Current zero motion detection duration threshold value (LSB = 64ms)
  * @see MPU6050_RA_ZRMOT_DUR
  */
-uint8_t MPU6050::getZeroMotionDetectionDuration() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
+uint8_t MPU6050_Base::getZeroMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set zero motion detection event duration threshold.
@@ -621,8 +622,8 @@ uint8_t MPU6050::getZeroMotionDetectionDuration() {
  * @see getZeroMotionDetectionDuration()
  * @see MPU6050_RA_ZRMOT_DUR
  */
-void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
+void MPU6050_Base::setZeroMotionDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration, wireObj);
 }
 
 // FIFO_EN register
@@ -633,8 +634,8 @@ void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) {
  * @return Current temperature FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getTempFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getTempFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set temperature FIFO enabled value.
@@ -642,8 +643,8 @@ bool MPU6050::getTempFIFOEnabled() {
  * @see getTempFIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setTempFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setTempFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get gyroscope X-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
@@ -651,8 +652,8 @@ void MPU6050::setTempFIFOEnabled(bool enabled) {
  * @return Current gyroscope X-axis FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getXGyroFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getXGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set gyroscope X-axis FIFO enabled value.
@@ -660,8 +661,8 @@ bool MPU6050::getXGyroFIFOEnabled() {
  * @see getXGyroFIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setXGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setXGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get gyroscope Y-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
@@ -669,8 +670,8 @@ void MPU6050::setXGyroFIFOEnabled(bool enabled) {
  * @return Current gyroscope Y-axis FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getYGyroFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getYGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set gyroscope Y-axis FIFO enabled value.
@@ -678,8 +679,8 @@ bool MPU6050::getYGyroFIFOEnabled() {
  * @see getYGyroFIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setYGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setYGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get gyroscope Z-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
@@ -687,8 +688,8 @@ void MPU6050::setYGyroFIFOEnabled(bool enabled) {
  * @return Current gyroscope Z-axis FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getZGyroFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getZGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set gyroscope Z-axis FIFO enabled value.
@@ -696,8 +697,8 @@ bool MPU6050::getZGyroFIFOEnabled() {
  * @see getZGyroFIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setZGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setZGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get accelerometer FIFO enabled value.
  * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
@@ -706,8 +707,8 @@ void MPU6050::setZGyroFIFOEnabled(bool enabled) {
  * @return Current accelerometer FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getAccelFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getAccelFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set accelerometer FIFO enabled value.
@@ -715,8 +716,8 @@ bool MPU6050::getAccelFIFOEnabled() {
  * @see getAccelFIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setAccelFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setAccelFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get Slave 2 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -724,8 +725,8 @@ void MPU6050::setAccelFIFOEnabled(bool enabled) {
  * @return Current Slave 2 FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getSlave2FIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getSlave2FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Slave 2 FIFO enabled value.
@@ -733,8 +734,8 @@ bool MPU6050::getSlave2FIFOEnabled() {
  * @see getSlave2FIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setSlave2FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setSlave2FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get Slave 1 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -742,8 +743,8 @@ void MPU6050::setSlave2FIFOEnabled(bool enabled) {
  * @return Current Slave 1 FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getSlave1FIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getSlave1FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Slave 1 FIFO enabled value.
@@ -751,8 +752,8 @@ bool MPU6050::getSlave1FIFOEnabled() {
  * @see getSlave1FIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setSlave1FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setSlave1FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get Slave 0 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -760,8 +761,8 @@ void MPU6050::setSlave1FIFOEnabled(bool enabled) {
  * @return Current Slave 0 FIFO enabled value
  * @see MPU6050_RA_FIFO_EN
  */
-bool MPU6050::getSlave0FIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getSlave0FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Slave 0 FIFO enabled value.
@@ -769,8 +770,8 @@ bool MPU6050::getSlave0FIFOEnabled() {
  * @see getSlave0FIFOEnabled()
  * @see MPU6050_RA_FIFO_EN
  */
-void MPU6050::setSlave0FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setSlave0FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled, wireObj);
 }
 
 // I2C_MST_CTRL register
@@ -790,8 +791,8 @@ void MPU6050::setSlave0FIFOEnabled(bool enabled) {
  * @return Current multi-master enabled value
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-bool MPU6050::getMultiMasterEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
+bool MPU6050_Base::getMultiMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set multi-master enabled value.
@@ -799,8 +800,8 @@ bool MPU6050::getMultiMasterEnabled() {
  * @see getMultiMasterEnabled()
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-void MPU6050::setMultiMasterEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
+void MPU6050_Base::setMultiMasterEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled, wireObj);
 }
 /** Get wait-for-external-sensor-data enabled value.
  * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
@@ -813,8 +814,8 @@ void MPU6050::setMultiMasterEnabled(bool enabled) {
  * @return Current wait-for-external-sensor-data enabled value
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-bool MPU6050::getWaitForExternalSensorEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
+bool MPU6050_Base::getWaitForExternalSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set wait-for-external-sensor-data enabled value.
@@ -822,8 +823,8 @@ bool MPU6050::getWaitForExternalSensorEnabled() {
  * @see getWaitForExternalSensorEnabled()
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-void MPU6050::setWaitForExternalSensorEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
+void MPU6050_Base::setWaitForExternalSensorEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled, wireObj);
 }
 /** Get Slave 3 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -831,8 +832,8 @@ void MPU6050::setWaitForExternalSensorEnabled(bool enabled) {
  * @return Current Slave 3 FIFO enabled value
  * @see MPU6050_RA_MST_CTRL
  */
-bool MPU6050::getSlave3FIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getSlave3FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Slave 3 FIFO enabled value.
@@ -840,8 +841,8 @@ bool MPU6050::getSlave3FIFOEnabled() {
  * @see getSlave3FIFOEnabled()
  * @see MPU6050_RA_MST_CTRL
  */
-void MPU6050::setSlave3FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setSlave3FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get slave read/write transition enabled value.
  * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
@@ -853,8 +854,8 @@ void MPU6050::setSlave3FIFOEnabled(bool enabled) {
  * @return Current slave read/write transition enabled value
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-bool MPU6050::getSlaveReadWriteTransitionEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
+bool MPU6050_Base::getSlaveReadWriteTransitionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set slave read/write transition enabled value.
@@ -862,8 +863,8 @@ bool MPU6050::getSlaveReadWriteTransitionEnabled() {
  * @see getSlaveReadWriteTransitionEnabled()
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
+void MPU6050_Base::setSlaveReadWriteTransitionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled, wireObj);
 }
 /** Get I2C master clock speed.
  * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
@@ -894,16 +895,16 @@ void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) {
  * @return Current I2C master clock speed
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-uint8_t MPU6050::getMasterClockSpeed() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer);
+uint8_t MPU6050_Base::getMasterClockSpeed() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set I2C master clock speed.
  * @reparam speed Current I2C master clock speed
  * @see MPU6050_RA_I2C_MST_CTRL
  */
-void MPU6050::setMasterClockSpeed(uint8_t speed) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
+void MPU6050_Base::setMasterClockSpeed(uint8_t speed) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed, wireObj);
 }
 
 // I2C_SLV* registers (Slave 0-3)
@@ -949,9 +950,9 @@ void MPU6050::setMasterClockSpeed(uint8_t speed) {
  * @return Current address for specified slave
  * @see MPU6050_RA_I2C_SLV0_ADDR
  */
-uint8_t MPU6050::getSlaveAddress(uint8_t num) {
+uint8_t MPU6050_Base::getSlaveAddress(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer);
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the I2C address of the specified slave (0-3).
@@ -960,9 +961,9 @@ uint8_t MPU6050::getSlaveAddress(uint8_t num) {
  * @see getSlaveAddress()
  * @see MPU6050_RA_I2C_SLV0_ADDR
  */
-void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) {
+void MPU6050_Base::setSlaveAddress(uint8_t num, uint8_t address) {
     if (num > 3) return;
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address);
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address, wireObj);
 }
 /** Get the active internal register for the specified slave (0-3).
  * Read/write operations for this slave will be done to whatever internal
@@ -975,9 +976,9 @@ void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) {
  * @return Current active register for specified slave
  * @see MPU6050_RA_I2C_SLV0_REG
  */
-uint8_t MPU6050::getSlaveRegister(uint8_t num) {
+uint8_t MPU6050_Base::getSlaveRegister(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer);
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the active internal register for the specified slave (0-3).
@@ -986,9 +987,9 @@ uint8_t MPU6050::getSlaveRegister(uint8_t num) {
  * @see getSlaveRegister()
  * @see MPU6050_RA_I2C_SLV0_REG
  */
-void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) {
+void MPU6050_Base::setSlaveRegister(uint8_t num, uint8_t reg) {
     if (num > 3) return;
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg);
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg, wireObj);
 }
 /** Get the enabled value for the specified slave (0-3).
  * When set to 1, this bit enables Slave 0 for data transfer operations. When
@@ -997,9 +998,9 @@ void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) {
  * @return Current enabled value for specified slave
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-bool MPU6050::getSlaveEnabled(uint8_t num) {
+bool MPU6050_Base::getSlaveEnabled(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer);
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the enabled value for the specified slave (0-3).
@@ -1008,9 +1009,9 @@ bool MPU6050::getSlaveEnabled(uint8_t num) {
  * @see getSlaveEnabled()
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) {
+void MPU6050_Base::setSlaveEnabled(uint8_t num, bool enabled) {
     if (num > 3) return;
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled);
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled, wireObj);
 }
 /** Get word pair byte-swapping enabled for the specified slave (0-3).
  * When set to 1, this bit enables byte swapping. When byte swapping is enabled,
@@ -1023,9 +1024,9 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) {
  * @return Current word pair byte-swapping enabled value for specified slave
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-bool MPU6050::getSlaveWordByteSwap(uint8_t num) {
+bool MPU6050_Base::getSlaveWordByteSwap(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set word pair byte-swapping enabled for the specified slave (0-3).
@@ -1034,9 +1035,9 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num) {
  * @see getSlaveWordByteSwap()
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) {
+void MPU6050_Base::setSlaveWordByteSwap(uint8_t num, bool enabled) {
     if (num > 3) return;
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled, wireObj);
 }
 /** Get write mode for the specified slave (0-3).
  * When set to 1, the transaction will read or write data only. When cleared to
@@ -1048,9 +1049,9 @@ void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) {
  * @return Current write mode for specified slave (0 = register address + data, 1 = data only)
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-bool MPU6050::getSlaveWriteMode(uint8_t num) {
+bool MPU6050_Base::getSlaveWriteMode(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set write mode for the specified slave (0-3).
@@ -1059,9 +1060,9 @@ bool MPU6050::getSlaveWriteMode(uint8_t num) {
  * @see getSlaveWriteMode()
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) {
+void MPU6050_Base::setSlaveWriteMode(uint8_t num, bool mode) {
     if (num > 3) return;
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode, wireObj);
 }
 /** Get word pair grouping order offset for the specified slave (0-3).
  * This sets specifies the grouping order of word pairs received from registers.
@@ -1074,9 +1075,9 @@ void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) {
  * @return Current word pair grouping order offset for specified slave
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-bool MPU6050::getSlaveWordGroupOffset(uint8_t num) {
+bool MPU6050_Base::getSlaveWordGroupOffset(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer);
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set word pair grouping order offset for the specified slave (0-3).
@@ -1085,9 +1086,9 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num) {
  * @see getSlaveWordGroupOffset()
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
+void MPU6050_Base::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
     if (num > 3) return;
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled);
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled, wireObj);
 }
 /** Get number of bytes to read for the specified slave (0-3).
  * Specifies the number of bytes transferred to and from Slave 0. Clearing this
@@ -1096,9 +1097,9 @@ void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
  * @return Number of bytes to read for specified slave
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-uint8_t MPU6050::getSlaveDataLength(uint8_t num) {
+uint8_t MPU6050_Base::getSlaveDataLength(uint8_t num) {
     if (num > 3) return 0;
-    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer);
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set number of bytes to read for the specified slave (0-3).
@@ -1107,9 +1108,9 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num) {
  * @see getSlaveDataLength()
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) {
+void MPU6050_Base::setSlaveDataLength(uint8_t num, uint8_t length) {
     if (num > 3) return;
-    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length);
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length, wireObj);
 }
 
 // I2C_SLV* registers (Slave 4)
@@ -1123,8 +1124,8 @@ void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) {
  * @see getSlaveAddress()
  * @see MPU6050_RA_I2C_SLV4_ADDR
  */
-uint8_t MPU6050::getSlave4Address() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
+uint8_t MPU6050_Base::getSlave4Address() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the I2C address of Slave 4.
@@ -1132,8 +1133,8 @@ uint8_t MPU6050::getSlave4Address() {
  * @see getSlave4Address()
  * @see MPU6050_RA_I2C_SLV4_ADDR
  */
-void MPU6050::setSlave4Address(uint8_t address) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address);
+void MPU6050_Base::setSlave4Address(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address, wireObj);
 }
 /** Get the active internal register for the Slave 4.
  * Read/write operations for this slave will be done to whatever internal
@@ -1142,8 +1143,8 @@ void MPU6050::setSlave4Address(uint8_t address) {
  * @return Current active register for Slave 4
  * @see MPU6050_RA_I2C_SLV4_REG
  */
-uint8_t MPU6050::getSlave4Register() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer);
+uint8_t MPU6050_Base::getSlave4Register() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the active internal register for Slave 4.
@@ -1151,8 +1152,8 @@ uint8_t MPU6050::getSlave4Register() {
  * @see getSlave4Register()
  * @see MPU6050_RA_I2C_SLV4_REG
  */
-void MPU6050::setSlave4Register(uint8_t reg) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg);
+void MPU6050_Base::setSlave4Register(uint8_t reg) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg, wireObj);
 }
 /** Set new byte to write to Slave 4.
  * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
@@ -1160,8 +1161,8 @@ void MPU6050::setSlave4Register(uint8_t reg) {
  * @param data New byte to write to Slave 4
  * @see MPU6050_RA_I2C_SLV4_DO
  */
-void MPU6050::setSlave4OutputByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data);
+void MPU6050_Base::setSlave4OutputByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data, wireObj);
 }
 /** Get the enabled value for the Slave 4.
  * When set to 1, this bit enables Slave 4 for data transfer operations. When
@@ -1169,8 +1170,8 @@ void MPU6050::setSlave4OutputByte(uint8_t data) {
  * @return Current enabled value for Slave 4
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-bool MPU6050::getSlave4Enabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
+bool MPU6050_Base::getSlave4Enabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the enabled value for Slave 4.
@@ -1178,8 +1179,8 @@ bool MPU6050::getSlave4Enabled() {
  * @see getSlave4Enabled()
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-void MPU6050::setSlave4Enabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
+void MPU6050_Base::setSlave4Enabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled, wireObj);
 }
 /** Get the enabled value for Slave 4 transaction interrupts.
  * When set to 1, this bit enables the generation of an interrupt signal upon
@@ -1190,8 +1191,8 @@ void MPU6050::setSlave4Enabled(bool enabled) {
  * @return Current enabled value for Slave 4 transaction interrupts.
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-bool MPU6050::getSlave4InterruptEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
+bool MPU6050_Base::getSlave4InterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set the enabled value for Slave 4 transaction interrupts.
@@ -1199,8 +1200,8 @@ bool MPU6050::getSlave4InterruptEnabled() {
  * @see getSlave4InterruptEnabled()
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-void MPU6050::setSlave4InterruptEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
+void MPU6050_Base::setSlave4InterruptEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled, wireObj);
 }
 /** Get write mode for Slave 4.
  * When set to 1, the transaction will read or write data only. When cleared to
@@ -1211,8 +1212,8 @@ void MPU6050::setSlave4InterruptEnabled(bool enabled) {
  * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only)
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-bool MPU6050::getSlave4WriteMode() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
+bool MPU6050_Base::getSlave4WriteMode() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set write mode for the Slave 4.
@@ -1220,8 +1221,8 @@ bool MPU6050::getSlave4WriteMode() {
  * @see getSlave4WriteMode()
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-void MPU6050::setSlave4WriteMode(bool mode) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
+void MPU6050_Base::setSlave4WriteMode(bool mode) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode, wireObj);
 }
 /** Get Slave 4 master delay value.
  * This configures the reduced access rate of I2C slaves relative to the Sample
@@ -1238,8 +1239,8 @@ void MPU6050::setSlave4WriteMode(bool mode) {
  * @return Current Slave 4 master delay value
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-uint8_t MPU6050::getSlave4MasterDelay() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer);
+uint8_t MPU6050_Base::getSlave4MasterDelay() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Slave 4 master delay value.
@@ -1247,8 +1248,8 @@ uint8_t MPU6050::getSlave4MasterDelay() {
  * @see getSlave4MasterDelay()
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
-void MPU6050::setSlave4MasterDelay(uint8_t delay) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
+void MPU6050_Base::setSlave4MasterDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay, wireObj);
 }
 /** Get last available byte read from Slave 4.
  * This register stores the data read from Slave 4. This field is populated
@@ -1256,8 +1257,8 @@ void MPU6050::setSlave4MasterDelay(uint8_t delay) {
  * @return Last available byte read from to Slave 4
  * @see MPU6050_RA_I2C_SLV4_DI
  */
-uint8_t MPU6050::getSlate4InputByte() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
+uint8_t MPU6050_Base::getSlate4InputByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
@@ -1272,8 +1273,8 @@ uint8_t MPU6050::getSlate4InputByte() {
  * @return FSYNC interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getPassthroughStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
+bool MPU6050_Base::getPassthroughStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 4 transaction done status.
@@ -1284,8 +1285,8 @@ bool MPU6050::getPassthroughStatus() {
  * @return Slave 4 transaction done status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave4IsDone() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
+bool MPU6050_Base::getSlave4IsDone() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get master arbitration lost status.
@@ -1295,8 +1296,8 @@ bool MPU6050::getSlave4IsDone() {
  * @return Master arbitration lost status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getLostArbitration() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
+bool MPU6050_Base::getLostArbitration() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 4 NACK status.
@@ -1306,8 +1307,8 @@ bool MPU6050::getLostArbitration() {
  * @return Slave 4 NACK interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave4Nack() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
+bool MPU6050_Base::getSlave4Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 3 NACK status.
@@ -1317,8 +1318,8 @@ bool MPU6050::getSlave4Nack() {
  * @return Slave 3 NACK interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave3Nack() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
+bool MPU6050_Base::getSlave3Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 2 NACK status.
@@ -1328,8 +1329,8 @@ bool MPU6050::getSlave3Nack() {
  * @return Slave 2 NACK interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave2Nack() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
+bool MPU6050_Base::getSlave2Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 1 NACK status.
@@ -1339,8 +1340,8 @@ bool MPU6050::getSlave2Nack() {
  * @return Slave 1 NACK interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave1Nack() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
+bool MPU6050_Base::getSlave1Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Slave 0 NACK status.
@@ -1350,8 +1351,8 @@ bool MPU6050::getSlave1Nack() {
  * @return Slave 0 NACK interrupt status
  * @see MPU6050_RA_I2C_MST_STATUS
  */
-bool MPU6050::getSlave0Nack() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
+bool MPU6050_Base::getSlave0Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
@@ -1363,8 +1364,8 @@ bool MPU6050::getSlave0Nack() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_LEVEL_BIT
  */
-bool MPU6050::getInterruptMode() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
+bool MPU6050_Base::getInterruptMode() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set interrupt logic level mode.
@@ -1373,8 +1374,8 @@ bool MPU6050::getInterruptMode() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_LEVEL_BIT
  */
-void MPU6050::setInterruptMode(bool mode) {
-   I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
+void MPU6050_Base::setInterruptMode(bool mode) {
+   I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode, wireObj);
 }
 /** Get interrupt drive mode.
  * Will be set 0 for push-pull, 1 for open-drain.
@@ -1382,8 +1383,8 @@ void MPU6050::setInterruptMode(bool mode) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_OPEN_BIT
  */
-bool MPU6050::getInterruptDrive() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
+bool MPU6050_Base::getInterruptDrive() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set interrupt drive mode.
@@ -1392,8 +1393,8 @@ bool MPU6050::getInterruptDrive() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_OPEN_BIT
  */
-void MPU6050::setInterruptDrive(bool drive) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
+void MPU6050_Base::setInterruptDrive(bool drive) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive, wireObj);
 }
 /** Get interrupt latch mode.
  * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
@@ -1401,8 +1402,8 @@ void MPU6050::setInterruptDrive(bool drive) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
  */
-bool MPU6050::getInterruptLatch() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
+bool MPU6050_Base::getInterruptLatch() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set interrupt latch mode.
@@ -1411,8 +1412,8 @@ bool MPU6050::getInterruptLatch() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
  */
-void MPU6050::setInterruptLatch(bool latch) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
+void MPU6050_Base::setInterruptLatch(bool latch) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch, wireObj);
 }
 /** Get interrupt latch clear mode.
  * Will be set 0 for status-read-only, 1 for any-register-read.
@@ -1420,8 +1421,8 @@ void MPU6050::setInterruptLatch(bool latch) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
  */
-bool MPU6050::getInterruptLatchClear() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
+bool MPU6050_Base::getInterruptLatchClear() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set interrupt latch clear mode.
@@ -1430,8 +1431,8 @@ bool MPU6050::getInterruptLatchClear() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
  */
-void MPU6050::setInterruptLatchClear(bool clear) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
+void MPU6050_Base::setInterruptLatchClear(bool clear) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear, wireObj);
 }
 /** Get FSYNC interrupt logic level mode.
  * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1439,8 +1440,8 @@ void MPU6050::setInterruptLatchClear(bool clear) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
  */
-bool MPU6050::getFSyncInterruptLevel() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
+bool MPU6050_Base::getFSyncInterruptLevel() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set FSYNC interrupt logic level mode.
@@ -1449,8 +1450,8 @@ bool MPU6050::getFSyncInterruptLevel() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
  */
-void MPU6050::setFSyncInterruptLevel(bool level) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+void MPU6050_Base::setFSyncInterruptLevel(bool level) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level, wireObj);
 }
 /** Get FSYNC pin interrupt enabled setting.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1458,8 +1459,8 @@ void MPU6050::setFSyncInterruptLevel(bool level) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
  */
-bool MPU6050::getFSyncInterruptEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
+bool MPU6050_Base::getFSyncInterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set FSYNC pin interrupt enabled setting.
@@ -1468,8 +1469,8 @@ bool MPU6050::getFSyncInterruptEnabled() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
  */
-void MPU6050::setFSyncInterruptEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
+void MPU6050_Base::setFSyncInterruptEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled, wireObj);
 }
 /** Get I2C bypass enabled status.
  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1482,8 +1483,8 @@ void MPU6050::setFSyncInterruptEnabled(bool enabled) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
  */
-bool MPU6050::getI2CBypassEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
+bool MPU6050_Base::getI2CBypassEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set I2C bypass enabled status.
@@ -1497,8 +1498,8 @@ bool MPU6050::getI2CBypassEnabled() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
  */
-void MPU6050::setI2CBypassEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
+void MPU6050_Base::setI2CBypassEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled, wireObj);
 }
 /** Get reference clock output enabled status.
  * When this bit is equal to 1, a reference clock output is provided at the
@@ -1509,8 +1510,8 @@ void MPU6050::setI2CBypassEnabled(bool enabled) {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
  */
-bool MPU6050::getClockOutputEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
+bool MPU6050_Base::getClockOutputEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set reference clock output enabled status.
@@ -1522,8 +1523,8 @@ bool MPU6050::getClockOutputEnabled() {
  * @see MPU6050_RA_INT_PIN_CFG
  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
  */
-void MPU6050::setClockOutputEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
+void MPU6050_Base::setClockOutputEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled, wireObj);
 }
 
 // INT_ENABLE register
@@ -1535,8 +1536,8 @@ void MPU6050::setClockOutputEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
-uint8_t MPU6050::getIntEnabled() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer);
+uint8_t MPU6050_Base::getIntEnabled() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set full interrupt enabled status.
@@ -1547,8 +1548,8 @@ uint8_t MPU6050::getIntEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
-void MPU6050::setIntEnabled(uint8_t enabled) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
+void MPU6050_Base::setIntEnabled(uint8_t enabled) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled, wireObj);
 }
 /** Get Free Fall interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1556,8 +1557,8 @@ void MPU6050::setIntEnabled(uint8_t enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
-bool MPU6050::getIntFreefallEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
+bool MPU6050_Base::getIntFreefallEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Free Fall interrupt enabled status.
@@ -1566,8 +1567,8 @@ bool MPU6050::getIntFreefallEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
-void MPU6050::setIntFreefallEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
+void MPU6050_Base::setIntFreefallEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled, wireObj);
 }
 /** Get Motion Detection interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1575,8 +1576,8 @@ void MPU6050::setIntFreefallEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_MOT_BIT
  **/
-bool MPU6050::getIntMotionEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
+bool MPU6050_Base::getIntMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Motion Detection interrupt enabled status.
@@ -1585,8 +1586,8 @@ bool MPU6050::getIntMotionEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_MOT_BIT
  **/
-void MPU6050::setIntMotionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
+void MPU6050_Base::setIntMotionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled, wireObj);
 }
 /** Get Zero Motion Detection interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1594,8 +1595,8 @@ void MPU6050::setIntMotionEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  **/
-bool MPU6050::getIntZeroMotionEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+bool MPU6050_Base::getIntZeroMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Zero Motion Detection interrupt enabled status.
@@ -1604,8 +1605,8 @@ bool MPU6050::getIntZeroMotionEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  **/
-void MPU6050::setIntZeroMotionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
+void MPU6050_Base::setIntZeroMotionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled, wireObj);
 }
 /** Get FIFO Buffer Overflow interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1613,8 +1614,8 @@ void MPU6050::setIntZeroMotionEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  **/
-bool MPU6050::getIntFIFOBufferOverflowEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+bool MPU6050_Base::getIntFIFOBufferOverflowEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set FIFO Buffer Overflow interrupt enabled status.
@@ -1623,8 +1624,8 @@ bool MPU6050::getIntFIFOBufferOverflowEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  **/
-void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+void MPU6050_Base::setIntFIFOBufferOverflowEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled, wireObj);
 }
 /** Get I2C Master interrupt enabled status.
  * This enables any of the I2C Master interrupt sources to generate an
@@ -1633,8 +1634,8 @@ void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  **/
-bool MPU6050::getIntI2CMasterEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+bool MPU6050_Base::getIntI2CMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set I2C Master interrupt enabled status.
@@ -1643,8 +1644,8 @@ bool MPU6050::getIntI2CMasterEnabled() {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  **/
-void MPU6050::setIntI2CMasterEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
+void MPU6050_Base::setIntI2CMasterEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled, wireObj);
 }
 /** Get Data Ready interrupt enabled setting.
  * This event occurs each time a write operation to all of the sensor registers
@@ -1653,8 +1654,8 @@ void MPU6050::setIntI2CMasterEnabled(bool enabled) {
  * @see MPU6050_RA_INT_ENABLE
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
-bool MPU6050::getIntDataReadyEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+bool MPU6050_Base::getIntDataReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Data Ready interrupt enabled status.
@@ -1663,8 +1664,8 @@ bool MPU6050::getIntDataReadyEnabled() {
  * @see MPU6050_RA_INT_CFG
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
-void MPU6050::setIntDataReadyEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
+void MPU6050_Base::setIntDataReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled, wireObj);
 }
 
 // INT_STATUS register
@@ -1676,8 +1677,8 @@ void MPU6050::setIntDataReadyEnabled(bool enabled) {
  * @return Current interrupt status
  * @see MPU6050_RA_INT_STATUS
  */
-uint8_t MPU6050::getIntStatus() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer);
+uint8_t MPU6050_Base::getIntStatus() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Free Fall interrupt status.
@@ -1687,8 +1688,8 @@ uint8_t MPU6050::getIntStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_FF_BIT
  */
-bool MPU6050::getIntFreefallStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
+bool MPU6050_Base::getIntFreefallStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Motion Detection interrupt status.
@@ -1698,8 +1699,8 @@ bool MPU6050::getIntFreefallStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_MOT_BIT
  */
-bool MPU6050::getIntMotionStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
+bool MPU6050_Base::getIntMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Zero Motion Detection interrupt status.
@@ -1709,8 +1710,8 @@ bool MPU6050::getIntMotionStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  */
-bool MPU6050::getIntZeroMotionStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+bool MPU6050_Base::getIntZeroMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get FIFO Buffer Overflow interrupt status.
@@ -1720,8 +1721,8 @@ bool MPU6050::getIntZeroMotionStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  */
-bool MPU6050::getIntFIFOBufferOverflowStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+bool MPU6050_Base::getIntFIFOBufferOverflowStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get I2C Master interrupt status.
@@ -1732,8 +1733,8 @@ bool MPU6050::getIntFIFOBufferOverflowStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  */
-bool MPU6050::getIntI2CMasterStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+bool MPU6050_Base::getIntI2CMasterStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Data Ready interrupt status.
@@ -1743,8 +1744,8 @@ bool MPU6050::getIntI2CMasterStatus() {
  * @see MPU6050_RA_INT_STATUS
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
-bool MPU6050::getIntDataReadyStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+bool MPU6050_Base::getIntDataReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
@@ -1766,7 +1767,11 @@ bool MPU6050::getIntDataReadyStatus() {
  * @see getRotation()
  * @see MPU6050_RA_ACCEL_XOUT_H
  */
-void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
+void MPU6050_Base::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
+    (void)mx; // unused parameter
+    (void)my; // unused parameter
+    (void)mz; // unused parameter
+    
     getMotion6(ax, ay, az, gx, gy, gz);
     // TODO: magnetometer integration
 }
@@ -1782,8 +1787,8 @@ void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int
  * @see getRotation()
  * @see MPU6050_RA_ACCEL_XOUT_H
  */
-void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
+void MPU6050_Base::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer, I2Cdev::readTimeout, wireObj);
     *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
     *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
     *az = (((int16_t)buffer[4]) << 8) | buffer[5];
@@ -1816,10 +1821,10 @@ void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int
  * <pre>
  * AFS_SEL | Full Scale Range | LSB Sensitivity
  * --------+------------------+----------------
- * 0       | +/- 2g           | 8192 LSB/mg
- * 1       | +/- 4g           | 4096 LSB/mg
- * 2       | +/- 8g           | 2048 LSB/mg
- * 3       | +/- 16g          | 1024 LSB/mg
+ * 0       | +/- 2g           | 16384 LSB/mg
+ * 1       | +/- 4g           | 8192 LSB/mg
+ * 2       | +/- 8g           | 4096 LSB/mg
+ * 3       | +/- 16g          | 2048 LSB/mg
  * </pre>
  *
  * @param x 16-bit signed integer container for X-axis acceleration
@@ -1827,8 +1832,8 @@ void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int
  * @param z 16-bit signed integer container for Z-axis acceleration
  * @see MPU6050_RA_GYRO_XOUT_H
  */
-void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
+void MPU6050_Base::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer, I2Cdev::readTimeout, wireObj);
     *x = (((int16_t)buffer[0]) << 8) | buffer[1];
     *y = (((int16_t)buffer[2]) << 8) | buffer[3];
     *z = (((int16_t)buffer[4]) << 8) | buffer[5];
@@ -1838,8 +1843,8 @@ void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
  * @see getMotion6()
  * @see MPU6050_RA_ACCEL_XOUT_H
  */
-int16_t MPU6050::getAccelerationX() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
+int16_t MPU6050_Base::getAccelerationX() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Y-axis accelerometer reading.
@@ -1847,8 +1852,8 @@ int16_t MPU6050::getAccelerationX() {
  * @see getMotion6()
  * @see MPU6050_RA_ACCEL_YOUT_H
  */
-int16_t MPU6050::getAccelerationY() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
+int16_t MPU6050_Base::getAccelerationY() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Z-axis accelerometer reading.
@@ -1856,8 +1861,8 @@ int16_t MPU6050::getAccelerationY() {
  * @see getMotion6()
  * @see MPU6050_RA_ACCEL_ZOUT_H
  */
-int16_t MPU6050::getAccelerationZ() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
+int16_t MPU6050_Base::getAccelerationZ() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
@@ -1867,8 +1872,8 @@ int16_t MPU6050::getAccelerationZ() {
  * @return Temperature reading in 16-bit 2's complement format
  * @see MPU6050_RA_TEMP_OUT_H
  */
-int16_t MPU6050::getTemperature() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
+int16_t MPU6050_Base::getTemperature() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
@@ -1906,8 +1911,8 @@ int16_t MPU6050::getTemperature() {
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_XOUT_H
  */
-void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
+void MPU6050_Base::getRotation(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer, I2Cdev::readTimeout, wireObj);
     *x = (((int16_t)buffer[0]) << 8) | buffer[1];
     *y = (((int16_t)buffer[2]) << 8) | buffer[3];
     *z = (((int16_t)buffer[4]) << 8) | buffer[5];
@@ -1917,8 +1922,8 @@ void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) {
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_XOUT_H
  */
-int16_t MPU6050::getRotationX() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
+int16_t MPU6050_Base::getRotationX() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Y-axis gyroscope reading.
@@ -1926,8 +1931,8 @@ int16_t MPU6050::getRotationX() {
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_YOUT_H
  */
-int16_t MPU6050::getRotationY() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
+int16_t MPU6050_Base::getRotationY() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Z-axis gyroscope reading.
@@ -1935,8 +1940,8 @@ int16_t MPU6050::getRotationY() {
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_ZOUT_H
  */
-int16_t MPU6050::getRotationZ() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
+int16_t MPU6050_Base::getRotationZ() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
@@ -2016,8 +2021,8 @@ int16_t MPU6050::getRotationZ() {
  * @param position Starting position (0-23)
  * @return Byte read from register
  */
-uint8_t MPU6050::getExternalSensorByte(int position) {
-    I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
+uint8_t MPU6050_Base::getExternalSensorByte(int position) {
+    I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Read word (2 bytes) from external sensor data registers.
@@ -2025,8 +2030,8 @@ uint8_t MPU6050::getExternalSensorByte(int position) {
  * @return Word read from register
  * @see getExternalSensorByte()
  */
-uint16_t MPU6050::getExternalSensorWord(int position) {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
+uint16_t MPU6050_Base::getExternalSensorWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((uint16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Read double word (4 bytes) from external sensor data registers.
@@ -2034,8 +2039,8 @@ uint16_t MPU6050::getExternalSensorWord(int position) {
  * @return Double word read from registers
  * @see getExternalSensorByte()
  */
-uint32_t MPU6050::getExternalSensorDWord(int position) {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
+uint32_t MPU6050_Base::getExternalSensorDWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer, I2Cdev::readTimeout, wireObj);
     return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
 }
 
@@ -2045,8 +2050,8 @@ uint32_t MPU6050::getExternalSensorDWord(int position) {
  * @return Motion detection status byte
  * @see MPU6050_RA_MOT_DETECT_STATUS
  */
-uint8_t MPU6050::getMotionStatus() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer);
+uint8_t MPU6050_Base::getMotionStatus() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get X-axis negative motion detection interrupt status.
@@ -2054,8 +2059,8 @@ uint8_t MPU6050::getMotionStatus() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_XNEG_BIT
  */
-bool MPU6050::getXNegMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
+bool MPU6050_Base::getXNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get X-axis positive motion detection interrupt status.
@@ -2063,8 +2068,8 @@ bool MPU6050::getXNegMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_XPOS_BIT
  */
-bool MPU6050::getXPosMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
+bool MPU6050_Base::getXPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Y-axis negative motion detection interrupt status.
@@ -2072,8 +2077,8 @@ bool MPU6050::getXPosMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_YNEG_BIT
  */
-bool MPU6050::getYNegMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
+bool MPU6050_Base::getYNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Y-axis positive motion detection interrupt status.
@@ -2081,8 +2086,8 @@ bool MPU6050::getYNegMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_YPOS_BIT
  */
-bool MPU6050::getYPosMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
+bool MPU6050_Base::getYPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Z-axis negative motion detection interrupt status.
@@ -2090,8 +2095,8 @@ bool MPU6050::getYPosMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_ZNEG_BIT
  */
-bool MPU6050::getZNegMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
+bool MPU6050_Base::getZNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get Z-axis positive motion detection interrupt status.
@@ -2099,8 +2104,8 @@ bool MPU6050::getZNegMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_ZPOS_BIT
  */
-bool MPU6050::getZPosMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
+bool MPU6050_Base::getZPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Get zero motion detection interrupt status.
@@ -2108,8 +2113,8 @@ bool MPU6050::getZPosMotionDetected() {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  * @see MPU6050_MOTION_MOT_ZRMOT_BIT
  */
-bool MPU6050::getZeroMotionDetected() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
+bool MPU6050_Base::getZeroMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
@@ -2123,9 +2128,9 @@ bool MPU6050::getZeroMotionDetected() {
  * @param data Byte to write
  * @see MPU6050_RA_I2C_SLV0_DO
  */
-void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) {
+void MPU6050_Base::setSlaveOutputByte(uint8_t num, uint8_t data) {
     if (num > 3) return;
-    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
+    I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data, wireObj);
 }
 
 // I2C_MST_DELAY_CTRL register
@@ -2138,8 +2143,8 @@ void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) {
  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
  */
-bool MPU6050::getExternalShadowDelayEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer);
+bool MPU6050_Base::getExternalShadowDelayEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set external data shadow delay enabled status.
@@ -2148,8 +2153,8 @@ bool MPU6050::getExternalShadowDelayEnabled() {
  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
  */
-void MPU6050::setExternalShadowDelayEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
+void MPU6050_Base::setExternalShadowDelayEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled, wireObj);
 }
 /** Get slave delay enabled status.
  * When a particular slave delay is enabled, the rate of access for the that
@@ -2169,10 +2174,10 @@ void MPU6050::setExternalShadowDelayEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
  */
-bool MPU6050::getSlaveDelayEnabled(uint8_t num) {
+bool MPU6050_Base::getSlaveDelayEnabled(uint8_t num) {
     // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
     if (num > 4) return 0;
-    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set slave delay enabled status.
@@ -2181,8 +2186,8 @@ bool MPU6050::getSlaveDelayEnabled(uint8_t num) {
  * @see MPU6050_RA_I2C_MST_DELAY_CTRL
  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
  */
-void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
+void MPU6050_Base::setSlaveDelayEnabled(uint8_t num, bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled, wireObj);
 }
 
 // SIGNAL_PATH_RESET register
@@ -2193,8 +2198,8 @@ void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) {
  * @see MPU6050_RA_SIGNAL_PATH_RESET
  * @see MPU6050_PATHRESET_GYRO_RESET_BIT
  */
-void MPU6050::resetGyroscopePath() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
+void MPU6050_Base::resetGyroscopePath() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true, wireObj);
 }
 /** Reset accelerometer signal path.
  * The reset will revert the signal path analog to digital converters and
@@ -2202,8 +2207,8 @@ void MPU6050::resetGyroscopePath() {
  * @see MPU6050_RA_SIGNAL_PATH_RESET
  * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
  */
-void MPU6050::resetAccelerometerPath() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
+void MPU6050_Base::resetAccelerometerPath() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true, wireObj);
 }
 /** Reset temperature sensor signal path.
  * The reset will revert the signal path analog to digital converters and
@@ -2211,8 +2216,8 @@ void MPU6050::resetAccelerometerPath() {
  * @see MPU6050_RA_SIGNAL_PATH_RESET
  * @see MPU6050_PATHRESET_TEMP_RESET_BIT
  */
-void MPU6050::resetTemperaturePath() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
+void MPU6050_Base::resetTemperaturePath() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true, wireObj);
 }
 
 // MOT_DETECT_CTRL register
@@ -2231,8 +2236,8 @@ void MPU6050::resetTemperaturePath() {
  * @see MPU6050_RA_MOT_DETECT_CTRL
  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
  */
-uint8_t MPU6050::getAccelerometerPowerOnDelay() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
+uint8_t MPU6050_Base::getAccelerometerPowerOnDelay() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set accelerometer power-on delay.
@@ -2241,8 +2246,8 @@ uint8_t MPU6050::getAccelerometerPowerOnDelay() {
  * @see MPU6050_RA_MOT_DETECT_CTRL
  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
  */
-void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+void MPU6050_Base::setAccelerometerPowerOnDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay, wireObj);
 }
 /** Get Free Fall detection counter decrement configuration.
  * Detection is registered by the Free Fall detection module after accelerometer
@@ -2270,8 +2275,8 @@ void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) {
  * @see MPU6050_RA_MOT_DETECT_CTRL
  * @see MPU6050_DETECT_FF_COUNT_BIT
  */
-uint8_t MPU6050::getFreefallDetectionCounterDecrement() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer);
+uint8_t MPU6050_Base::getFreefallDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Free Fall detection counter decrement configuration.
@@ -2280,8 +2285,8 @@ uint8_t MPU6050::getFreefallDetectionCounterDecrement() {
  * @see MPU6050_RA_MOT_DETECT_CTRL
  * @see MPU6050_DETECT_FF_COUNT_BIT
  */
-void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
+void MPU6050_Base::setFreefallDetectionCounterDecrement(uint8_t decrement) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement, wireObj);
 }
 /** Get Motion detection counter decrement configuration.
  * Detection is registered by the Motion detection module after accelerometer
@@ -2306,8 +2311,8 @@ void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) {
  * please refer to Registers 29 to 32.
  *
  */
-uint8_t MPU6050::getMotionDetectionCounterDecrement() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer);
+uint8_t MPU6050_Base::getMotionDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Motion detection counter decrement configuration.
@@ -2316,8 +2321,8 @@ uint8_t MPU6050::getMotionDetectionCounterDecrement() {
  * @see MPU6050_RA_MOT_DETECT_CTRL
  * @see MPU6050_DETECT_MOT_COUNT_BIT
  */
-void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
+void MPU6050_Base::setMotionDetectionCounterDecrement(uint8_t decrement) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement, wireObj);
 }
 
 // USER_CTRL register
@@ -2330,8 +2335,8 @@ void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_FIFO_EN_BIT
  */
-bool MPU6050::getFIFOEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
+bool MPU6050_Base::getFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set FIFO enabled status.
@@ -2340,8 +2345,8 @@ bool MPU6050::getFIFOEnabled() {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_FIFO_EN_BIT
  */
-void MPU6050::setFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
+void MPU6050_Base::setFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled, wireObj);
 }
 /** Get I2C Master Mode enabled status.
  * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
@@ -2354,8 +2359,8 @@ void MPU6050::setFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
  */
-bool MPU6050::getI2CMasterModeEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
+bool MPU6050_Base::getI2CMasterModeEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set I2C Master Mode enabled status.
@@ -2364,15 +2369,15 @@ bool MPU6050::getI2CMasterModeEnabled() {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
  */
-void MPU6050::setI2CMasterModeEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
+void MPU6050_Base::setI2CMasterModeEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled, wireObj);
 }
 /** Switch from I2C to SPI mode (MPU-6000 only)
  * If this is set, the primary SPI interface will be enabled in place of the
  * disabled primary I2C interface.
  */
-void MPU6050::switchSPIEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+void MPU6050_Base::switchSPIEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled, wireObj);
 }
 /** Reset the FIFO.
  * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
@@ -2380,8 +2385,8 @@ void MPU6050::switchSPIEnabled(bool enabled) {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_FIFO_RESET_BIT
  */
-void MPU6050::resetFIFO() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
+void MPU6050_Base::resetFIFO() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true, wireObj);
 }
 /** Reset the I2C Master.
  * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
@@ -2389,8 +2394,8 @@ void MPU6050::resetFIFO() {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
  */
-void MPU6050::resetI2CMaster() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
+void MPU6050_Base::resetI2CMaster() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true, wireObj);
 }
 /** Reset all sensor registers and signal paths.
  * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
@@ -2404,8 +2409,8 @@ void MPU6050::resetI2CMaster() {
  * @see MPU6050_RA_USER_CTRL
  * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
  */
-void MPU6050::resetSensors() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
+void MPU6050_Base::resetSensors() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true, wireObj);
 }
 
 // PWR_MGMT_1 register
@@ -2415,8 +2420,8 @@ void MPU6050::resetSensors() {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_DEVICE_RESET_BIT
  */
-void MPU6050::reset() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
+void MPU6050_Base::reset() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true, wireObj);
 }
 /** Get sleep mode status.
  * Setting the SLEEP bit in the register puts the device into very low power
@@ -2429,8 +2434,8 @@ void MPU6050::reset() {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_SLEEP_BIT
  */
-bool MPU6050::getSleepEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
+bool MPU6050_Base::getSleepEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set sleep mode status.
@@ -2439,8 +2444,8 @@ bool MPU6050::getSleepEnabled() {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_SLEEP_BIT
  */
-void MPU6050::setSleepEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
+void MPU6050_Base::setSleepEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled, wireObj);
 }
 /** Get wake cycle enabled status.
  * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
@@ -2450,8 +2455,8 @@ void MPU6050::setSleepEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_CYCLE_BIT
  */
-bool MPU6050::getWakeCycleEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
+bool MPU6050_Base::getWakeCycleEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set wake cycle enabled status.
@@ -2460,8 +2465,8 @@ bool MPU6050::getWakeCycleEnabled() {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_CYCLE_BIT
  */
-void MPU6050::setWakeCycleEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
+void MPU6050_Base::setWakeCycleEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled, wireObj);
 }
 /** Get temperature sensor enabled status.
  * Control the usage of the internal temperature sensor.
@@ -2474,8 +2479,8 @@ void MPU6050::setWakeCycleEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_TEMP_DIS_BIT
  */
-bool MPU6050::getTempSensorEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
+bool MPU6050_Base::getTempSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0] == 0; // 1 is actually disabled here
 }
 /** Set temperature sensor enabled status.
@@ -2488,9 +2493,9 @@ bool MPU6050::getTempSensorEnabled() {
  * @see MPU6050_RA_PWR_MGMT_1
  * @see MPU6050_PWR1_TEMP_DIS_BIT
  */
-void MPU6050::setTempSensorEnabled(bool enabled) {
+void MPU6050_Base::setTempSensorEnabled(bool enabled) {
     // 1 is actually disabled here
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled, wireObj);
 }
 /** Get clock source setting.
  * @return Current clock source setting
@@ -2498,8 +2503,8 @@ void MPU6050::setTempSensorEnabled(bool enabled) {
  * @see MPU6050_PWR1_CLKSEL_BIT
  * @see MPU6050_PWR1_CLKSEL_LENGTH
  */
-uint8_t MPU6050::getClockSource() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);
+uint8_t MPU6050_Base::getClockSource() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set clock source setting.
@@ -2532,8 +2537,8 @@ uint8_t MPU6050::getClockSource() {
  * @see MPU6050_PWR1_CLKSEL_BIT
  * @see MPU6050_PWR1_CLKSEL_LENGTH
  */
-void MPU6050::setClockSource(uint8_t source) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
+void MPU6050_Base::setClockSource(uint8_t source) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source, wireObj);
 }
 
 // PWR_MGMT_2 register
@@ -2561,16 +2566,16 @@ void MPU6050::setClockSource(uint8_t source) {
  * @return Current wake frequency
  * @see MPU6050_RA_PWR_MGMT_2
  */
-uint8_t MPU6050::getWakeFrequency() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
+uint8_t MPU6050_Base::getWakeFrequency() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set wake frequency in Accel-Only Low Power Mode.
  * @param frequency New wake frequency
  * @see MPU6050_RA_PWR_MGMT_2
  */
-void MPU6050::setWakeFrequency(uint8_t frequency) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
+void MPU6050_Base::setWakeFrequency(uint8_t frequency) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency, wireObj);
 }
 
 /** Get X-axis accelerometer standby enabled status.
@@ -2579,8 +2584,8 @@ void MPU6050::setWakeFrequency(uint8_t frequency) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_XA_BIT
  */
-bool MPU6050::getStandbyXAccelEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
+bool MPU6050_Base::getStandbyXAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set X-axis accelerometer standby enabled status.
@@ -2589,8 +2594,8 @@ bool MPU6050::getStandbyXAccelEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_XA_BIT
  */
-void MPU6050::setStandbyXAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
+void MPU6050_Base::setStandbyXAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled, wireObj);
 }
 /** Get Y-axis accelerometer standby enabled status.
  * If enabled, the Y-axis will not gather or report data (or use power).
@@ -2598,8 +2603,8 @@ void MPU6050::setStandbyXAccelEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_YA_BIT
  */
-bool MPU6050::getStandbyYAccelEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
+bool MPU6050_Base::getStandbyYAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Y-axis accelerometer standby enabled status.
@@ -2608,8 +2613,8 @@ bool MPU6050::getStandbyYAccelEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_YA_BIT
  */
-void MPU6050::setStandbyYAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
+void MPU6050_Base::setStandbyYAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled, wireObj);
 }
 /** Get Z-axis accelerometer standby enabled status.
  * If enabled, the Z-axis will not gather or report data (or use power).
@@ -2617,8 +2622,8 @@ void MPU6050::setStandbyYAccelEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_ZA_BIT
  */
-bool MPU6050::getStandbyZAccelEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
+bool MPU6050_Base::getStandbyZAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Z-axis accelerometer standby enabled status.
@@ -2627,8 +2632,8 @@ bool MPU6050::getStandbyZAccelEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_ZA_BIT
  */
-void MPU6050::setStandbyZAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
+void MPU6050_Base::setStandbyZAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled, wireObj);
 }
 /** Get X-axis gyroscope standby enabled status.
  * If enabled, the X-axis will not gather or report data (or use power).
@@ -2636,8 +2641,8 @@ void MPU6050::setStandbyZAccelEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_XG_BIT
  */
-bool MPU6050::getStandbyXGyroEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
+bool MPU6050_Base::getStandbyXGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set X-axis gyroscope standby enabled status.
@@ -2646,8 +2651,8 @@ bool MPU6050::getStandbyXGyroEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_XG_BIT
  */
-void MPU6050::setStandbyXGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
+void MPU6050_Base::setStandbyXGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled, wireObj);
 }
 /** Get Y-axis gyroscope standby enabled status.
  * If enabled, the Y-axis will not gather or report data (or use power).
@@ -2655,8 +2660,8 @@ void MPU6050::setStandbyXGyroEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_YG_BIT
  */
-bool MPU6050::getStandbyYGyroEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
+bool MPU6050_Base::getStandbyYGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Y-axis gyroscope standby enabled status.
@@ -2665,8 +2670,8 @@ bool MPU6050::getStandbyYGyroEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_YG_BIT
  */
-void MPU6050::setStandbyYGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
+void MPU6050_Base::setStandbyYGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled, wireObj);
 }
 /** Get Z-axis gyroscope standby enabled status.
  * If enabled, the Z-axis will not gather or report data (or use power).
@@ -2674,8 +2679,8 @@ void MPU6050::setStandbyYGyroEnabled(bool enabled) {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_ZG_BIT
  */
-bool MPU6050::getStandbyZGyroEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
+bool MPU6050_Base::getStandbyZGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Z-axis gyroscope standby enabled status.
@@ -2684,8 +2689,8 @@ bool MPU6050::getStandbyZGyroEnabled() {
  * @see MPU6050_RA_PWR_MGMT_2
  * @see MPU6050_PWR2_STBY_ZG_BIT
  */
-void MPU6050::setStandbyZGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+void MPU6050_Base::setStandbyZGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled, wireObj);
 }
 
 // FIFO_COUNT* registers
@@ -2697,8 +2702,8 @@ void MPU6050::setStandbyZGyroEnabled(bool enabled) {
  * set of sensor data bound to be stored in the FIFO (register 35 and 36).
  * @return Current FIFO buffer size
  */
-uint16_t MPU6050::getFIFOCount() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
+uint16_t MPU6050_Base::getFIFOCount() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((uint16_t)buffer[0]) << 8) | buffer[1];
 }
 
@@ -2729,18 +2734,34 @@ uint16_t MPU6050::getFIFOCount() {
  *
  * @return Byte from FIFO buffer
  */
-uint8_t MPU6050::getFIFOByte() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer);
+uint8_t MPU6050_Base::getFIFOByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
+void MPU6050_Base::getFIFOBytes(uint8_t *data, uint8_t length) {
     if(length > 0){
-        I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
+        I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data, I2Cdev::readTimeout, wireObj);
     } else {
     	*data = 0;
     }
 }
 
+/** Get timeout to get a packet from FIFO buffer.
+ * @return Current timeout to get a packet from FIFO buffer
+ * @see MPU6050_FIFO_DEFAULT_TIMEOUT
+ */
+uint32_t MPU6050_Base::getFIFOTimeout() {
+	return fifoTimeout;
+}
+
+/** Set timeout to get a packet from FIFO buffer.
+ * @param New timeout to get a packet from FIFO buffer
+ * @see MPU6050_FIFO_DEFAULT_TIMEOUT
+ */
+void MPU6050_Base::setFIFOTimeout(uint32_t fifoTimeout) {
+	this->fifoTimeout = fifoTimeout;
+}
+
 /** Get latest byte from FIFO buffer no matter how much time has passed.
  * ===                  GetCurrentFIFOPacket                    ===
  * ================================================================
@@ -2748,24 +2769,25 @@ void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
  *         2) when recovering from overflow
  *         0) when no valid data is available
  * ================================================================ */
- int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
+ int8_t MPU6050_Base::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
      int16_t fifoC;
      // This section of code is for when we allowed more than 1 packet to be acquired
      uint32_t BreakTimer = micros();
+     bool packetReceived = false;
      do {
          if ((fifoC = getFIFOCount())  > length) {
 
              if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
                  resetFIFO(); // Fixes any overflow corruption
                  fifoC = 0;
-                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (11000))); // Get Next New Packet
+                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (getFIFOTimeout()))); // Get Next New Packet
                  } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
-                 uint8_t Trash[BUFFER_LENGTH];
+                 uint8_t Trash[I2CDEVLIB_WIRE_BUFFER_LENGTH];
                  while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
                      fifoC = fifoC - length; // Save the last packet
                      uint16_t  RemoveBytes;
                      while (fifoC) { // fifo count will reach zero so this is safe
-                         RemoveBytes = min((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
+                         RemoveBytes = (fifoC < I2CDEVLIB_WIRE_BUFFER_LENGTH) ? fifoC : I2CDEVLIB_WIRE_BUFFER_LENGTH; // Buffer Length is different than the packet length this will efficiently clear the buffer
                          getFIFOBytes(Trash, (uint8_t)RemoveBytes);
                          fifoC -= RemoveBytes;
                      }
@@ -2774,8 +2796,9 @@ void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
          }
          if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
          // We have 1 packet
-         if ((micros() - BreakTimer) > (11000)) return 0;
-     } while (fifoC != length);
+         packetReceived = fifoC == length;
+         if (!packetReceived && (micros() - BreakTimer) > (getFIFOTimeout())) return 0;
+     } while (!packetReceived);
      getFIFOBytes(data, length); //Get 1 packet
      return 1;
 }
@@ -2785,8 +2808,8 @@ void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
  * @see getFIFOByte()
  * @see MPU6050_RA_FIFO_R_W
  */
-void MPU6050::setFIFOByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
+void MPU6050_Base::setFIFOByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data, wireObj);
 }
 
 // WHO_AM_I register
@@ -2798,8 +2821,8 @@ void MPU6050::setFIFOByte(uint8_t data) {
  * @see MPU6050_WHO_AM_I_BIT
  * @see MPU6050_WHO_AM_I_LENGTH
  */
-uint8_t MPU6050::getDeviceID() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
+uint8_t MPU6050_Base::getDeviceID() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 /** Set Device ID.
@@ -2811,238 +2834,238 @@ uint8_t MPU6050::getDeviceID() {
  * @see MPU6050_WHO_AM_I_BIT
  * @see MPU6050_WHO_AM_I_LENGTH
  */
-void MPU6050::setDeviceID(uint8_t id) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
+void MPU6050_Base::setDeviceID(uint8_t id) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id, wireObj);
 }
 
 // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
 
 // XG_OFFS_TC register
 
-uint8_t MPU6050::getOTPBankValid() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
+uint8_t MPU6050_Base::getOTPBankValid() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setOTPBankValid(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+void MPU6050_Base::setOTPBankValid(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled, wireObj);
 }
-int8_t MPU6050::getXGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+int8_t MPU6050_Base::getXGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setXGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+void MPU6050_Base::setXGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj);
 }
 
 // YG_OFFS_TC register
 
-int8_t MPU6050::getYGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+int8_t MPU6050_Base::getYGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setYGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+void MPU6050_Base::setYGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj);
 }
 
 // ZG_OFFS_TC register
 
-int8_t MPU6050::getZGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+int8_t MPU6050_Base::getZGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setZGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+void MPU6050_Base::setZGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj);
 }
 
 // X_FINE_GAIN register
 
-int8_t MPU6050::getXFineGain() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
+int8_t MPU6050_Base::getXFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setXFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+void MPU6050_Base::setXFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain, wireObj);
 }
 
 // Y_FINE_GAIN register
 
-int8_t MPU6050::getYFineGain() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
+int8_t MPU6050_Base::getYFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setYFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+void MPU6050_Base::setYFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain, wireObj);
 }
 
 // Z_FINE_GAIN register
 
-int8_t MPU6050::getZFineGain() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
+int8_t MPU6050_Base::getZFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setZFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+void MPU6050_Base::setZFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain, wireObj);
 }
 
 // XA_OFFS_* registers
 
-int16_t MPU6050::getXAccelOffset() {
+int16_t MPU6050_Base::getXAccelOffset() {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setXAccelOffset(int16_t offset) {
+void MPU6050_Base::setXAccelOffset(int16_t offset) {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+	I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj);
 }
 
 // YA_OFFS_* register
 
-int16_t MPU6050::getYAccelOffset() {
+int16_t MPU6050_Base::getYAccelOffset() {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setYAccelOffset(int16_t offset) {
+void MPU6050_Base::setYAccelOffset(int16_t offset) {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+	I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj);
 }
 
 // ZA_OFFS_* register
 
-int16_t MPU6050::getZAccelOffset() {
+int16_t MPU6050_Base::getZAccelOffset() {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setZAccelOffset(int16_t offset) {
+void MPU6050_Base::setZAccelOffset(int16_t offset) {
 	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+	I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj);
 }
 
 // XG_OFFS_USR* registers
 
-int16_t MPU6050::getXGyroOffset() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
+int16_t MPU6050_Base::getXGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setXGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+void MPU6050_Base::setXGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset, wireObj);
 }
 
 // YG_OFFS_USR* register
 
-int16_t MPU6050::getYGyroOffset() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
+int16_t MPU6050_Base::getYGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setYGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+void MPU6050_Base::setYGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset, wireObj);
 }
 
 // ZG_OFFS_USR* register
 
-int16_t MPU6050::getZGyroOffset() {
-    I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
+int16_t MPU6050_Base::getZGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj);
     return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setZGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+void MPU6050_Base::setZGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset, wireObj);
 }
 
 // INT_ENABLE register (DMP functions)
 
-bool MPU6050::getIntPLLReadyEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+bool MPU6050_Base::getIntPLLReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setIntPLLReadyEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+void MPU6050_Base::setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled, wireObj);
 }
-bool MPU6050::getIntDMPEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+bool MPU6050_Base::getIntDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setIntDMPEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+void MPU6050_Base::setIntDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled, wireObj);
 }
 
 // DMP_INT_STATUS
 
-bool MPU6050::getDMPInt5Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
+bool MPU6050_Base::getDMPInt5Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getDMPInt4Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
+bool MPU6050_Base::getDMPInt4Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getDMPInt3Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
+bool MPU6050_Base::getDMPInt3Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getDMPInt2Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
+bool MPU6050_Base::getDMPInt2Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getDMPInt1Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
+bool MPU6050_Base::getDMPInt1Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getDMPInt0Status() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
+bool MPU6050_Base::getDMPInt0Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
 // INT_STATUS register (DMP functions)
 
-bool MPU6050::getIntPLLReadyStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+bool MPU6050_Base::getIntPLLReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-bool MPU6050::getIntDMPStatus() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+bool MPU6050_Base::getIntDMPStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
 
 // USER_CTRL register (DMP functions)
 
-bool MPU6050::getDMPEnabled() {
-    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
+bool MPU6050_Base::getDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setDMPEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+void MPU6050_Base::setDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled, wireObj);
 }
-void MPU6050::resetDMP() {
-    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
+void MPU6050_Base::resetDMP() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true, wireObj);
 }
 
 // BANK_SEL register
 
-void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
+void MPU6050_Base::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
     bank &= 0x1F;
     if (userBank) bank |= 0x20;
     if (prefetchEnabled) bank |= 0x40;
-    I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
+    I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank, wireObj);
 }
 
 // MEM_START_ADDR register
 
-void MPU6050::setMemoryStartAddress(uint8_t address) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
+void MPU6050_Base::setMemoryStartAddress(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address, wireObj);
 }
 
 // MEM_R_W register
 
-uint8_t MPU6050::readMemoryByte() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
+uint8_t MPU6050_Base::readMemoryByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::writeMemoryByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
+void MPU6050_Base::writeMemoryByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data, wireObj);
 }
-void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
+void MPU6050_Base::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
     setMemoryBank(bank);
     setMemoryStartAddress(address);
     uint8_t chunkSize;
@@ -3057,7 +3080,7 @@ void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, ui
         if (chunkSize > 256 - address) chunkSize = 256 - address;
 
         // read the chunk of data as specified
-        I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
+        I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i, I2Cdev::readTimeout, wireObj);
         
         // increase byte index by [chunkSize]
         i += chunkSize;
@@ -3073,7 +3096,7 @@ void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, ui
         }
     }
 }
-bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+bool MPU6050_Base::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
     setMemoryBank(bank);
     setMemoryStartAddress(address);
     uint8_t chunkSize;
@@ -3101,13 +3124,13 @@ bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t b
             progBuffer = (uint8_t *)data + i;
         }
 
-        I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+        I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer, wireObj);
 
         // verify data if needed
         if (verify && verifyBuffer) {
             setMemoryBank(bank);
             setMemoryStartAddress(address);
-            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer, I2Cdev::readTimeout, wireObj);
             if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                 /*Serial.print("Block write verification error, bank ");
                 Serial.print(bank, DEC);
@@ -3149,10 +3172,10 @@ bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t b
     if (useProgMem) free(progBuffer);
     return true;
 }
-bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
+bool MPU6050_Base::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
     return writeMemoryBlock(data, dataSize, bank, address, verify, true);
 }
-bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+bool MPU6050_Base::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
     uint8_t *progBuffer = 0;
 	uint8_t success, special;
     uint16_t i, j;
@@ -3211,7 +3234,7 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, b
                 //setIntZeroMotionEnabled(true);
                 //setIntFIFOBufferOverflowEnabled(true);
                 //setIntDMPEnabled(true);
-                I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32);  // single operation
+                I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32, wireObj);  // single operation
 
                 success = true;
             } else {
@@ -3228,28 +3251,28 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, b
     if (useProgMem) free(progBuffer);
     return true;
 }
-bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
+bool MPU6050_Base::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
     return writeDMPConfigurationSet(data, dataSize, true);
 }
 
 // DMP_CFG_1 register
 
-uint8_t MPU6050::getDMPConfig1() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
+uint8_t MPU6050_Base::getDMPConfig1() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setDMPConfig1(uint8_t config) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
+void MPU6050_Base::setDMPConfig1(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config, wireObj);
 }
 
 // DMP_CFG_2 register
 
-uint8_t MPU6050::getDMPConfig2() {
-    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
+uint8_t MPU6050_Base::getDMPConfig2() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer, I2Cdev::readTimeout, wireObj);
     return buffer[0];
 }
-void MPU6050::setDMPConfig2(uint8_t config) {
-    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
+void MPU6050_Base::setDMPConfig2(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config, wireObj);
 }
 
 
@@ -3259,7 +3282,7 @@ void MPU6050::setDMPConfig2(uint8_t config) {
 /**
   @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
 */
-void MPU6050::CalibrateGyro(uint8_t Loops ) {
+void MPU6050_Base::CalibrateGyro(uint8_t Loops ) {
   double kP = 0.3;
   double kI = 90;
   float x;
@@ -3273,7 +3296,7 @@ void MPU6050::CalibrateGyro(uint8_t Loops ) {
 /**
   @brief      Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings
 */
-void MPU6050::CalibrateAccel(uint8_t Loops ) {
+void MPU6050_Base::CalibrateAccel(uint8_t Loops ) {
 
 	float kP = 0.3;
 	float kI = 20;
@@ -3284,7 +3307,7 @@ void MPU6050::CalibrateAccel(uint8_t Loops ) {
 	PID( 0x3B, kP, kI,  Loops);
 }
 
-void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
+void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 	uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13;
 
 	int16_t  Data;
@@ -3293,10 +3316,12 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 	uint8_t shift =(SaveAddress == 0x77)?3:2;
 	float Error, PTerm, ITerm[3];
 	int16_t eSample;
-	uint32_t eSum ;
+	uint32_t eSum;
+	uint16_t gravity = 8192; // prevent uninitialized compiler warning
+	if (ReadAddress == 0x3B) gravity = 16384 >> getFullScaleAccelRange();
 	Serial.write('>');
 	for (int i = 0; i < 3; i++) {
-		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
+		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word)
 		Reading = Data;
 		if(SaveAddress != 0x13){
 			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
@@ -3310,9 +3335,9 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 		for (int c = 0; c < 100; c++) {// 100 PI Calculations
 			eSum = 0;
 			for (int i = 0; i < 3; i++) {
-				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
+				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word)
 				Reading = Data;
-				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384;	//remove Gravity
+				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= gravity;	//remove Gravity
 				Error = -Reading;
 				eSum += abs(Reading);
 				PTerm = kP * Error;
@@ -3321,7 +3346,7 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 					Data = round((PTerm + ITerm[i] ) / 8);		//Compute PID Output
 					Data = ((Data)&0xFFFE) |BitZero[i];			// Insert Bit0 Saved at beginning
 				} else Data = round((PTerm + ITerm[i] ) / 4);	//Compute PID Output
-				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj);
 			}
 			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
 				c = 0;
@@ -3339,33 +3364,32 @@ void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
 				Data = round((ITerm[i] ) / 8);		//Compute PID Output
 				Data = ((Data)&0xFFFE) |BitZero[i];	// Insert Bit0 Saved at beginning
 			} else Data = round((ITerm[i]) / 4);
-			I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+			I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj);
 		}
 	}
 	resetFIFO();
 	resetDMP();
 }
 
-#define printfloatx(Name,Variable,Spaces,Precision,EndTxt) { Serial.print(F(Name)); {char S[(Spaces + Precision + 3)];Serial.print(F(" ")); Serial.print(dtostrf((float)Variable,Spaces,Precision ,S));}Serial.print(F(EndTxt)); }//Name,Variable,Spaces,Precision,EndTxt
-void MPU6050::PrintActiveOffsets() {
+void MPU6050_Base::PrintActiveOffsets() {
 	uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
 	int16_t Data[3];
 	//Serial.print(F("Offset Register 0x"));
 	//Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX);
-	Serial.print(F("\n//           X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro\n//OFFSETS   "));
-	if(AOffsetRegister == 0x06)	I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data);
+	Serial.print(F("\n//         X Accel  Y Accel  Z Accel   X Gyro   Y Gyro   Z Gyro\n// OFFSETS "));
+	if(AOffsetRegister == 0x06)	I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj);
 	else {
-		I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data);
-		I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1);
-		I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2);
+		I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data, I2Cdev::readTimeout, wireObj);
+		I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1, I2Cdev::readTimeout, wireObj);
+		I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2, I2Cdev::readTimeout, wireObj);
 	}
 	//	A_OFFSET_H_READ_A_OFFS(Data);
-	printfloatx("", Data[0], 5, 0, ",  ");
-	printfloatx("", Data[1], 5, 0, ",  ");
-	printfloatx("", Data[2], 5, 0, ",  ");
-	I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data);
+    Serial.print((float)Data[0], 5); Serial.print(",  ");
+    Serial.print((float)Data[1], 5); Serial.print(",  ");
+    Serial.print((float)Data[2], 5); Serial.print(",  ");
+	I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj);
 	//	XG_OFFSET_H_READ_OFFS_USR(Data);
-	printfloatx("", Data[0], 5, 0, ",  ");
-	printfloatx("", Data[1], 5, 0, ",  ");
-	printfloatx("", Data[2], 5, 0, "\n");
+    Serial.print((float)Data[0], 5); Serial.print(",  ");
+    Serial.print((float)Data[1], 5); Serial.print(",  ");
+    Serial.print((float)Data[2], 5); Serial.print("\n");
 }
diff --git a/lib/mpu6050/MPU6050.h b/lib/mpu6050/MPU6050.h
index 4be352d..888d4db 100644
--- a/lib/mpu6050/MPU6050.h
+++ b/lib/mpu6050/MPU6050.h
@@ -4,6 +4,7 @@
 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
 //
 // Changelog:
+//  2021/09/27 - split implementations out of header files, finally
 //     ... - ongoing debug release
 
 // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
@@ -38,12 +39,15 @@ THE SOFTWARE.
 #define _MPU6050_H_
 
 #include "I2Cdev.h"
+#include "helper_3dmath.h"
 
 // supporting link:  http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517
 // also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
 
 #ifdef __AVR__
 #include <avr/pgmspace.h>
+#elif defined(ESP32)
+    #include <pgmspace.h>
 #else
 //#define PROGMEM /* empty */
 //#define pgm_read_byte(x) (*(x))
@@ -431,11 +435,11 @@ THE SOFTWARE.
 #define MPU6050_DMP_MEMORY_BANK_SIZE    256
 #define MPU6050_DMP_MEMORY_CHUNK_SIZE   16
 
-// note: DMP code memory blocks defined at end of header file
+#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000
 
-class MPU6050 {
+class MPU6050_Base {
     public:
-        MPU6050(uint8_t address=MPU6050_DEFAULT_ADDRESS);
+        MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0);
 
         void initialize();
         bool testConnection();
@@ -717,6 +721,8 @@ class MPU6050 {
 		int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length);
         void setFIFOByte(uint8_t data);
         void getFIFOBytes(uint8_t *data, uint8_t length);
+        void setFIFOTimeout(uint32_t fifoTimeout);
+        uint32_t getFIFOTimeout();
 
         // WHO_AM_I register
         uint8_t getDeviceID();
@@ -827,215 +833,16 @@ class MPU6050 {
 		void PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops);  // Does the math
 		void PrintActiveOffsets(); // See the results of the Calibration
 
-
-
-        // special methods for MotionApps 2.0 implementation
-        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
-
-            uint8_t dmpInitialize();
-            bool dmpPacketAvailable();
-
-            uint8_t dmpSetFIFORate(uint8_t fifoRate);
-            uint8_t dmpGetFIFORate();
-            uint8_t dmpGetSampleStepSizeMS();
-            uint8_t dmpGetSampleFrequency();
-            int32_t dmpDecodeTemperature(int8_t tempReg);
-            
-            // Register callbacks after a packet of FIFO data is processed
-            //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
-            //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
-            uint8_t dmpRunFIFORateProcesses();
-            
-            // Setup FIFO for various output
-            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
-            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
-            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
-
-            // Get Fixed Point data from FIFO
-            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
-            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
-            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
-            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
-            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
-            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
-            
-            uint8_t dmpGetEuler(float *data, Quaternion *q);
-            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
-
-            // Get Floating Point data from FIFO
-            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
-
-            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
-            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
-
-            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
-
-            uint8_t dmpInitFIFOParam();
-            uint8_t dmpCloseFIFO();
-            uint8_t dmpSetGyroDataSource(uint8_t source);
-            uint8_t dmpDecodeQuantizedAccel();
-            uint32_t dmpGetGyroSumOfSquare();
-            uint32_t dmpGetAccelSumOfSquare();
-            void dmpOverrideQuaternion(long *q);
-            uint16_t dmpGetFIFOPacketSize();
-            uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof
-        #endif
-
-        // special methods for MotionApps 4.1 implementation
-        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
-
-            uint8_t dmpInitialize();
-            bool dmpPacketAvailable();
-
-            uint8_t dmpSetFIFORate(uint8_t fifoRate);
-            uint8_t dmpGetFIFORate();
-            uint8_t dmpGetSampleStepSizeMS();
-            uint8_t dmpGetSampleFrequency();
-            int32_t dmpDecodeTemperature(int8_t tempReg);
-            
-            // Register callbacks after a packet of FIFO data is processed
-            //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
-            //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
-            uint8_t dmpRunFIFORateProcesses();
-            
-            // Setup FIFO for various output
-            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
-            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
-            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
-
-            // Get Fixed Point data from FIFO
-            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
-            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
-            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
-            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
-            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
-            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
-            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
-            
-            uint8_t dmpGetEuler(float *data, Quaternion *q);
-            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
-
-            // Get Floating Point data from FIFO
-            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
-            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
-
-            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
-            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
-
-            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
-
-            uint8_t dmpInitFIFOParam();
-            uint8_t dmpCloseFIFO();
-            uint8_t dmpSetGyroDataSource(uint8_t source);
-            uint8_t dmpDecodeQuantizedAccel();
-            uint32_t dmpGetGyroSumOfSquare();
-            uint32_t dmpGetAccelSumOfSquare();
-            void dmpOverrideQuaternion(long *q);
-            uint16_t dmpGetFIFOPacketSize();
-        #endif
-
-    private:
+    protected:
         uint8_t devAddr;
+        void *wireObj;
         uint8_t buffer[14];
-    #if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41)
-        uint8_t *dmpPacketBuffer;
-        uint16_t dmpPacketSize;
-    #endif
+        uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
 };
 
+#ifndef I2CDEVLIB_MPU6050_TYPEDEF
+#define I2CDEVLIB_MPU6050_TYPEDEF
+typedef MPU6050_Base MPU6050;
+#endif
+
 #endif /* _MPU6050_H_ */
diff --git a/lib/mpu6050/helper_3dmath.h b/lib/mpu6050/helper_3dmath.h
new file mode 100644
index 0000000..9ed260e
--- /dev/null
+++ b/lib/mpu6050/helper_3dmath.h
@@ -0,0 +1,216 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 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
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+
+class Quaternion {
+    public:
+        float w;
+        float x;
+        float y;
+        float z;
+        
+        Quaternion() {
+            w = 1.0f;
+            x = 0.0f;
+            y = 0.0f;
+            z = 0.0f;
+        }
+        
+        Quaternion(float nw, float nx, float ny, float nz) {
+            w = nw;
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        Quaternion getProduct(Quaternion q) {
+            // Quaternion multiplication is defined by:
+            //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+            //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+            //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+            //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+            return Quaternion(
+                w*q.w - x*q.x - y*q.y - z*q.z,  // new w
+                w*q.x + x*q.w + y*q.z - z*q.y,  // new x
+                w*q.y - x*q.z + y*q.w + z*q.x,  // new y
+                w*q.z + x*q.y - y*q.x + z*q.w); // new z
+        }
+
+        Quaternion getConjugate() {
+            return Quaternion(w, -x, -y, -z);
+        }
+        
+        float getMagnitude() {
+            return sqrt(w*w + x*x + y*y + z*z);
+        }
+        
+        void normalize() {
+            float m = getMagnitude();
+            w /= m;
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        Quaternion getNormalized() {
+            Quaternion r(w, x, y, z);
+            r.normalize();
+            return r;
+        }
+};
+
+class VectorInt16 {
+    public:
+        int16_t x;
+        int16_t y;
+        int16_t z;
+
+        VectorInt16() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorInt16 getNormalized() {
+            VectorInt16 r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            // http://www.cprogramming.com/tutorial/3d/quaternions.html
+            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+            // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+            // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+        
+            // P_out = q * P_in * conj(q)
+            // - P_out is the output vector
+            // - q is the orientation quaternion
+            // - P_in is the input vector (a*aReal)
+            // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorInt16 getRotated(Quaternion *q) {
+            VectorInt16 r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+class VectorFloat {
+    public:
+        float x;
+        float y;
+        float z;
+
+        VectorFloat() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorFloat(float nx, float ny, float nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorFloat getNormalized() {
+            VectorFloat r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorFloat getRotated(Quaternion *q) {
+            VectorFloat r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+#endif /* _HELPER_3DMATH_H_ */
\ No newline at end of file
diff --git a/platformio.ini b/platformio.ini
index 2527a13..9594a3a 100644
--- a/platformio.ini
+++ b/platformio.ini
@@ -19,6 +19,7 @@ platform = espressif8266
 framework = arduino
 board = d1_mini
 build_unflags = 
+0src_build_flags = -Wunused-variable -Wregister -Wchar-subscripts
 build_flags = #-O0 -Wl,-Map,output.map
 	-D BAUD=${common_env_data.monitor_speed}
 	-D ACTIVATE_OTA	
@@ -33,16 +34,17 @@ build_flags = #-O0 -Wl,-Map,output.map
 	-D USER_SSID=\""\""           # =\""myssid\""
 	-D USER_SSID_PWD=\""\""       # =\""mypwd\""	
 	-D CFG_APPVER="\"0.4.0\""
-lib_deps = 
-#	https://github.com/jrowberg/i2cdevlib.git 		# Using local copy of this library
-	https://github.com/codeplea/tinyexpr
-	https://github.com/graphitemaster/incbin 		
-	https://github.com/khoih-prog/ESP_DoubleResetDetector
-	https://github.com/tzapu/WiFiManager
-	https://github.com/thijse/Arduino-Log
-	https://github.com/bblanchon/ArduinoJson
-	https://github.com/PaulStoffregen/OneWire
-	https://github.com/milesburton/Arduino-Temperature-Control-Library
+lib_deps = # Switched to forks for better version control.
+	# Using local copy of this library
+  #https://github.com/mp-se/i2cdevlib																	#	https://github.com/jrowberg/i2cdevlib.git 		
+	https://github.com/mp-se/tinyexpr																		#	https://github.com/codeplea/tinyexpr
+	https://github.com/mp-se/incbin																			#	https://github.com/graphitemaster/incbin
+	https://github.com/mp-se/ESP_DoubleResetDetector										# https://github.com/khoih-prog/ESP_DoubleResetDetector
+	https://github.com/mp-se/WiFiManager																# https://github.com/tzapu/WiFiManager
+	https://github.com/mp-se/Arduino-Log																# https://github.com/thijse/Arduino-Log
+	https://github.com/mp-se/ArduinoJson																# https://github.com/bblanchon/ArduinoJson
+	https://github.com/mp-se/OneWire																		# https://github.com/PaulStoffregen/OneWire
+	https://github.com/mp-se/Arduino-Temperature-Control-Library				# https://github.com/milesburton/Arduino-Temperature-Control-Library
 
 [env:gravity-debug]
 upload_speed = ${common_env_data.upload_speed}