diff --git a/.gitignore b/.gitignore
index 0485ba8..71a7bfc 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,2 +1,5 @@
+# macOS finder files
+**/.DS_Store
 
-\.DS_Store
+# Firmware binaries
+examples/**/build/
\ No newline at end of file
diff --git a/examples/IMU_Zero/Makefile b/examples/IMU_Zero/Makefile
new file mode 100644
index 0000000..e5e6357
--- /dev/null
+++ b/examples/IMU_Zero/Makefile
@@ -0,0 +1,32 @@
+# BOARD_TAG = arduino:mbed_rp2040:pico
+BOARD_TAG = rp2040:rp2040:rpipico
+MONITOR_PORT = /dev/cu.usbmodem11401
+ARDUINO_CLI_PATH := arduino-cli
+
+help:
+	@echo "Usage: make [target] [BOARD=rp2040|esp32|esp32s3]"
+	@echo "Targets:"
+	@echo "  compile: compile the firmware"
+	@echo "  upload: upload the firmware to the board"
+	@echo "  monitor: monitor the serial port"
+	@echo "  clean: clean the cache"
+	@echo "  all: compile, upload, monitor"
+	@echo "Examples:"
+	@echo "  make compile BOARD=rp2040"
+	@echo "  make upload BOARD=rp2040"
+	@echo "  make all BOARD=esp32s3"
+
+compile:
+	$(ARDUINO_CLI_PATH) compile --fqbn $(BOARD_TAG) --export-binaries
+
+upload:
+	@$(ARDUINO_CLI_PATH) upload -p $(MONITOR_PORT) --fqbn $(BOARD_TAG) --verbose
+
+monitor:
+	@$(ARDUINO_CLI_PATH) monitor -p $(MONITOR_PORT) --config baudrate=9600
+	# screen $(MONITOR_PORT) 9600
+
+clean:
+	@$(ARDUINO_CLI_PATH) cache clean
+
+all: compile upload monitor
diff --git a/examples/MPU6050_raw/Makefile b/examples/MPU6050_raw/Makefile
new file mode 100644
index 0000000..4748c58
--- /dev/null
+++ b/examples/MPU6050_raw/Makefile
@@ -0,0 +1,32 @@
+# BOARD_TAG = arduino:mbed_rp2040:pico
+BOARD_TAG = rp2040:rp2040:rpipico
+MONITOR_PORT = /dev/cu.usbmodem11401
+ARDUINO_CLI_PATH := arduino-cli
+
+help:
+	@echo "Usage: make [target] [BOARD=rp2040|esp32|esp32s3]"
+	@echo "Targets:"
+	@echo "  compile: compile the firmware"
+	@echo "  upload: upload the firmware to the board"
+	@echo "  monitor: monitor the serial port"
+	@echo "  clean: clean the cache"
+	@echo "  all: compile, upload, monitor"
+	@echo "Examples:"
+	@echo "  make compile BOARD=rp2040"
+	@echo "  make upload BOARD=rp2040"
+	@echo "  make all BOARD=esp32s3"
+
+compile:
+	$(ARDUINO_CLI_PATH) compile --fqbn $(BOARD_TAG) --export-binaries
+
+upload:
+	@$(ARDUINO_CLI_PATH) upload -p $(MONITOR_PORT) --fqbn $(BOARD_TAG) --verbose
+
+monitor:
+	@$(ARDUINO_CLI_PATH) monitor -p $(MONITOR_PORT) --config baudrate=38400
+	# screen $(MONITOR_PORT) 9600
+
+clean:
+	@$(ARDUINO_CLI_PATH) cache clean
+
+all: compile upload monitor
diff --git a/src/MPU6050.cpp b/src/MPU6050.cpp
index e48342d..98b4a33 100644
--- a/src/MPU6050.cpp
+++ b/src/MPU6050.cpp
@@ -40,6 +40,10 @@ THE SOFTWARE.
 #include "api/deprecated-avr-comp/avr/dtostrf.c.impl"
 #endif
 
+#ifndef DEBUG
+// #define DEBUG 0  // Uncomment to enable debug output
+#endif
+
 #ifndef BUFFER_LENGTH
 // band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations)
 #define BUFFER_LENGTH 32
@@ -51,7 +55,7 @@ THE SOFTWARE.
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address):devAddr(address) {
+MPU6050::MPU6050(uint8_t address) : devAddr(address) {
 }
 
 /** Power on and prepare for general usage.
@@ -62,10 +66,10 @@ MPU6050::MPU6050(uint8_t address):devAddr(address) {
  * the default internal clock source.
  */
 void MPU6050::initialize() {
-    setClockSource( (MPU6050_IMU::MPU6050_CLOCK_PLL_XGYRO));
-    setFullScaleGyroRange( (MPU6050_IMU::MPU6050_GYRO_FS_250));
-    setFullScaleAccelRange( (MPU6050_IMU::MPU6050_ACCEL_FS_2));
-    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
+  setClockSource((MPU6050_IMU::MPU6050_CLOCK_PLL_XGYRO));
+  setFullScaleGyroRange((MPU6050_IMU::MPU6050_GYRO_FS_250));
+  setFullScaleAccelRange((MPU6050_IMU::MPU6050_ACCEL_FS_2));
+  setSleepEnabled(false);  // thanks to Jack Elston for pointing this one out!
 }
 
 /** Verify the I2C connection.
@@ -73,8 +77,8 @@ void MPU6050::initialize() {
  * @return True if connection is valid, false otherwise
  */
 bool MPU6050::testConnection() {
-    uint8_t deviceId = getDeviceID();
-    return (deviceId == 0x34) || (deviceId == 0xC);
+  uint8_t deviceId = getDeviceID();
+  return (deviceId == 0x34) || (deviceId == 0xC);
 }
 
 // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
@@ -86,8 +90,8 @@ bool MPU6050::testConnection() {
  * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
  */
 uint8_t MPU6050::getAuxVDDIOLevel() {
-    I2Cdev::readBit(devAddr,  (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC),  (MPU6050_IMU::MPU6050_TC_PWR_MODE_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_PWR_MODE_BIT), buffer);
+  return buffer[0];
 }
 /** Set the auxiliary I2C supply voltage level.
  * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
@@ -96,7 +100,7 @@ uint8_t MPU6050::getAuxVDDIOLevel() {
  * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
  */
 void MPU6050::setAuxVDDIOLevel(uint8_t level) {
-    I2Cdev::writeBit(devAddr,  (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC),  (MPU6050_IMU::MPU6050_TC_PWR_MODE_BIT), level);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_PWR_MODE_BIT), level);
 }
 
 // SMPLRT_DIV register
@@ -123,8 +127,8 @@ void MPU6050::setAuxVDDIOLevel(uint8_t level) {
  * @see MPU6050_RA_SMPLRT_DIV
  */
 uint8_t MPU6050::getRate() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SMPLRT_DIV), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SMPLRT_DIV), buffer);
+  return buffer[0];
 }
 /** Set gyroscope sample rate divider.
  * @param rate New sample rate divider
@@ -132,7 +136,7 @@ uint8_t MPU6050::getRate() {
  * @see MPU6050_RA_SMPLRT_DIV
  */
 void MPU6050::setRate(uint8_t rate) {
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SMPLRT_DIV), rate);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_SMPLRT_DIV), rate);
 }
 
 // CONFIG register
@@ -165,8 +169,8 @@ void MPU6050::setRate(uint8_t rate) {
  * @return FSYNC configuration value
  */
 uint8_t MPU6050::getExternalFrameSync() {
-    I2Cdev::readBits(devAddr,  (MPU6050_IMU::MPU6050_RA_CONFIG),  (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_BIT), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_CONFIG), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_BIT), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set external FSYNC configuration.
  * @see getExternalFrameSync()
@@ -174,7 +178,7 @@ uint8_t MPU6050::getExternalFrameSync() {
  * @param sync New FSYNC configuration value
  */
 void MPU6050::setExternalFrameSync(uint8_t sync) {
-    I2Cdev::writeBits(devAddr,  (MPU6050_IMU::MPU6050_RA_CONFIG),  (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_BIT), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_LENGTH), sync);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_CONFIG), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_BIT), (MPU6050_IMU::MPU6050_CFG_EXT_SYNC_SET_LENGTH), sync);
 }
 /** Get digital low-pass filter configuration.
  * The DLPF_CFG parameter sets the digital low pass filter configuration. It
@@ -205,8 +209,8 @@ void MPU6050::setExternalFrameSync(uint8_t sync) {
  * @see MPU6050_CFG_DLPF_CFG_LENGTH
  */
 uint8_t MPU6050::getDLPFMode() {
-    I2Cdev::readBits(devAddr,  (MPU6050_IMU::MPU6050_RA_CONFIG),  (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_BIT), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_CONFIG), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_BIT), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set digital low-pass filter configuration.
  * @param mode New DLFP configuration setting
@@ -217,7 +221,7 @@ uint8_t MPU6050::getDLPFMode() {
  * @see MPU6050_CFG_DLPF_CFG_LENGTH
  */
 void MPU6050::setDLPFMode(uint8_t mode) {
-    I2Cdev::writeBits(devAddr,  (MPU6050_IMU::MPU6050_RA_CONFIG),  (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_BIT), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_LENGTH), mode);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_CONFIG), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_BIT), (MPU6050_IMU::MPU6050_CFG_DLPF_CFG_LENGTH), mode);
 }
 
 // GYRO_CONFIG register
@@ -240,8 +244,8 @@ void MPU6050::setDLPFMode(uint8_t mode) {
  * @see (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH)
  */
 uint8_t MPU6050::getFullScaleGyroRange() {
-    I2Cdev::readBits(devAddr,  (MPU6050_IMU::MPU6050_RA_GYRO_CONFIG),  (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_BIT), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_CONFIG), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_BIT), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set full-scale gyroscope range.
  * @param range New full-scale gyroscope range value
@@ -252,7 +256,7 @@ uint8_t MPU6050::getFullScaleGyroRange() {
  * @see (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH)
  */
 void MPU6050::setFullScaleGyroRange(uint8_t range) {
-    I2Cdev::writeBits(devAddr,  (MPU6050_IMU::MPU6050_RA_GYRO_CONFIG),  (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_BIT), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH), range);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_CONFIG), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_BIT), (MPU6050_IMU::MPU6050_GCONFIG_FS_SEL_LENGTH), range);
 }
 
 // SELF TEST FACTORY TRIM VALUES
@@ -262,9 +266,9 @@ void MPU6050::setFullScaleGyroRange(uint8_t range) {
  * @see MPU6050_RA_SELF_TEST_X
  */
 uint8_t MPU6050::getAccelXSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_X), &buffer[0]);
-	I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_A), &buffer[1]);
-    return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_X), &buffer[0]);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_A), &buffer[1]);
+  return (buffer[0] >> 3) | ((buffer[1] >> 4) & 0x03);
 }
 
 /** Get self-test factory trim value for accelerometer Y axis.
@@ -272,9 +276,9 @@ uint8_t MPU6050::getAccelXSelfTestFactoryTrim() {
  * @see MPU6050_RA_SELF_TEST_Y
  */
 uint8_t MPU6050::getAccelYSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_Y), &buffer[0]);
-	I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_A), &buffer[1]);
-    return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_Y), &buffer[0]);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_A), &buffer[1]);
+  return (buffer[0] >> 3) | ((buffer[1] >> 2) & 0x03);
 }
 
 /** Get self-test factory trim value for accelerometer Z axis.
@@ -282,8 +286,8 @@ uint8_t MPU6050::getAccelYSelfTestFactoryTrim() {
  * @see MPU6050_RA_SELF_TEST_Z
  */
 uint8_t MPU6050::getAccelZSelfTestFactoryTrim() {
-    I2Cdev::readBytes(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_Z), 2, buffer);
-    return (buffer[0]>>3) | (buffer[1] & 0x03);
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_Z), 2, buffer);
+  return (buffer[0] >> 3) | (buffer[1] & 0x03);
 }
 
 /** Get self-test factory trim value for gyro X axis.
@@ -291,8 +295,8 @@ uint8_t MPU6050::getAccelZSelfTestFactoryTrim() {
  * @see MPU6050_RA_SELF_TEST_X
  */
 uint8_t MPU6050::getGyroXSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_X), buffer);
-    return (buffer[0] & 0x1F);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_X), buffer);
+  return (buffer[0] & 0x1F);
 }
 
 /** Get self-test factory trim value for gyro Y axis.
@@ -300,8 +304,8 @@ uint8_t MPU6050::getGyroXSelfTestFactoryTrim() {
  * @see MPU6050_RA_SELF_TEST_Y
  */
 uint8_t MPU6050::getGyroYSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_Y), buffer);
-    return (buffer[0] & 0x1F);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_Y), buffer);
+  return (buffer[0] & 0x1F);
 }
 
 /** Get self-test factory trim value for gyro Z axis.
@@ -309,8 +313,8 @@ uint8_t MPU6050::getGyroYSelfTestFactoryTrim() {
  * @see MPU6050_RA_SELF_TEST_Z
  */
 uint8_t MPU6050::getGyroZSelfTestFactoryTrim() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_SELF_TEST_Z), buffer);
-    return (buffer[0] & 0x1F);
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_SELF_TEST_Z), buffer);
+  return (buffer[0] & 0x1F);
 }
 
 // ACCEL_CONFIG register
@@ -320,45 +324,45 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim() {
  * @see MPU6050_RA_ACCEL_CONFIG
  */
 bool MPU6050::getAccelXSelfTest() {
-    I2Cdev::readBit(devAddr,  (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_XA_ST_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_XA_ST_BIT), buffer);
+  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_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_XA_ST_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_XA_ST_BIT), enabled);
 }
 /** 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_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_YA_ST_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_YA_ST_BIT), buffer);
+  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_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_YA_ST_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_YA_ST_BIT), enabled);
 }
 /** 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_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_ZA_ST_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ZA_ST_BIT), buffer);
+  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_IMU::MPU6050_RA_ACCEL_CONFIG),  (MPU6050_IMU::MPU6050_ACONFIG_ZA_ST_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ZA_ST_BIT), enabled);
 }
 /** Get full-scale accelerometer range.
  * The FS_SEL parameter allows setting the full-scale range of the accelerometer
@@ -378,15 +382,15 @@ void MPU6050::setAccelZSelfTest(bool enabled) {
  * @see (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_LENGTH)
  */
 uint8_t MPU6050::getFullScaleAccelRange() {
-    I2Cdev::readBits(devAddr,  (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_BIT), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_BIT), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_LENGTH), buffer);
+  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_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_BIT), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_LENGTH), range);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_BIT), (MPU6050_IMU::MPU6050_ACONFIG_AFS_SEL_LENGTH), range);
 }
 /** Get the high-pass filter configuration.
  * The DHPF is a filter module in the path leading to motion detectors (Free
@@ -424,8 +428,8 @@ void MPU6050::setFullScaleAccelRange(uint8_t range) {
  * @see MPU6050_RA_ACCEL_CONFIG
  */
 uint8_t MPU6050::getDHPFMode() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_BIT), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_BIT), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set the high-pass filter configuration.
  * @param bandwidth New high-pass filter configuration
@@ -434,7 +438,7 @@ uint8_t MPU6050::getDHPFMode() {
  * @see MPU6050_RA_ACCEL_CONFIG
  */
 void MPU6050::setDHPFMode(uint8_t bandwidth) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_BIT), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_LENGTH), bandwidth);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_CONFIG), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_BIT), (MPU6050_IMU::MPU6050_ACONFIG_ACCEL_HPF_LENGTH), bandwidth);
 }
 
 // FF_THR register
@@ -455,8 +459,8 @@ void MPU6050::setDHPFMode(uint8_t bandwidth) {
  * @see MPU6050_RA_FF_THR
  */
 uint8_t MPU6050::getFreefallDetectionThreshold() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_THR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_THR), buffer);
+  return buffer[0];
 }
 /** Get free-fall event acceleration threshold.
  * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
@@ -464,7 +468,7 @@ uint8_t MPU6050::getFreefallDetectionThreshold() {
  * @see MPU6050_RA_FF_THR
  */
 void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_THR), threshold);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_THR), threshold);
 }
 
 // FF_DUR register
@@ -487,8 +491,8 @@ void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) {
  * @see MPU6050_RA_FF_DUR
  */
 uint8_t MPU6050::getFreefallDetectionDuration() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_DUR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_DUR), buffer);
+  return buffer[0];
 }
 /** Get free-fall event duration threshold.
  * @param duration New free-fall duration threshold value (LSB = 1ms)
@@ -496,7 +500,7 @@ uint8_t MPU6050::getFreefallDetectionDuration() {
  * @see MPU6050_RA_FF_DUR
  */
 void MPU6050::setFreefallDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_DUR), duration);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FF_DUR), duration);
 }
 
 // MOT_THR register
@@ -521,8 +525,8 @@ void MPU6050::setFreefallDetectionDuration(uint8_t duration) {
  * @see MPU6050_RA_MOT_THR
  */
 uint8_t MPU6050::getMotionDetectionThreshold() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_THR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_THR), buffer);
+  return buffer[0];
 }
 /** Set motion detection event acceleration threshold.
  * @param threshold New motion detection acceleration threshold value (LSB = 2mg)
@@ -530,7 +534,7 @@ uint8_t MPU6050::getMotionDetectionThreshold() {
  * @see MPU6050_RA_MOT_THR
  */
 void MPU6050::setMotionDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_THR), threshold);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_THR), threshold);
 }
 
 // MOT_DUR register
@@ -551,8 +555,8 @@ void MPU6050::setMotionDetectionThreshold(uint8_t threshold) {
  * @see MPU6050_RA_MOT_DUR
  */
 uint8_t MPU6050::getMotionDetectionDuration() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DUR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DUR), buffer);
+  return buffer[0];
 }
 /** Set motion detection event duration threshold.
  * @param duration New motion detection duration threshold value (LSB = 1ms)
@@ -560,7 +564,7 @@ uint8_t MPU6050::getMotionDetectionDuration() {
  * @see MPU6050_RA_MOT_DUR
  */
 void MPU6050::setMotionDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DUR), duration);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DUR), duration);
 }
 
 // ZRMOT_THR register
@@ -591,8 +595,8 @@ void MPU6050::setMotionDetectionDuration(uint8_t duration) {
  * @see MPU6050_RA_ZRMOT_THR
  */
 uint8_t MPU6050::getZeroMotionDetectionThreshold() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_THR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_THR), buffer);
+  return buffer[0];
 }
 /** Set zero motion detection event acceleration threshold.
  * @param threshold New zero motion detection acceleration threshold value (LSB = 2mg)
@@ -600,7 +604,7 @@ uint8_t MPU6050::getZeroMotionDetectionThreshold() {
  * @see MPU6050_RA_ZRMOT_THR
  */
 void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_THR), threshold);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_THR), threshold);
 }
 
 // ZRMOT_DUR register
@@ -622,8 +626,8 @@ void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) {
  * @see MPU6050_RA_ZRMOT_DUR
  */
 uint8_t MPU6050::getZeroMotionDetectionDuration() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_DUR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_DUR), buffer);
+  return buffer[0];
 }
 /** Set zero motion detection event duration threshold.
  * @param duration New zero motion detection duration threshold value (LSB = 1ms)
@@ -631,7 +635,7 @@ uint8_t MPU6050::getZeroMotionDetectionDuration() {
  * @see MPU6050_RA_ZRMOT_DUR
  */
 void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_DUR), duration);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_ZRMOT_DUR), duration);
 }
 
 // FIFO_EN register
@@ -643,8 +647,8 @@ void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getTempFIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_TEMP_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_TEMP_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set temperature FIFO enabled value.
  * @param enabled New temperature FIFO enabled value
@@ -652,7 +656,7 @@ bool MPU6050::getTempFIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setTempFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_TEMP_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_TEMP_FIFO_EN_BIT), enabled);
 }
 /** Get gyroscope X-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
@@ -661,8 +665,8 @@ void MPU6050::setTempFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getXGyroFIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_XG_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_XG_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set gyroscope X-axis FIFO enabled value.
  * @param enabled New gyroscope X-axis FIFO enabled value
@@ -670,7 +674,7 @@ bool MPU6050::getXGyroFIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setXGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_XG_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_XG_FIFO_EN_BIT), enabled);
 }
 /** Get gyroscope Y-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
@@ -679,8 +683,8 @@ void MPU6050::setXGyroFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getYGyroFIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_YG_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_YG_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set gyroscope Y-axis FIFO enabled value.
  * @param enabled New gyroscope Y-axis FIFO enabled value
@@ -688,7 +692,7 @@ bool MPU6050::getYGyroFIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setYGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_YG_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_YG_FIFO_EN_BIT), enabled);
 }
 /** Get gyroscope Z-axis FIFO enabled value.
  * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
@@ -697,8 +701,8 @@ void MPU6050::setYGyroFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getZGyroFIFOEnabled() {
-    I2Cdev::readBit (devAddr, (MPU6050_IMU:: MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ZG_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ZG_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set gyroscope Z-axis FIFO enabled value.
  * @param enabled New gyroscope Z-axis FIFO enabled value
@@ -706,7 +710,7 @@ bool MPU6050::getZGyroFIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setZGyroFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit (devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ZG_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ZG_FIFO_EN_BIT), enabled);
 }
 /** Get accelerometer FIFO enabled value.
  * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
@@ -716,8 +720,8 @@ void MPU6050::setZGyroFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getAccelFIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ACCEL_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ACCEL_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set accelerometer FIFO enabled value.
  * @param enabled New accelerometer FIFO enabled value
@@ -725,7 +729,7 @@ bool MPU6050::getAccelFIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setAccelFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ACCEL_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_ACCEL_FIFO_EN_BIT), enabled);
 }
 /** Get Slave 2 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -734,8 +738,8 @@ void MPU6050::setAccelFIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getSlave2FIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV2_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV2_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set Slave 2 FIFO enabled value.
  * @param enabled New Slave 2 FIFO enabled value
@@ -743,7 +747,7 @@ bool MPU6050::getSlave2FIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setSlave2FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV2_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV2_FIFO_EN_BIT), enabled);
 }
 /** Get Slave 1 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -752,8 +756,8 @@ void MPU6050::setSlave2FIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getSlave1FIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV1_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV1_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set Slave 1 FIFO enabled value.
  * @param enabled New Slave 1 FIFO enabled value
@@ -761,7 +765,7 @@ bool MPU6050::getSlave1FIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setSlave1FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV1_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV1_FIFO_EN_BIT), enabled);
 }
 /** Get Slave 0 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -770,8 +774,8 @@ void MPU6050::setSlave1FIFOEnabled(bool enabled) {
  * @see MPU6050_RA_FIFO_EN
  */
 bool MPU6050::getSlave0FIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV0_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV0_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set Slave 0 FIFO enabled value.
  * @param enabled New Slave 0 FIFO enabled value
@@ -779,7 +783,7 @@ bool MPU6050::getSlave0FIFOEnabled() {
  * @see MPU6050_RA_FIFO_EN
  */
 void MPU6050::setSlave0FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV0_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_EN), (MPU6050_IMU::MPU6050_SLV0_FIFO_EN_BIT), enabled);
 }
 
 // I2C_MST_CTRL register
@@ -800,8 +804,8 @@ void MPU6050::setSlave0FIFOEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 bool MPU6050::getMultiMasterEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_MULT_MST_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_MULT_MST_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set multi-master enabled value.
  * @param enabled New multi-master enabled value
@@ -809,7 +813,7 @@ bool MPU6050::getMultiMasterEnabled() {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 void MPU6050::setMultiMasterEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_MULT_MST_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_MULT_MST_EN_BIT), enabled);
 }
 /** Get wait-for-external-sensor-data enabled value.
  * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
@@ -823,8 +827,8 @@ void MPU6050::setMultiMasterEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 bool MPU6050::getWaitForExternalSensorEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_WAIT_FOR_ES_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_WAIT_FOR_ES_BIT), buffer);
+  return buffer[0];
 }
 /** Set wait-for-external-sensor-data enabled value.
  * @param enabled New wait-for-external-sensor-data enabled value
@@ -832,7 +836,7 @@ bool MPU6050::getWaitForExternalSensorEnabled() {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 void MPU6050::setWaitForExternalSensorEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_WAIT_FOR_ES_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_WAIT_FOR_ES_BIT), enabled);
 }
 /** Get Slave 3 FIFO enabled value.
  * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
@@ -841,8 +845,8 @@ void MPU6050::setWaitForExternalSensorEnabled(bool enabled) {
  * @see MPU6050_RA_MST_CTRL
  */
 bool MPU6050::getSlave3FIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_SLV_3_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_SLV_3_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set Slave 3 FIFO enabled value.
  * @param enabled New Slave 3 FIFO enabled value
@@ -850,7 +854,7 @@ bool MPU6050::getSlave3FIFOEnabled() {
  * @see MPU6050_RA_MST_CTRL
  */
 void MPU6050::setSlave3FIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_SLV_3_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_SLV_3_FIFO_EN_BIT), enabled);
 }
 /** Get slave read/write transition enabled value.
  * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
@@ -863,8 +867,8 @@ void MPU6050::setSlave3FIFOEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 bool MPU6050::getSlaveReadWriteTransitionEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_P_NSR_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_P_NSR_BIT), buffer);
+  return buffer[0];
 }
 /** Set slave read/write transition enabled value.
  * @param enabled New slave read/write transition enabled value
@@ -872,7 +876,7 @@ bool MPU6050::getSlaveReadWriteTransitionEnabled() {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_P_NSR_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_P_NSR_BIT), enabled);
 }
 /** Get I2C master clock speed.
  * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
@@ -904,15 +908,15 @@ void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_MST_CTRL
  */
 uint8_t MPU6050::getMasterClockSpeed() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_CLK_BIT), (MPU6050_IMU::MPU6050_I2C_MST_CLK_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_CLK_BIT), (MPU6050_IMU::MPU6050_I2C_MST_CLK_LENGTH), buffer);
+  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_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_CLK_BIT), (MPU6050_IMU::MPU6050_I2C_MST_CLK_LENGTH), speed);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_CTRL), (MPU6050_IMU::MPU6050_I2C_MST_CLK_BIT), (MPU6050_IMU::MPU6050_I2C_MST_CLK_LENGTH), speed);
 }
 
 // I2C_SLV* registers (Slave 0-3)
@@ -959,9 +963,9 @@ void MPU6050::setMasterClockSpeed(uint8_t speed) {
  * @see MPU6050_RA_I2C_SLV0_ADDR
  */
 uint8_t MPU6050::getSlaveAddress(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_ADDR) + num*3, buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_ADDR) + num * 3, buffer);
+  return buffer[0];
 }
 /** Set the I2C address of the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -970,8 +974,8 @@ uint8_t MPU6050::getSlaveAddress(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_ADDR
  */
 void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) {
-    if (num > 3) return;
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_ADDR) + num*3, address);
+  if (num > 3) return;
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_ADDR) + num * 3, address);
 }
 /** Get the active internal register for the specified slave (0-3).
  * Read/write operations for this slave will be done to whatever internal
@@ -985,9 +989,9 @@ void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) {
  * @see MPU6050_RA_I2C_SLV0_REG
  */
 uint8_t MPU6050::getSlaveRegister(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_REG) + num*3, buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_REG) + num * 3, buffer);
+  return buffer[0];
 }
 /** Set the active internal register for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -996,8 +1000,8 @@ uint8_t MPU6050::getSlaveRegister(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_REG
  */
 void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) {
-    if (num > 3) return;
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_REG) + num*3, reg);
+  if (num > 3) return;
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_REG) + num * 3, reg);
 }
 /** Get the enabled value for the specified slave (0-3).
  * When set to 1, this bit enables Slave 0 for data transfer operations. When
@@ -1007,9 +1011,9 @@ void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 bool MPU6050::getSlaveEnabled(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_EN_BIT), buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set the enabled value for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1018,8 +1022,8 @@ bool MPU6050::getSlaveEnabled(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) {
-    if (num > 3) return;
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_EN_BIT), enabled);
+  if (num > 3) return;
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_EN_BIT), enabled);
 }
 /** 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,
@@ -1033,9 +1037,9 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 bool MPU6050::getSlaveWordByteSwap(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_BYTE_SW_BIT), buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_BYTE_SW_BIT), buffer);
+  return buffer[0];
 }
 /** Set word pair byte-swapping enabled for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1044,8 +1048,8 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) {
-    if (num > 3) return;
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_BYTE_SW_BIT), enabled);
+  if (num > 3) return;
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_BYTE_SW_BIT), enabled);
 }
 /** Get write mode for the specified slave (0-3).
  * When set to 1, the transaction will read or write data only. When cleared to
@@ -1058,9 +1062,9 @@ void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 bool MPU6050::getSlaveWriteMode(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_REG_DIS_BIT), buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_REG_DIS_BIT), buffer);
+  return buffer[0];
 }
 /** Set write mode for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1069,8 +1073,8 @@ bool MPU6050::getSlaveWriteMode(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) {
-    if (num > 3) return;
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_REG_DIS_BIT), mode);
+  if (num > 3) return;
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_REG_DIS_BIT), mode);
 }
 /** Get word pair grouping order offset for the specified slave (0-3).
  * This sets specifies the grouping order of word pairs received from registers.
@@ -1084,9 +1088,9 @@ void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 bool MPU6050::getSlaveWordGroupOffset(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_GRP_BIT), buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_GRP_BIT), buffer);
+  return buffer[0];
 }
 /** Set word pair grouping order offset for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1095,8 +1099,8 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
-    if (num > 3) return;
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_GRP_BIT), enabled);
+  if (num > 3) return;
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_GRP_BIT), enabled);
 }
 /** 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
@@ -1106,9 +1110,9 @@ void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 uint8_t MPU6050::getSlaveDataLength(uint8_t num) {
-    if (num > 3) return 0;
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_LEN_BIT), (MPU6050_IMU::MPU6050_I2C_SLV_LEN_LENGTH), buffer);
-    return buffer[0];
+  if (num > 3) return 0;
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_LEN_BIT), (MPU6050_IMU::MPU6050_I2C_SLV_LEN_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set number of bytes to read for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1117,8 +1121,8 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num) {
  * @see MPU6050_RA_I2C_SLV0_CTRL
  */
 void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) {
-    if (num > 3) return;
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num*3, (MPU6050_IMU::MPU6050_I2C_SLV_LEN_BIT), (MPU6050_IMU::MPU6050_I2C_SLV_LEN_LENGTH), length);
+  if (num > 3) return;
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_CTRL) + num * 3, (MPU6050_IMU::MPU6050_I2C_SLV_LEN_BIT), (MPU6050_IMU::MPU6050_I2C_SLV_LEN_LENGTH), length);
 }
 
 // I2C_SLV* registers (Slave 4)
@@ -1133,8 +1137,8 @@ void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) {
  * @see MPU6050_RA_I2C_SLV4_ADDR
  */
 uint8_t MPU6050::getSlave4Address() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_ADDR), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_ADDR), buffer);
+  return buffer[0];
 }
 /** Set the I2C address of Slave 4.
  * @param address New address for Slave 4
@@ -1142,7 +1146,7 @@ uint8_t MPU6050::getSlave4Address() {
  * @see MPU6050_RA_I2C_SLV4_ADDR
  */
 void MPU6050::setSlave4Address(uint8_t address) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_ADDR), address);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_ADDR), address);
 }
 /** Get the active internal register for the Slave 4.
  * Read/write operations for this slave will be done to whatever internal
@@ -1152,8 +1156,8 @@ void MPU6050::setSlave4Address(uint8_t address) {
  * @see MPU6050_RA_I2C_SLV4_REG
  */
 uint8_t MPU6050::getSlave4Register() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_REG), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_REG), buffer);
+  return buffer[0];
 }
 /** Set the active internal register for Slave 4.
  * @param reg New active register for Slave 4
@@ -1161,7 +1165,7 @@ uint8_t MPU6050::getSlave4Register() {
  * @see MPU6050_RA_I2C_SLV4_REG
  */
 void MPU6050::setSlave4Register(uint8_t reg) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_REG), reg);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_REG), reg);
 }
 /** Set new byte to write to Slave 4.
  * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
@@ -1170,7 +1174,7 @@ void MPU6050::setSlave4Register(uint8_t reg) {
  * @see MPU6050_RA_I2C_SLV4_DO
  */
 void MPU6050::setSlave4OutputByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_DO), data);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_DO), data);
 }
 /** Get the enabled value for the Slave 4.
  * When set to 1, this bit enables Slave 4 for data transfer operations. When
@@ -1179,8 +1183,8 @@ void MPU6050::setSlave4OutputByte(uint8_t data) {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 bool MPU6050::getSlave4Enabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set the enabled value for Slave 4.
  * @param enabled New enabled value for Slave 4
@@ -1188,7 +1192,7 @@ bool MPU6050::getSlave4Enabled() {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 void MPU6050::setSlave4Enabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_EN_BIT), enabled);
 }
 /** Get the enabled value for Slave 4 transaction interrupts.
  * When set to 1, this bit enables the generation of an interrupt signal upon
@@ -1200,8 +1204,8 @@ void MPU6050::setSlave4Enabled(bool enabled) {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 bool MPU6050::getSlave4InterruptEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_INT_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_INT_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set the enabled value for Slave 4 transaction interrupts.
  * @param enabled New enabled value for Slave 4 transaction interrupts.
@@ -1209,7 +1213,7 @@ bool MPU6050::getSlave4InterruptEnabled() {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 void MPU6050::setSlave4InterruptEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_INT_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_INT_EN_BIT), enabled);
 }
 /** Get write mode for Slave 4.
  * When set to 1, the transaction will read or write data only. When cleared to
@@ -1221,8 +1225,8 @@ void MPU6050::setSlave4InterruptEnabled(bool enabled) {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 bool MPU6050::getSlave4WriteMode() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_REG_DIS_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_REG_DIS_BIT), buffer);
+  return buffer[0];
 }
 /** Set write mode for the Slave 4.
  * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data only)
@@ -1230,7 +1234,7 @@ bool MPU6050::getSlave4WriteMode() {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 void MPU6050::setSlave4WriteMode(bool mode) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_REG_DIS_BIT), mode);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_REG_DIS_BIT), mode);
 }
 /** Get Slave 4 master delay value.
  * This configures the reduced access rate of I2C slaves relative to the Sample
@@ -1248,8 +1252,8 @@ void MPU6050::setSlave4WriteMode(bool mode) {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 uint8_t MPU6050::getSlave4MasterDelay() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_BIT), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_BIT), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set Slave 4 master delay value.
  * @param delay New Slave 4 master delay value
@@ -1257,7 +1261,7 @@ uint8_t MPU6050::getSlave4MasterDelay() {
  * @see MPU6050_RA_I2C_SLV4_CTRL
  */
 void MPU6050::setSlave4MasterDelay(uint8_t delay) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_BIT), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_LENGTH), delay);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_CTRL), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_BIT), (MPU6050_IMU::MPU6050_I2C_SLV4_MST_DLY_LENGTH), delay);
 }
 /** Get last available byte read from Slave 4.
  * This register stores the data read from Slave 4. This field is populated
@@ -1266,8 +1270,8 @@ void MPU6050::setSlave4MasterDelay(uint8_t delay) {
  * @see MPU6050_RA_I2C_SLV4_DI
  */
 uint8_t MPU6050::getSlate4InputByte() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_DI), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV4_DI), buffer);
+  return buffer[0];
 }
 
 // I2C_MST_STATUS register
@@ -1282,8 +1286,8 @@ uint8_t MPU6050::getSlate4InputByte() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getPassthroughStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_PASS_THROUGH_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_PASS_THROUGH_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 4 transaction done status.
  * Automatically sets to 1 when a Slave 4 transaction has completed. This
@@ -1294,8 +1298,8 @@ bool MPU6050::getPassthroughStatus() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave4IsDone() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV4_DONE_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV4_DONE_BIT), buffer);
+  return buffer[0];
 }
 /** Get master arbitration lost status.
  * This bit automatically sets to 1 when the I2C Master has lost arbitration of
@@ -1305,8 +1309,8 @@ bool MPU6050::getSlave4IsDone() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getLostArbitration() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_LOST_ARB_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_LOST_ARB_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 4 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1316,8 +1320,8 @@ bool MPU6050::getLostArbitration() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave4Nack() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV4_NACK_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV4_NACK_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 3 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1327,8 +1331,8 @@ bool MPU6050::getSlave4Nack() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave3Nack() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV3_NACK_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV3_NACK_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 2 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1338,8 +1342,8 @@ bool MPU6050::getSlave3Nack() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave2Nack() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV2_NACK_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV2_NACK_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 1 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1349,8 +1353,8 @@ bool MPU6050::getSlave2Nack() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave1Nack() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV1_NACK_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV1_NACK_BIT), buffer);
+  return buffer[0];
 }
 /** Get Slave 0 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1360,8 +1364,8 @@ bool MPU6050::getSlave1Nack() {
  * @see MPU6050_RA_I2C_MST_STATUS
  */
 bool MPU6050::getSlave0Nack() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV0_NACK_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_STATUS), (MPU6050_IMU::MPU6050_MST_I2C_SLV0_NACK_BIT), buffer);
+  return buffer[0];
 }
 
 // INT_PIN_CFG register
@@ -1373,8 +1377,8 @@ bool MPU6050::getSlave0Nack() {
  * @see MPU6050_INTCFG_INT_LEVEL_BIT
  */
 bool MPU6050::getInterruptMode() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_LEVEL_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_LEVEL_BIT), buffer);
+  return buffer[0];
 }
 /** Set interrupt logic level mode.
  * @param mode New interrupt mode (0=active-high, 1=active-low)
@@ -1383,7 +1387,7 @@ bool MPU6050::getInterruptMode() {
  * @see MPU6050_INTCFG_INT_LEVEL_BIT
  */
 void MPU6050::setInterruptMode(bool mode) {
-   I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_LEVEL_BIT), mode);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_LEVEL_BIT), mode);
 }
 /** Get interrupt drive mode.
  * Will be set 0 for push-pull, 1 for open-drain.
@@ -1392,8 +1396,8 @@ void MPU6050::setInterruptMode(bool mode) {
  * @see MPU6050_INTCFG_INT_OPEN_BIT
  */
 bool MPU6050::getInterruptDrive() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_OPEN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_OPEN_BIT), buffer);
+  return buffer[0];
 }
 /** Set interrupt drive mode.
  * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
@@ -1402,7 +1406,7 @@ bool MPU6050::getInterruptDrive() {
  * @see MPU6050_INTCFG_INT_OPEN_BIT
  */
 void MPU6050::setInterruptDrive(bool drive) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_OPEN_BIT), drive);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_OPEN_BIT), drive);
 }
 /** Get interrupt latch mode.
  * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
@@ -1411,8 +1415,8 @@ void MPU6050::setInterruptDrive(bool drive) {
  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
  */
 bool MPU6050::getInterruptLatch() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_LATCH_INT_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_LATCH_INT_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set interrupt latch mode.
  * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
@@ -1421,7 +1425,7 @@ bool MPU6050::getInterruptLatch() {
  * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
  */
 void MPU6050::setInterruptLatch(bool latch) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_LATCH_INT_EN_BIT), latch);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_LATCH_INT_EN_BIT), latch);
 }
 /** Get interrupt latch clear mode.
  * Will be set 0 for status-read-only, 1 for any-register-read.
@@ -1430,8 +1434,8 @@ void MPU6050::setInterruptLatch(bool latch) {
  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
  */
 bool MPU6050::getInterruptLatchClear() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_RD_CLEAR_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_RD_CLEAR_BIT), buffer);
+  return buffer[0];
 }
 /** Set interrupt latch clear mode.
  * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
@@ -1440,7 +1444,7 @@ bool MPU6050::getInterruptLatchClear() {
  * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
  */
 void MPU6050::setInterruptLatchClear(bool clear) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_RD_CLEAR_BIT), clear);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_INT_RD_CLEAR_BIT), clear);
 }
 /** Get FSYNC interrupt logic level mode.
  * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1449,8 +1453,8 @@ void MPU6050::setInterruptLatchClear(bool clear) {
  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
  */
 bool MPU6050::getFSyncInterruptLevel() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), buffer);
+  return buffer[0];
 }
 /** Set FSYNC interrupt logic level mode.
  * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1459,7 +1463,7 @@ bool MPU6050::getFSyncInterruptLevel() {
  * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
  */
 void MPU6050::setFSyncInterruptLevel(bool level) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), level);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT), level);
 }
 /** Get FSYNC pin interrupt enabled setting.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1468,8 +1472,8 @@ void MPU6050::setFSyncInterruptLevel(bool level) {
  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
  */
 bool MPU6050::getFSyncInterruptEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set FSYNC pin interrupt enabled setting.
  * @param enabled New FSYNC pin interrupt enabled setting
@@ -1478,7 +1482,7 @@ bool MPU6050::getFSyncInterruptEnabled() {
  * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
  */
 void MPU6050::setFSyncInterruptEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_FSYNC_INT_EN_BIT), enabled);
 }
 /** Get I2C bypass enabled status.
  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1492,8 +1496,8 @@ void MPU6050::setFSyncInterruptEnabled(bool enabled) {
  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
  */
 bool MPU6050::getI2CBypassEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_I2C_BYPASS_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_I2C_BYPASS_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set I2C bypass enabled status.
  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1507,7 +1511,7 @@ bool MPU6050::getI2CBypassEnabled() {
  * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
  */
 void MPU6050::setI2CBypassEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_I2C_BYPASS_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_I2C_BYPASS_EN_BIT), enabled);
 }
 /** Get reference clock output enabled status.
  * When this bit is equal to 1, a reference clock output is provided at the
@@ -1519,8 +1523,8 @@ void MPU6050::setI2CBypassEnabled(bool enabled) {
  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
  */
 bool MPU6050::getClockOutputEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_CLKOUT_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_CLKOUT_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set reference clock output enabled status.
  * When this bit is equal to 1, a reference clock output is provided at the
@@ -1532,7 +1536,7 @@ bool MPU6050::getClockOutputEnabled() {
  * @see MPU6050_INTCFG_CLKOUT_EN_BIT
  */
 void MPU6050::setClockOutputEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_CLKOUT_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_PIN_CFG), (MPU6050_IMU::MPU6050_INTCFG_CLKOUT_EN_BIT), enabled);
 }
 
 // INT_ENABLE register
@@ -1545,8 +1549,8 @@ void MPU6050::setClockOutputEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
 uint8_t MPU6050::getIntEnabled() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), buffer);
+  return buffer[0];
 }
 /** Set full interrupt enabled status.
  * Full register byte for all interrupts, for quick reading. Each bit should be
@@ -1557,7 +1561,7 @@ uint8_t MPU6050::getIntEnabled() {
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
 void MPU6050::setIntEnabled(uint8_t enabled) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), enabled);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), enabled);
 }
 /** Get Free Fall interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1566,8 +1570,8 @@ void MPU6050::setIntEnabled(uint8_t enabled) {
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
 bool MPU6050::getIntFreefallEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), buffer);
+  return buffer[0];
 }
 /** Set Free Fall interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1576,7 +1580,7 @@ bool MPU6050::getIntFreefallEnabled() {
  * @see MPU6050_INTERRUPT_FF_BIT
  **/
 void MPU6050::setIntFreefallEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), enabled);
 }
 /** Get Motion Detection interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1585,8 +1589,8 @@ void MPU6050::setIntFreefallEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_MOT_BIT
  **/
 bool MPU6050::getIntMotionEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), buffer);
+  return buffer[0];
 }
 /** Set Motion Detection interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1595,7 +1599,7 @@ bool MPU6050::getIntMotionEnabled() {
  * @see MPU6050_INTERRUPT_MOT_BIT
  **/
 void MPU6050::setIntMotionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), enabled);
 }
 /** Get Zero Motion Detection interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1604,8 +1608,8 @@ void MPU6050::setIntMotionEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  **/
 bool MPU6050::getIntZeroMotionEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), buffer);
+  return buffer[0];
 }
 /** Set Zero Motion Detection interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1614,7 +1618,7 @@ bool MPU6050::getIntZeroMotionEnabled() {
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  **/
 void MPU6050::setIntZeroMotionEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), enabled);
 }
 /** Get FIFO Buffer Overflow interrupt enabled status.
  * Will be set 0 for disabled, 1 for enabled.
@@ -1623,8 +1627,8 @@ void MPU6050::setIntZeroMotionEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  **/
 bool MPU6050::getIntFIFOBufferOverflowEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), buffer);
+  return buffer[0];
 }
 /** Set FIFO Buffer Overflow interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1633,7 +1637,7 @@ bool MPU6050::getIntFIFOBufferOverflowEnabled() {
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  **/
 void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), enabled);
 }
 /** Get I2C Master interrupt enabled status.
  * This enables any of the I2C Master interrupt sources to generate an
@@ -1643,8 +1647,8 @@ void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  **/
 bool MPU6050::getIntI2CMasterEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), buffer);
+  return buffer[0];
 }
 /** Set I2C Master interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1653,7 +1657,7 @@ bool MPU6050::getIntI2CMasterEnabled() {
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  **/
 void MPU6050::setIntI2CMasterEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), enabled);
 }
 /** Get Data Ready interrupt enabled setting.
  * This event occurs each time a write operation to all of the sensor registers
@@ -1663,8 +1667,8 @@ void MPU6050::setIntI2CMasterEnabled(bool enabled) {
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
 bool MPU6050::getIntDataReadyEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), buffer);
+  return buffer[0];
 }
 /** Set Data Ready interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1673,7 +1677,7 @@ bool MPU6050::getIntDataReadyEnabled() {
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
 void MPU6050::setIntDataReadyEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), enabled);
 }
 
 // INT_STATUS register
@@ -1686,8 +1690,8 @@ void MPU6050::setIntDataReadyEnabled(bool enabled) {
  * @see MPU6050_RA_INT_STATUS
  */
 uint8_t MPU6050::getIntStatus() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), buffer);
+  return buffer[0];
 }
 /** Get Free Fall interrupt status.
  * This bit automatically sets to 1 when a Free Fall interrupt has been
@@ -1697,8 +1701,8 @@ uint8_t MPU6050::getIntStatus() {
  * @see MPU6050_INTERRUPT_FF_BIT
  */
 bool MPU6050::getIntFreefallStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_FF_BIT), buffer);
+  return buffer[0];
 }
 /** Get Motion Detection interrupt status.
  * This bit automatically sets to 1 when a Motion Detection interrupt has been
@@ -1708,8 +1712,8 @@ bool MPU6050::getIntFreefallStatus() {
  * @see MPU6050_INTERRUPT_MOT_BIT
  */
 bool MPU6050::getIntMotionStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_MOT_BIT), buffer);
+  return buffer[0];
 }
 /** Get Zero Motion Detection interrupt status.
  * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
@@ -1719,8 +1723,8 @@ bool MPU6050::getIntMotionStatus() {
  * @see MPU6050_INTERRUPT_ZMOT_BIT
  */
 bool MPU6050::getIntZeroMotionStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_ZMOT_BIT), buffer);
+  return buffer[0];
 }
 /** Get FIFO Buffer Overflow interrupt status.
  * This bit automatically sets to 1 when a Free Fall interrupt has been
@@ -1730,8 +1734,8 @@ bool MPU6050::getIntZeroMotionStatus() {
  * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
  */
 bool MPU6050::getIntFIFOBufferOverflowStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT), buffer);
+  return buffer[0];
 }
 /** Get I2C Master interrupt status.
  * This bit automatically sets to 1 when an I2C Master interrupt has been
@@ -1742,8 +1746,8 @@ bool MPU6050::getIntFIFOBufferOverflowStatus() {
  * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
  */
 bool MPU6050::getIntI2CMasterStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_I2C_MST_INT_BIT), buffer);
+  return buffer[0];
 }
 /** Get Data Ready interrupt status.
  * This bit automatically sets to 1 when a Data Ready interrupt has been
@@ -1753,8 +1757,8 @@ bool MPU6050::getIntI2CMasterStatus() {
  * @see MPU6050_INTERRUPT_DATA_RDY_BIT
  */
 bool MPU6050::getIntDataReadyStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_DATA_RDY_BIT), buffer);
+  return buffer[0];
 }
 
 // ACCEL_*OUT_* registers
@@ -1775,9 +1779,9 @@ 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) {
-    getMotion6(ax, ay, az, gx, gy, gz);
-    // TODO: magnetometer integration
+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) {
+  getMotion6(ax, ay, az, gx, gy, gz);
+  // TODO: magnetometer integration
 }
 /** Get raw 6-axis motion sensor readings (accel/gyro).
  * Retrieves all currently available motion sensor values.
@@ -1791,14 +1795,14 @@ 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_IMU::MPU6050_RA_ACCEL_XOUT_H), 14, buffer);
-    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
-    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
-    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
-    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
-    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
-    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
+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_IMU::MPU6050_RA_ACCEL_XOUT_H), 14, buffer);
+  *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
+  *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
+  *az = (((int16_t)buffer[4]) << 8) | buffer[5];
+  *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
+  *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
+  *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
 }
 /** Get 3-axis accelerometer readings.
  * These registers store the most recent accelerometer measurements.
@@ -1836,11 +1840,11 @@ 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_IMU::MPU6050_RA_ACCEL_XOUT_H), 6, buffer);
-    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
-    *y = (((int16_t)buffer[2]) << 8) | buffer[3];
-    *z = (((int16_t)buffer[4]) << 8) | buffer[5];
+void MPU6050::getAcceleration(int16_t *x, int16_t *y, int16_t *z) {
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_XOUT_H), 6, buffer);
+  *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+  *y = (((int16_t)buffer[2]) << 8) | buffer[3];
+  *z = (((int16_t)buffer[4]) << 8) | buffer[5];
 }
 /** Get X-axis accelerometer reading.
  * @return X-axis acceleration measurement in 16-bit 2's complement format
@@ -1848,8 +1852,8 @@ void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
  * @see MPU6050_RA_ACCEL_XOUT_H
  */
 int16_t MPU6050::getAccelerationX() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_XOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_XOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Y-axis accelerometer reading.
  * @return Y-axis acceleration measurement in 16-bit 2's complement format
@@ -1857,8 +1861,8 @@ int16_t MPU6050::getAccelerationX() {
  * @see MPU6050_RA_ACCEL_YOUT_H
  */
 int16_t MPU6050::getAccelerationY() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_YOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_YOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Z-axis accelerometer reading.
  * @return Z-axis acceleration measurement in 16-bit 2's complement format
@@ -1866,8 +1870,8 @@ int16_t MPU6050::getAccelerationY() {
  * @see MPU6050_RA_ACCEL_ZOUT_H
  */
 int16_t MPU6050::getAccelerationZ() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_ZOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ACCEL_ZOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
 // TEMP_OUT_* registers
@@ -1877,8 +1881,8 @@ int16_t MPU6050::getAccelerationZ() {
  * @see MPU6050_RA_TEMP_OUT_H
  */
 int16_t MPU6050::getTemperature() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_TEMP_OUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_TEMP_OUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
 // GYRO_*OUT_* registers
@@ -1915,11 +1919,11 @@ 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_IMU::MPU6050_RA_GYRO_XOUT_H), 6, buffer);
-    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
-    *y = (((int16_t)buffer[2]) << 8) | buffer[3];
-    *z = (((int16_t)buffer[4]) << 8) | buffer[5];
+void MPU6050::getRotation(int16_t *x, int16_t *y, int16_t *z) {
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_XOUT_H), 6, buffer);
+  *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+  *y = (((int16_t)buffer[2]) << 8) | buffer[3];
+  *z = (((int16_t)buffer[4]) << 8) | buffer[5];
 }
 /** Get X-axis gyroscope reading.
  * @return X-axis rotation measurement in 16-bit 2's complement format
@@ -1927,8 +1931,8 @@ void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) {
  * @see MPU6050_RA_GYRO_XOUT_H
  */
 int16_t MPU6050::getRotationX() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_XOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_XOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Y-axis gyroscope reading.
  * @return Y-axis rotation measurement in 16-bit 2's complement format
@@ -1936,8 +1940,8 @@ int16_t MPU6050::getRotationX() {
  * @see MPU6050_RA_GYRO_YOUT_H
  */
 int16_t MPU6050::getRotationY() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_YOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_YOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Get Z-axis gyroscope reading.
  * @return Z-axis rotation measurement in 16-bit 2's complement format
@@ -1945,8 +1949,8 @@ int16_t MPU6050::getRotationY() {
  * @see MPU6050_RA_GYRO_ZOUT_H
  */
 int16_t MPU6050::getRotationZ() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_ZOUT_H), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_GYRO_ZOUT_H), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 
 // EXT_SENS_DATA_* registers
@@ -2026,8 +2030,8 @@ int16_t MPU6050::getRotationZ() {
  * @return Byte read from register
  */
 uint8_t MPU6050::getExternalSensorByte(int position) {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, buffer);
+  return buffer[0];
 }
 /** Read word (2 bytes) from external sensor data registers.
  * @param position Starting position (0-21)
@@ -2035,8 +2039,8 @@ uint8_t MPU6050::getExternalSensorByte(int position) {
  * @see getExternalSensorByte()
  */
 uint16_t MPU6050::getExternalSensorWord(int position) {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, 2, buffer);
-    return (((uint16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, 2, buffer);
+  return (((uint16_t)buffer[0]) << 8) | buffer[1];
 }
 /** Read double word (4 bytes) from external sensor data registers.
  * @param position Starting position (0-20)
@@ -2044,8 +2048,8 @@ uint16_t MPU6050::getExternalSensorWord(int position) {
  * @see getExternalSensorByte()
  */
 uint32_t MPU6050::getExternalSensorDWord(int position) {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, 4, buffer);
-    return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_EXT_SENS_DATA_00) + position, 4, buffer);
+  return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
 }
 
 // MOT_DETECT_STATUS register
@@ -2055,8 +2059,8 @@ uint32_t MPU6050::getExternalSensorDWord(int position) {
  * @see MPU6050_RA_MOT_DETECT_STATUS
  */
 uint8_t MPU6050::getMotionStatus() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), buffer);
+  return buffer[0];
 }
 /** Get X-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2064,8 +2068,8 @@ uint8_t MPU6050::getMotionStatus() {
  * @see MPU6050_MOTION_MOT_XNEG_BIT
  */
 bool MPU6050::getXNegMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_XNEG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_XNEG_BIT), buffer);
+  return buffer[0];
 }
 /** Get X-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2073,8 +2077,8 @@ bool MPU6050::getXNegMotionDetected() {
  * @see MPU6050_MOTION_MOT_XPOS_BIT
  */
 bool MPU6050::getXPosMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_XPOS_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_XPOS_BIT), buffer);
+  return buffer[0];
 }
 /** Get Y-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2082,8 +2086,8 @@ bool MPU6050::getXPosMotionDetected() {
  * @see MPU6050_MOTION_MOT_YNEG_BIT
  */
 bool MPU6050::getYNegMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_YNEG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_YNEG_BIT), buffer);
+  return buffer[0];
 }
 /** Get Y-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2091,8 +2095,8 @@ bool MPU6050::getYNegMotionDetected() {
  * @see MPU6050_MOTION_MOT_YPOS_BIT
  */
 bool MPU6050::getYPosMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_YPOS_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_YPOS_BIT), buffer);
+  return buffer[0];
 }
 /** Get Z-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2100,8 +2104,8 @@ bool MPU6050::getYPosMotionDetected() {
  * @see MPU6050_MOTION_MOT_ZNEG_BIT
  */
 bool MPU6050::getZNegMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZNEG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZNEG_BIT), buffer);
+  return buffer[0];
 }
 /** Get Z-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2109,8 +2113,8 @@ bool MPU6050::getZNegMotionDetected() {
  * @see MPU6050_MOTION_MOT_ZPOS_BIT
  */
 bool MPU6050::getZPosMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZPOS_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZPOS_BIT), buffer);
+  return buffer[0];
 }
 /** Get zero motion detection interrupt status.
  * @return Motion detection status
@@ -2118,8 +2122,8 @@ bool MPU6050::getZPosMotionDetected() {
  * @see MPU6050_MOTION_MOT_ZRMOT_BIT
  */
 bool MPU6050::getZeroMotionDetected() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZRMOT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_STATUS), (MPU6050_IMU::MPU6050_MOTION_MOT_ZRMOT_BIT), buffer);
+  return buffer[0];
 }
 
 // I2C_SLV*_DO register
@@ -2133,8 +2137,8 @@ bool MPU6050::getZeroMotionDetected() {
  * @see MPU6050_RA_I2C_SLV0_DO
  */
 void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) {
-    if (num > 3) return;
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_DO) + num, data);
+  if (num > 3) return;
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_SLV0_DO) + num, data);
 }
 
 // I2C_MST_DELAY_CTRL register
@@ -2148,8 +2152,8 @@ void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) {
  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
  */
 bool MPU6050::getExternalShadowDelayEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), (MPU6050_IMU::MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), (MPU6050_IMU::MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT), buffer);
+  return buffer[0];
 }
 /** Set external data shadow delay enabled status.
  * @param enabled New external data shadow delay enabled status.
@@ -2158,7 +2162,7 @@ bool MPU6050::getExternalShadowDelayEnabled() {
  * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
  */
 void MPU6050::setExternalShadowDelayEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), (MPU6050_IMU::MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), (MPU6050_IMU::MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT), enabled);
 }
 /** Get slave delay enabled status.
  * When a particular slave delay is enabled, the rate of access for the that
@@ -2179,10 +2183,10 @@ void MPU6050::setExternalShadowDelayEnabled(bool enabled) {
  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
  */
 bool MPU6050::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_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), num, buffer);
-    return buffer[0];
+  // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
+  if (num > 4) return 0;
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), num, buffer);
+  return buffer[0];
 }
 /** Set slave delay enabled status.
  * @param num Slave number (0-4)
@@ -2191,7 +2195,7 @@ bool MPU6050::getSlaveDelayEnabled(uint8_t num) {
  * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
  */
 void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), num, enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_I2C_MST_DELAY_CTRL), num, enabled);
 }
 
 // SIGNAL_PATH_RESET register
@@ -2203,7 +2207,7 @@ void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) {
  * @see MPU6050_PATHRESET_GYRO_RESET_BIT
  */
 void MPU6050::resetGyroscopePath() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_GYRO_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_GYRO_RESET_BIT), true);
 }
 /** Reset accelerometer signal path.
  * The reset will revert the signal path analog to digital converters and
@@ -2212,7 +2216,7 @@ void MPU6050::resetGyroscopePath() {
  * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
  */
 void MPU6050::resetAccelerometerPath() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_ACCEL_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_ACCEL_RESET_BIT), true);
 }
 /** Reset temperature sensor signal path.
  * The reset will revert the signal path analog to digital converters and
@@ -2221,7 +2225,7 @@ void MPU6050::resetAccelerometerPath() {
  * @see MPU6050_PATHRESET_TEMP_RESET_BIT
  */
 void MPU6050::resetTemperaturePath() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_TEMP_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_SIGNAL_PATH_RESET), (MPU6050_IMU::MPU6050_PATHRESET_TEMP_RESET_BIT), true);
 }
 
 // MOT_DETECT_CTRL register
@@ -2241,8 +2245,8 @@ void MPU6050::resetTemperaturePath() {
  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
  */
 uint8_t MPU6050::getAccelerometerPowerOnDelay() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_BIT), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_BIT), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set accelerometer power-on delay.
  * @param delay New accelerometer power-on delay (0-3)
@@ -2251,7 +2255,7 @@ uint8_t MPU6050::getAccelerometerPowerOnDelay() {
  * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
  */
 void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_BIT), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH), delay);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_BIT), (MPU6050_IMU::MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH), delay);
 }
 /** Get Free Fall detection counter decrement configuration.
  * Detection is registered by the Free Fall detection module after accelerometer
@@ -2280,8 +2284,8 @@ void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) {
  * @see MPU6050_DETECT_FF_COUNT_BIT
  */
 uint8_t MPU6050::getFreefallDetectionCounterDecrement() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set Free Fall detection counter decrement configuration.
  * @param decrement New decrement configuration value
@@ -2290,7 +2294,7 @@ uint8_t MPU6050::getFreefallDetectionCounterDecrement() {
  * @see MPU6050_DETECT_FF_COUNT_BIT
  */
 void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_LENGTH), decrement);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_FF_COUNT_LENGTH), decrement);
 }
 /** Get Motion detection counter decrement configuration.
  * Detection is registered by the Motion detection module after accelerometer
@@ -2316,8 +2320,8 @@ void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) {
  *
  */
 uint8_t MPU6050::getMotionDetectionCounterDecrement() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set Motion detection counter decrement configuration.
  * @param decrement New decrement configuration value
@@ -2326,7 +2330,7 @@ uint8_t MPU6050::getMotionDetectionCounterDecrement() {
  * @see MPU6050_DETECT_MOT_COUNT_BIT
  */
 void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_LENGTH), decrement);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_MOT_DETECT_CTRL), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_BIT), (MPU6050_IMU::MPU6050_DETECT_MOT_COUNT_LENGTH), decrement);
 }
 
 // USER_CTRL register
@@ -2340,8 +2344,8 @@ void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) {
  * @see MPU6050_USERCTRL_FIFO_EN_BIT
  */
 bool MPU6050::getFIFOEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set FIFO enabled status.
  * @param enabled New FIFO enabled status
@@ -2350,7 +2354,7 @@ bool MPU6050::getFIFOEnabled() {
  * @see MPU6050_USERCTRL_FIFO_EN_BIT
  */
 void MPU6050::setFIFOEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_EN_BIT), enabled);
 }
 /** Get I2C Master Mode enabled status.
  * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
@@ -2364,8 +2368,8 @@ void MPU6050::setFIFOEnabled(bool enabled) {
  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
  */
 bool MPU6050::getI2CMasterModeEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_EN_BIT), buffer);
+  return buffer[0];
 }
 /** Set I2C Master Mode enabled status.
  * @param enabled New I2C Master Mode enabled status
@@ -2374,14 +2378,14 @@ bool MPU6050::getI2CMasterModeEnabled() {
  * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
  */
 void MPU6050::setI2CMasterModeEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_EN_BIT), enabled);
 }
 /** 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_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_IF_DIS_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_IF_DIS_BIT), enabled);
 }
 /** Reset the FIFO.
  * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
@@ -2390,7 +2394,7 @@ void MPU6050::switchSPIEnabled(bool enabled) {
  * @see MPU6050_USERCTRL_FIFO_RESET_BIT
  */
 void MPU6050::resetFIFO() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_FIFO_RESET_BIT), true);
 }
 /** Reset the I2C Master.
  * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
@@ -2399,7 +2403,7 @@ void MPU6050::resetFIFO() {
  * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
  */
 void MPU6050::resetI2CMaster() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_I2C_MST_RESET_BIT), true);
 }
 /** Reset all sensor registers and signal paths.
  * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
@@ -2414,7 +2418,7 @@ void MPU6050::resetI2CMaster() {
  * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
  */
 void MPU6050::resetSensors() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_SIG_COND_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_SIG_COND_RESET_BIT), true);
 }
 
 // PWR_MGMT_1 register
@@ -2425,7 +2429,7 @@ void MPU6050::resetSensors() {
  * @see MPU6050_PWR1_DEVICE_RESET_BIT
  */
 void MPU6050::reset() {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_DEVICE_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_DEVICE_RESET_BIT), true);
 }
 /** Get sleep mode status.
  * Setting the SLEEP bit in the register puts the device into very low power
@@ -2439,8 +2443,8 @@ void MPU6050::reset() {
  * @see MPU6050_PWR1_SLEEP_BIT
  */
 bool MPU6050::getSleepEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_SLEEP_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_SLEEP_BIT), buffer);
+  return buffer[0];
 }
 /** Set sleep mode status.
  * @param enabled New sleep mode enabled status
@@ -2449,7 +2453,7 @@ bool MPU6050::getSleepEnabled() {
  * @see MPU6050_PWR1_SLEEP_BIT
  */
 void MPU6050::setSleepEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_SLEEP_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_SLEEP_BIT), enabled);
 }
 /** Get wake cycle enabled status.
  * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
@@ -2460,8 +2464,8 @@ void MPU6050::setSleepEnabled(bool enabled) {
  * @see MPU6050_PWR1_CYCLE_BIT
  */
 bool MPU6050::getWakeCycleEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CYCLE_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CYCLE_BIT), buffer);
+  return buffer[0];
 }
 /** Set wake cycle enabled status.
  * @param enabled New sleep mode enabled status
@@ -2470,7 +2474,7 @@ bool MPU6050::getWakeCycleEnabled() {
  * @see MPU6050_PWR1_CYCLE_BIT
  */
 void MPU6050::setWakeCycleEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CYCLE_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CYCLE_BIT), enabled);
 }
 /** Get temperature sensor enabled status.
  * Control the usage of the internal temperature sensor.
@@ -2484,8 +2488,8 @@ void MPU6050::setWakeCycleEnabled(bool enabled) {
  * @see MPU6050_PWR1_TEMP_DIS_BIT
  */
 bool MPU6050::getTempSensorEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_TEMP_DIS_BIT), buffer);
-    return buffer[0] == 0; // 1 is actually disabled here
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_TEMP_DIS_BIT), buffer);
+  return buffer[0] == 0;  // 1 is actually disabled here
 }
 /** Set temperature sensor enabled status.
  * Note: this register stores the *disabled* value, but for consistency with the
@@ -2498,8 +2502,8 @@ bool MPU6050::getTempSensorEnabled() {
  * @see MPU6050_PWR1_TEMP_DIS_BIT
  */
 void MPU6050::setTempSensorEnabled(bool enabled) {
-    // 1 is actually disabled here
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_TEMP_DIS_BIT), !enabled);
+  // 1 is actually disabled here
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_TEMP_DIS_BIT), !enabled);
 }
 /** Get clock source setting.
  * @return Current clock source setting
@@ -2508,8 +2512,8 @@ void MPU6050::setTempSensorEnabled(bool enabled) {
  * @see (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH)
  */
 uint8_t MPU6050::getClockSource() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_BIT), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_BIT), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set clock source setting.
  * An internal 8MHz oscillator, gyroscope based clock, or external sources can
@@ -2542,7 +2546,7 @@ uint8_t MPU6050::getClockSource() {
  * @see (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH)
  */
 void MPU6050::setClockSource(uint8_t source) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_BIT), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH), source);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_1), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_BIT), (MPU6050_IMU::MPU6050_PWR1_CLKSEL_LENGTH), source);
 }
 
 // PWR_MGMT_2 register
@@ -2571,15 +2575,15 @@ void MPU6050::setClockSource(uint8_t source) {
  * @see MPU6050_RA_PWR_MGMT_2
  */
 uint8_t MPU6050::getWakeFrequency() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_BIT), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_BIT), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_LENGTH), buffer);
+  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_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_BIT), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_LENGTH), frequency);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_BIT), (MPU6050_IMU::MPU6050_PWR2_LP_WAKE_CTRL_LENGTH), frequency);
 }
 
 /** Get X-axis accelerometer standby enabled status.
@@ -2589,8 +2593,8 @@ void MPU6050::setWakeFrequency(uint8_t frequency) {
  * @see MPU6050_PWR2_STBY_XA_BIT
  */
 bool MPU6050::getStandbyXAccelEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XA_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XA_BIT), buffer);
+  return buffer[0];
 }
 /** Set X-axis accelerometer standby enabled status.
  * @param New X-axis standby enabled status
@@ -2599,7 +2603,7 @@ bool MPU6050::getStandbyXAccelEnabled() {
  * @see MPU6050_PWR2_STBY_XA_BIT
  */
 void MPU6050::setStandbyXAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XA_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XA_BIT), enabled);
 }
 /** Get Y-axis accelerometer standby enabled status.
  * If enabled, the Y-axis will not gather or report data (or use power).
@@ -2608,8 +2612,8 @@ void MPU6050::setStandbyXAccelEnabled(bool enabled) {
  * @see MPU6050_PWR2_STBY_YA_BIT
  */
 bool MPU6050::getStandbyYAccelEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YA_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YA_BIT), buffer);
+  return buffer[0];
 }
 /** Set Y-axis accelerometer standby enabled status.
  * @param New Y-axis standby enabled status
@@ -2618,7 +2622,7 @@ bool MPU6050::getStandbyYAccelEnabled() {
  * @see MPU6050_PWR2_STBY_YA_BIT
  */
 void MPU6050::setStandbyYAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YA_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YA_BIT), enabled);
 }
 /** Get Z-axis accelerometer standby enabled status.
  * If enabled, the Z-axis will not gather or report data (or use power).
@@ -2627,8 +2631,8 @@ void MPU6050::setStandbyYAccelEnabled(bool enabled) {
  * @see MPU6050_PWR2_STBY_ZA_BIT
  */
 bool MPU6050::getStandbyZAccelEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZA_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZA_BIT), buffer);
+  return buffer[0];
 }
 /** Set Z-axis accelerometer standby enabled status.
  * @param New Z-axis standby enabled status
@@ -2637,7 +2641,7 @@ bool MPU6050::getStandbyZAccelEnabled() {
  * @see MPU6050_PWR2_STBY_ZA_BIT
  */
 void MPU6050::setStandbyZAccelEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZA_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZA_BIT), enabled);
 }
 /** Get X-axis gyroscope standby enabled status.
  * If enabled, the X-axis will not gather or report data (or use power).
@@ -2646,8 +2650,8 @@ void MPU6050::setStandbyZAccelEnabled(bool enabled) {
  * @see MPU6050_PWR2_STBY_XG_BIT
  */
 bool MPU6050::getStandbyXGyroEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XG_BIT), buffer);
+  return buffer[0];
 }
 /** Set X-axis gyroscope standby enabled status.
  * @param New X-axis standby enabled status
@@ -2656,7 +2660,7 @@ bool MPU6050::getStandbyXGyroEnabled() {
  * @see MPU6050_PWR2_STBY_XG_BIT
  */
 void MPU6050::setStandbyXGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XG_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_XG_BIT), enabled);
 }
 /** Get Y-axis gyroscope standby enabled status.
  * If enabled, the Y-axis will not gather or report data (or use power).
@@ -2665,8 +2669,8 @@ void MPU6050::setStandbyXGyroEnabled(bool enabled) {
  * @see MPU6050_PWR2_STBY_YG_BIT
  */
 bool MPU6050::getStandbyYGyroEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YG_BIT), buffer);
+  return buffer[0];
 }
 /** Set Y-axis gyroscope standby enabled status.
  * @param New Y-axis standby enabled status
@@ -2675,7 +2679,7 @@ bool MPU6050::getStandbyYGyroEnabled() {
  * @see MPU6050_PWR2_STBY_YG_BIT
  */
 void MPU6050::setStandbyYGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YG_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_YG_BIT), enabled);
 }
 /** Get Z-axis gyroscope standby enabled status.
  * If enabled, the Z-axis will not gather or report data (or use power).
@@ -2684,8 +2688,8 @@ void MPU6050::setStandbyYGyroEnabled(bool enabled) {
  * @see MPU6050_PWR2_STBY_ZG_BIT
  */
 bool MPU6050::getStandbyZGyroEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZG_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZG_BIT), buffer);
+  return buffer[0];
 }
 /** Set Z-axis gyroscope standby enabled status.
  * @param New Z-axis standby enabled status
@@ -2694,7 +2698,7 @@ bool MPU6050::getStandbyZGyroEnabled() {
  * @see MPU6050_PWR2_STBY_ZG_BIT
  */
 void MPU6050::setStandbyZGyroEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZG_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_PWR_MGMT_2), (MPU6050_IMU::MPU6050_PWR2_STBY_ZG_BIT), enabled);
 }
 
 // FIFO_COUNT* registers
@@ -2707,8 +2711,8 @@ void MPU6050::setStandbyZGyroEnabled(bool enabled) {
  * @return Current FIFO buffer size
  */
 uint16_t MPU6050::getFIFOCount() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_COUNTH), 2, buffer);
-    return (((uint16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_COUNTH), 2, buffer);
+  return (((uint16_t)buffer[0]) << 8) | buffer[1];
 }
 
 // FIFO_R_W register
@@ -2739,15 +2743,15 @@ uint16_t MPU6050::getFIFOCount() {
  * @return Byte from FIFO buffer
  */
 uint8_t MPU6050::getFIFOByte() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), buffer);
+  return buffer[0];
 }
 void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) {
-    if(length > 0){
-        I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), length, data);
-    } else {
-    	*data = 0;
-    }
+  if (length > 0) {
+    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), length, data);
+  } else {
+    *data = 0;
+  }
 }
 
 /** Get latest byte from FIFO buffer no matter how much time has passed.
@@ -2757,45 +2761,44 @@ 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
-     int16_t fifoC;
-     // This section of code is for when we allowed more than 1 packet to be acquired
-     uint32_t BreakTimer = micros();
-     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
-                 } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
-                 uint8_t Trash[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
-                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
-                         fifoC -= RemoveBytes;
-                     }
-                 }
-             }
-         }
-         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);
-     getFIFOBytes(data, length); //Get 1 packet
-     return 1;
+int8_t MPU6050::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();
+  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
+      } else {  // We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
+        uint8_t Trash[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
+            getFIFOBytes(Trash, (uint8_t)RemoveBytes);
+            fifoC -= RemoveBytes;
+          }
+        }
+      }
+    }
+    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);
+  getFIFOBytes(data, length);  // Get 1 packet
+  return 1;
 }
 
-
 /** Write byte to FIFO buffer.
  * @see getFIFOByte()
  * @see MPU6050_RA_FIFO_R_W
  */
 void MPU6050::setFIFOByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), data);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_FIFO_R_W), data);
 }
 
 // WHO_AM_I register
@@ -2808,8 +2811,8 @@ void MPU6050::setFIFOByte(uint8_t data) {
  * @see (MPU6050_IMU::MPU6050_WHO_AM_I_LENGTH)
  */
 uint8_t MPU6050::getDeviceID() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_WHO_AM_I), (MPU6050_IMU::MPU6050_WHO_AM_I_BIT), (MPU6050_IMU::MPU6050_WHO_AM_I_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_WHO_AM_I), (MPU6050_IMU::MPU6050_WHO_AM_I_BIT), (MPU6050_IMU::MPU6050_WHO_AM_I_LENGTH), buffer);
+  return buffer[0];
 }
 /** Set Device ID.
  * Write a new ID into the WHO_AM_I register (no idea why this should ever be
@@ -2821,7 +2824,7 @@ uint8_t MPU6050::getDeviceID() {
  * @see (MPU6050_WHO_AM_I_LENGTH)
  */
 void MPU6050::setDeviceID(uint8_t id) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_WHO_AM_I), (MPU6050_IMU::MPU6050_WHO_AM_I_BIT), (MPU6050_IMU::MPU6050_WHO_AM_I_LENGTH), id);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_WHO_AM_I), (MPU6050_IMU::MPU6050_WHO_AM_I_BIT), (MPU6050_IMU::MPU6050_WHO_AM_I_LENGTH), id);
 }
 
 // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
@@ -2829,446 +2832,451 @@ void MPU6050::setDeviceID(uint8_t id) {
 // XG_OFFS_TC register
 
 uint8_t MPU6050::getOTPBankValid() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OTP_BNK_VLD_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OTP_BNK_VLD_BIT), buffer);
+  return buffer[0];
 }
 void MPU6050::setOTPBankValid(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OTP_BNK_VLD_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OTP_BNK_VLD_BIT), enabled);
 }
 int8_t MPU6050::getXGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
+  return buffer[0];
 }
 void MPU6050::setXGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
 }
 
 // YG_OFFS_TC register
 
 int8_t MPU6050::getYGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
+  return buffer[0];
 }
 void MPU6050::setYGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
 }
 
 // ZG_OFFS_TC register
 
 int8_t MPU6050::getZGyroOffsetTC() {
-    I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
-    return buffer[0];
+  I2Cdev::readBits(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), buffer);
+  return buffer[0];
 }
 void MPU6050::setZGyroOffsetTC(int8_t offset) {
-    I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
+  I2Cdev::writeBits(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_TC), (MPU6050_IMU::MPU6050_TC_OFFSET_BIT), (MPU6050_IMU::MPU6050_TC_OFFSET_LENGTH), offset);
 }
 
 // X_FINE_GAIN register
 
 int8_t MPU6050::getXFineGain() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_X_FINE_GAIN), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_X_FINE_GAIN), buffer);
+  return buffer[0];
 }
 void MPU6050::setXFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_X_FINE_GAIN), gain);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_X_FINE_GAIN), gain);
 }
 
 // Y_FINE_GAIN register
 
 int8_t MPU6050::getYFineGain() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_Y_FINE_GAIN), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_Y_FINE_GAIN), buffer);
+  return buffer[0];
 }
 void MPU6050::setYFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_Y_FINE_GAIN), gain);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_Y_FINE_GAIN), gain);
 }
 
 // Z_FINE_GAIN register
 
 int8_t MPU6050::getZFineGain() {
-    I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_Z_FINE_GAIN), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_Z_FINE_GAIN), buffer);
+  return buffer[0];
 }
 void MPU6050::setZFineGain(int8_t gain) {
-    I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_Z_FINE_GAIN), gain);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_Z_FINE_GAIN), gain);
 }
 
 // XA_OFFS_* registers
 
 int16_t MPU6050::getXAccelOffset() {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_XA_OFFS_H):0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_XA_OFFS_H) : 0x77);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setXAccelOffset(int16_t offset) {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_XA_OFFS_H):0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_XA_OFFS_H) : 0x77);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::writeWord(devAddr, SaveAddress, offset);
 }
 
 // YA_OFFS_* register
 
 int16_t MPU6050::getYAccelOffset() {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_YA_OFFS_H):0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_YA_OFFS_H) : 0x7A);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setYAccelOffset(int16_t offset) {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_YA_OFFS_H):0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_YA_OFFS_H) : 0x7A);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::writeWord(devAddr, SaveAddress, offset);
 }
 
 // ZA_OFFS_* register
 
 int16_t MPU6050::getZAccelOffset() {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_ZA_OFFS_H):0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_ZA_OFFS_H) : 0x7D);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setZAccelOffset(int16_t offset) {
-	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? (MPU6050_IMU::MPU6050_RA_ZA_OFFS_H):0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
-	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+  uint8_t SaveAddress = ((getDeviceID() < 0x38) ? (MPU6050_IMU::MPU6050_RA_ZA_OFFS_H) : 0x7D);  // MPU6050,MPU9150 Vs MPU6500,MPU9250
+  I2Cdev::writeWord(devAddr, SaveAddress, offset);
 }
 
 // XG_OFFS_USR* registers
 
 int16_t MPU6050::getXGyroOffset() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_USRH), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_USRH), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setXGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_USRH), offset);
+  I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_XG_OFFS_USRH), offset);
 }
 
 // YG_OFFS_USR* register
 
 int16_t MPU6050::getYGyroOffset() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_USRH), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_USRH), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setYGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_USRH), offset);
+  I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_YG_OFFS_USRH), offset);
 }
 
 // ZG_OFFS_USR* register
 
 int16_t MPU6050::getZGyroOffset() {
-    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_USRH), 2, buffer);
-    return (((int16_t)buffer[0]) << 8) | buffer[1];
+  I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_USRH), 2, buffer);
+  return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
 void MPU6050::setZGyroOffset(int16_t offset) {
-    I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_USRH), offset);
+  I2Cdev::writeWord(devAddr, (MPU6050_IMU::MPU6050_RA_ZG_OFFS_USRH), offset);
 }
 
 // INT_ENABLE register (DMP functions)
 
 bool MPU6050::getIntPLLReadyEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), buffer);
+  return buffer[0];
 }
 void MPU6050::setIntPLLReadyEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), enabled);
 }
 bool MPU6050::getIntDMPEnabled() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), buffer);
+  return buffer[0];
 }
 void MPU6050::setIntDMPEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), enabled);
 }
 
 // DMP_INT_STATUS
 
 bool MPU6050::getDMPInt5Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_5_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_5_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getDMPInt4Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_4_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_4_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getDMPInt3Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_3_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_3_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getDMPInt2Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_2_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_2_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getDMPInt1Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_1_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_1_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getDMPInt0Status() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_0_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_INT_STATUS), (MPU6050_IMU::MPU6050_DMPINT_0_BIT), buffer);
+  return buffer[0];
 }
 
 // INT_STATUS register (DMP functions)
 
 bool MPU6050::getIntPLLReadyStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_PLL_RDY_INT_BIT), buffer);
+  return buffer[0];
 }
 bool MPU6050::getIntDMPStatus() {
-    I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_INT_STATUS), (MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT), buffer);
+  return buffer[0];
 }
 
 // USER_CTRL register (DMP functions)
 
 bool MPU6050::getDMPEnabled() {
-    I2Cdev::readBit(devAddr,  (MPU6050_IMU::MPU6050_RA_USER_CTRL),  (MPU6050_IMU::MPU6050_USERCTRL_DMP_EN_BIT), buffer);
-    return buffer[0];
+  I2Cdev::readBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_DMP_EN_BIT), buffer);
+  return buffer[0];
 }
 void MPU6050::setDMPEnabled(bool enabled) {
-    I2Cdev::writeBit(devAddr,  (MPU6050_IMU::MPU6050_RA_USER_CTRL),  (MPU6050_IMU::MPU6050_USERCTRL_DMP_EN_BIT), enabled);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_DMP_EN_BIT), enabled);
 }
 void MPU6050::resetDMP() {
-    I2Cdev::writeBit(devAddr,  (MPU6050_IMU::MPU6050_RA_USER_CTRL),  (MPU6050_IMU::MPU6050_USERCTRL_DMP_RESET_BIT), true);
+  I2Cdev::writeBit(devAddr, (MPU6050_IMU::MPU6050_RA_USER_CTRL), (MPU6050_IMU::MPU6050_USERCTRL_DMP_RESET_BIT), true);
 }
 
 // BANK_SEL register
 
 void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
-    bank &= 0x1F;
-    if (userBank) bank |= 0x20;
-    if (prefetchEnabled) bank |= 0x40;
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_BANK_SEL), bank);
+  bank &= 0x1F;
+  if (userBank) bank |= 0x20;
+  if (prefetchEnabled) bank |= 0x40;
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_BANK_SEL), bank);
 }
 
 // MEM_START_ADDR register
 
 void MPU6050::setMemoryStartAddress(uint8_t address) {
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_START_ADDR), address);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_START_ADDR), address);
 }
 
 // MEM_R_W register
 
 uint8_t MPU6050::readMemoryByte() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_R_W), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_R_W), buffer);
+  return buffer[0];
 }
 void MPU6050::writeMemoryByte(uint8_t data) {
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_R_W), data);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_R_W), data);
 }
 void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
-    setMemoryBank(bank);
-    setMemoryStartAddress(address);
-    uint8_t chunkSize;
-    for (uint16_t i = 0; i < dataSize;) {
-        // determine correct chunk size according to bank position and data size
-        chunkSize = (MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE);
+  setMemoryBank(bank);
+  setMemoryStartAddress(address);
+  uint8_t chunkSize;
+  for (uint16_t i = 0; i < dataSize;) {
+    // determine correct chunk size according to bank position and data size
+    chunkSize = (MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE);
 
-        // make sure we don't go past the data size
-        if (i + chunkSize > dataSize) chunkSize = dataSize - i;
+    // make sure we don't go past the data size
+    if (i + chunkSize > dataSize) chunkSize = dataSize - i;
 
-        // make sure this chunk doesn't go past the bank boundary (256 bytes)
-        if (chunkSize > 256 - address) chunkSize = 256 - address;
+    // make sure this chunk doesn't go past the bank boundary (256 bytes)
+    if (chunkSize > 256 - address) chunkSize = 256 - address;
 
-        // read the chunk of data as specified
-        I2Cdev::readBytes(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, data + i);
+    // read the chunk of data as specified
+    I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, data + i);
 
-        // increase byte index by [chunkSize]
-        i += chunkSize;
+    // increase byte index by [chunkSize]
+    i += chunkSize;
 
-        // uint8_t automatically wraps to 0 at 256
-        address += chunkSize;
+    // uint8_t automatically wraps to 0 at 256
+    address += chunkSize;
 
-        // if we aren't done, update bank (if necessary) and address
-        if (i < dataSize) {
-            if (address == 0) bank++;
-            setMemoryBank(bank);
-            setMemoryStartAddress(address);
-        }
+    // if we aren't done, update bank (if necessary) and address
+    if (i < dataSize) {
+      if (address == 0) bank++;
+      setMemoryBank(bank);
+      setMemoryStartAddress(address);
     }
+  }
 }
 bool MPU6050::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;
-    uint8_t *verifyBuffer=0;
-    uint8_t *progBuffer=0;
-    uint16_t i;
-    uint8_t j;
-    if (verify) verifyBuffer = (uint8_t *)malloc((MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE));
-    if (useProgMem) progBuffer = (uint8_t *)malloc((MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE));
-    for (i = 0; i < dataSize;) {
-        // determine correct chunk size according to bank position and data size
-        chunkSize = (MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE);
-
-        // make sure we don't go past the data size
-        if (i + chunkSize > dataSize) chunkSize = dataSize - i;
-
-        // make sure this chunk doesn't go past the bank boundary (256 bytes)
-        if (chunkSize > 256 - address) chunkSize = 256 - address;
-
-        if (useProgMem) {
-            // write the chunk of data as specified
-            for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
-        } else {
-            // write the chunk of data as specified
-            progBuffer = (uint8_t *)data + i;
-        }
+  setMemoryBank(bank);
+  setMemoryStartAddress(address);
+  uint8_t chunkSize;
+  uint8_t *verifyBuffer = 0;
+  uint8_t *progBuffer = 0;
+  uint16_t i;
+  uint8_t j;
+  if (verify) verifyBuffer = (uint8_t *)malloc((MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE));
+  if (useProgMem) progBuffer = (uint8_t *)malloc((MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE));
+  for (i = 0; i < dataSize;) {
+    // determine correct chunk size according to bank position and data size
+    chunkSize = (MPU6050_IMU::MPU6050_DMP_MEMORY_CHUNK_SIZE);
+
+    // make sure we don't go past the data size
+    if (i + chunkSize > dataSize) chunkSize = dataSize - i;
+
+    // make sure this chunk doesn't go past the bank boundary (256 bytes)
+    if (chunkSize > 256 - address) chunkSize = 256 - address;
 
-        I2Cdev::writeBytes(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, progBuffer);
-
-        // verify data if needed
-        if (verify && verifyBuffer) {
-            setMemoryBank(bank);
-            setMemoryStartAddress(address);
-            I2Cdev::readBytes(devAddr,  (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, verifyBuffer);
-            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
-                /*Serial.print("Block write verification error, bank ");
-                Serial.print(bank, DEC);
-                Serial.print(", address ");
-                Serial.print(address, DEC);
-                Serial.print("!\nExpected:");
-                for (j = 0; j < chunkSize; j++) {
-                    Serial.print(" 0x");
-                    if (progBuffer[j] < 16) Serial.print("0");
-                    Serial.print(progBuffer[j], HEX);
-                }
-                Serial.print("\nReceived:");
-                for (uint8_t j = 0; j < chunkSize; j++) {
-                    Serial.print(" 0x");
-                    if (verifyBuffer[i + j] < 16) Serial.print("0");
-                    Serial.print(verifyBuffer[i + j], HEX);
-                }
-                Serial.print("\n");*/
-                free(verifyBuffer);
-                if (useProgMem) free(progBuffer);
-                return false; // uh oh.
-            }
+    if (useProgMem) {
+      // write the chunk of data as specified
+      for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
+    } else {
+      // write the chunk of data as specified
+      progBuffer = (uint8_t *)data + i;
+    }
+
+    I2Cdev::writeBytes(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, progBuffer);
+
+    // verify data if needed
+    if (verify && verifyBuffer) {
+      setMemoryBank(bank);
+      setMemoryStartAddress(address);
+      I2Cdev::readBytes(devAddr, (MPU6050_IMU::MPU6050_RA_MEM_R_W), chunkSize, verifyBuffer);
+      if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+#ifdef DEBUG
+        Serial.print("Block write verification error, bank ");
+        Serial.print(bank, DEC);
+        Serial.print(", address ");
+        Serial.print(address, DEC);
+        Serial.print("!\nExpected:");
+        for (j = 0; j < chunkSize; j++) {
+          Serial.print(" 0x");
+          if (progBuffer[j] < 16) Serial.print("0");
+          Serial.print(progBuffer[j], HEX);
+        }
+        Serial.print("\nReceived:");
+        for (uint8_t j = 0; j < chunkSize; j++) {
+          Serial.print(" 0x");
+          if (verifyBuffer[i + j] < 16) Serial.print("0");
+          Serial.print(verifyBuffer[i + j], HEX);
         }
+        Serial.print("\n");
+#endif
+        free(verifyBuffer);
+        if (useProgMem) free(progBuffer);
+        return false;  // uh oh.
+      }
+    }
 
-        // increase byte index by [chunkSize]
-        i += chunkSize;
+    // increase byte index by [chunkSize]
+    i += chunkSize;
 
-        // uint8_t automatically wraps to 0 at 256
-        address += chunkSize;
+    // uint8_t automatically wraps to 0 at 256
+    address += chunkSize;
 
-        // if we aren't done, update bank (if necessary) and address
-        if (i < dataSize) {
-            if (address == 0) bank++;
-            setMemoryBank(bank);
-            setMemoryStartAddress(address);
-        }
+    // if we aren't done, update bank (if necessary) and address
+    if (i < dataSize) {
+      if (address == 0) bank++;
+      setMemoryBank(bank);
+      setMemoryStartAddress(address);
     }
-    if (verify) free(verifyBuffer);
-    if (useProgMem) free(progBuffer);
-    return true;
+  }
+  if (verify) free(verifyBuffer);
+  if (useProgMem) free(progBuffer);
+  return true;
 }
 bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
-    return writeMemoryBlock(data, dataSize, bank, address, verify, true);
+  return writeMemoryBlock(data, dataSize, bank, address, verify, true);
 }
 bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
-    uint8_t *progBuffer = 0;
-	uint8_t success, special;
-    uint16_t i, j;
+  uint8_t *progBuffer = 0;
+  uint8_t success, special;
+  uint16_t i, j;
+  if (useProgMem) {
+    progBuffer = (uint8_t *)malloc(8);  // assume 8-byte blocks, realloc later if necessary
+  }
+
+  // config set data is a long string of blocks with the following structure:
+  // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
+  uint8_t bank, offset, length;
+  for (i = 0; i < dataSize;) {
     if (useProgMem) {
-        progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
+      bank = pgm_read_byte(data + i++);
+      offset = pgm_read_byte(data + i++);
+      length = pgm_read_byte(data + i++);
+    } else {
+      bank = data[i++];
+      offset = data[i++];
+      length = data[i++];
     }
 
-    // config set data is a long string of blocks with the following structure:
-    // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
-    uint8_t bank, offset, length;
-    for (i = 0; i < dataSize;) {
-        if (useProgMem) {
-            bank = pgm_read_byte(data + i++);
-            offset = pgm_read_byte(data + i++);
-            length = pgm_read_byte(data + i++);
-        } else {
-            bank = data[i++];
-            offset = data[i++];
-            length = data[i++];
-        }
-
-        // write data or perform special action
-        if (length > 0) {
-            // regular block of data to write
-            /*Serial.print("Writing config block to bank ");
-            Serial.print(bank);
-            Serial.print(", offset ");
-            Serial.print(offset);
-            Serial.print(", length=");
-            Serial.println(length);*/
-            if (useProgMem) {
-                if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
-                for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
-            } else {
-                progBuffer = (uint8_t *)data + i;
-            }
-            success = writeMemoryBlock(progBuffer, length, bank, offset, true);
-            i += length;
-        } else {
-            // special instruction
-            // NOTE: this kind of behavior (what and when to do certain things)
-            // is totally undocumented. This code is in here based on observed
-            // behavior only, and exactly why (or even whether) it has to be here
-            // is anybody's guess for now.
-            if (useProgMem) {
-                special = pgm_read_byte(data + i++);
-            } else {
-                special = data[i++];
-            }
-            /*Serial.print("Special command code ");
-            Serial.print(special, HEX);
-            Serial.println(" found...");*/
-            if (special == 0x01) {
-                // enable DMP-related interrupts
-
-                //setIntZeroMotionEnabled(true);
-                //setIntFIFOBufferOverflowEnabled(true);
-                //setIntDMPEnabled(true);
-                I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_INT_ENABLE), 0x32);  // single operation
-
-                success = true;
-            } else {
-                // unknown special command
-                success = false;
-            }
-        }
+    // write data or perform special action
+    if (length > 0) {
+      // regular block of data to write
+#ifdef DEBUG
+      Serial.print("Writing config block to bank ");
+      Serial.print(bank);
+      Serial.print(", offset ");
+      Serial.print(offset);
+      Serial.print(", length=");
+      Serial.println(length);
+#endif
+      if (useProgMem) {
+        if (sizeof(progBuffer) < length) progBuffer = (uint8_t *)realloc(progBuffer, length);
+        for (j = 0; j < length; j++) progBuffer[j] = pgm_read_byte(data + i + j);
+      } else {
+        progBuffer = (uint8_t *)data + i;
+      }
+      success = writeMemoryBlock(progBuffer, length, bank, offset, true);
+      i += length;
+    } else {
+      // special instruction
+      // NOTE: this kind of behavior (what and when to do certain things)
+      // is totally undocumented. This code is in here based on observed
+      // behavior only, and exactly why (or even whether) it has to be here
+      // is anybody's guess for now.
+      if (useProgMem) {
+        special = pgm_read_byte(data + i++);
+      } else {
+        special = data[i++];
+      }
+#ifdef DEBUG
+      Serial.print("Special command code ");
+      Serial.print(special, HEX);
+      Serial.println(" found...");
+#endif
+      if (special == 0x01) {
+        // enable DMP-related interrupts
+
+        // setIntZeroMotionEnabled(true);
+        // setIntFIFOBufferOverflowEnabled(true);
+        // setIntDMPEnabled(true);
+        I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_INT_ENABLE), 0x32);  // single operation
+
+        success = true;
+      } else {
+        // unknown special command
+        success = false;
+      }
+    }
 
-        if (!success) {
-            if (useProgMem) free(progBuffer);
-            return false; // uh oh
-        }
+    if (!success) {
+      if (useProgMem) free(progBuffer);
+      return false;  // uh oh
     }
-    if (useProgMem) free(progBuffer);
-    return true;
+  }
+  if (useProgMem) free(progBuffer);
+  return true;
 }
 bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
-    return writeDMPConfigurationSet(data, dataSize, true);
+  return writeDMPConfigurationSet(data, dataSize, true);
 }
 
 // DMP_CFG_1 register
 
 uint8_t MPU6050::getDMPConfig1() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_DMP_CFG_1), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_CFG_1), buffer);
+  return buffer[0];
 }
 void MPU6050::setDMPConfig1(uint8_t config) {
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_DMP_CFG_1), config);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_CFG_1), config);
 }
 
 // DMP_CFG_2 register
 
 uint8_t MPU6050::getDMPConfig2() {
-    I2Cdev::readByte(devAddr,  (MPU6050_IMU::MPU6050_RA_DMP_CFG_2), buffer);
-    return buffer[0];
+  I2Cdev::readByte(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_CFG_2), buffer);
+  return buffer[0];
 }
 void MPU6050::setDMPConfig2(uint8_t config) {
-    I2Cdev::writeByte(devAddr,  (MPU6050_IMU::MPU6050_RA_DMP_CFG_2), config);
+  I2Cdev::writeByte(devAddr, (MPU6050_IMU::MPU6050_RA_DMP_CFG_2), config);
 }
 
-
 //***************************************************************************************
 //**********************           Calibration Routines            **********************
 //***************************************************************************************
 /**
   @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
 */
-void MPU6050::CalibrateGyro(uint8_t Loops ) {
+void MPU6050::CalibrateGyro(uint8_t Loops) {
   double kP = 0.3;
   double kI = 90;
   float x;
@@ -3276,105 +3284,123 @@ void MPU6050::CalibrateGyro(uint8_t Loops ) {
   kP *= x;
   kI *= x;
 
-  PID( 0x43,  kP, kI,  Loops);
+  PID(0x43, kP, kI, Loops);
 }
 
 /**
   @brief      Fully calibrate Accel from ZERO in about 6-7 Loops 600-700 readings
 */
-void MPU6050::CalibrateAccel(uint8_t Loops ) {
-
-	float kP = 0.3;
-	float kI = 20;
-	float x;
-	x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
-	kP *= x;
-	kI *= x;
-	PID( 0x3B, kP, kI,  Loops);
-}
-
-void MPU6050::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
-	uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13;
-
-	int16_t  Data;
-	float Reading;
-	int16_t BitZero[3];
-	uint8_t shift =(SaveAddress == 0x77)?3:2;
-	float Error, PTerm, ITerm[3];
-	int16_t eSample;
-	uint32_t eSum ;
-	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)
-		Reading = Data;
-		if(SaveAddress != 0x13){
-			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
-			ITerm[i] = ((float)Reading) * 8;
-			} else {
-			ITerm[i] = Reading * 4;
-		}
-	}
-	for (int L = 0; L < Loops; L++) {
-		eSample = 0;
-		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)
-				Reading = Data;
-				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384;	//remove Gravity
-				Error = -Reading;
-				eSum += abs(Reading);
-				PTerm = kP * Error;
-				ITerm[i] += (Error * 0.001) * kI;				// Integral term 1000 Calculations a second = 0.001
-				if(SaveAddress != 0x13){
-					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);
-			}
-			if((c == 99) && eSum > 1000){						// Error is still to great to continue
-				c = 0;
-				Serial.write('*');
-			}
-			if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++;	// Successfully found offsets prepare to  advance
-			if((eSum < 100) && (c > 10) && (eSample >= 10)) break;		// Advance to next Loop
-			delay(1);
-		}
-		Serial.write('.');
-		kP *= .75;
-		kI *= .75;
-		for (int i = 0; i < 3; i++){
-			if(SaveAddress != 0x13) {
-				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);
-		}
-	}
-	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::CalibrateAccel(uint8_t Loops) {
+  float kP = 0.3;
+  float kI = 20;
+  float x;
+  x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
+  kP *= x;
+  kI *= x;
+  PID(0x3B, kP, kI, Loops);
+}
+
+void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops) {
+  uint8_t SaveAddress = (ReadAddress == 0x3B) ? ((getDeviceID() < 0x38) ? 0x06 : 0x77) : 0x13;
+
+  int16_t Data;
+  float Reading;
+  int16_t BitZero[3];
+  uint8_t shift = (SaveAddress == 0x77) ? 3 : 2;
+  float Error, PTerm, ITerm[3];
+  int16_t eSample;
+  uint32_t eSum;
+#ifdef DEBUG
+  Serial.write('>');
+#endif
+  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)
+    Reading = Data;
+    if (SaveAddress != 0x13) {
+      BitZero[i] = Data & 1;  // Capture Bit Zero to properly handle Accelerometer calibration
+      ITerm[i] = ((float)Reading) * 8;
+    } else {
+      ITerm[i] = Reading * 4;
+    }
+  }
+  for (int L = 0; L < Loops; L++) {
+    eSample = 0;
+    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)
+        Reading = Data;
+        if ((ReadAddress == 0x3B) && (i == 2)) Reading -= 16384;  // remove Gravity
+        Error = -Reading;
+        eSum += abs(Reading);
+        PTerm = kP * Error;
+        ITerm[i] += (Error * 0.001) * kI;  // Integral term 1000 Calculations a second = 0.001
+        if (SaveAddress != 0x13) {
+          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);
+      }
+      if ((c == 99) && eSum > 1000) {  // Error is still to great to continue
+        c = 0;
+#ifdef DEBUG
+        Serial.write('*');
+#endif
+      }
+      if ((eSum * ((ReadAddress == 0x3B) ? .05 : 1)) < 5) eSample++;  // Successfully found offsets prepare to  advance
+      if ((eSum < 100) && (c > 10) && (eSample >= 10)) break;         // Advance to next Loop
+      delay(1);
+    }
+#ifdef DEBUG
+    Serial.write('.');
+#endif
+    kP *= .75;
+    kI *= .75;
+    for (int i = 0; i < 3; i++) {
+      if (SaveAddress != 0x13) {
+        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);
+    }
+  }
+  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() {
-	uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? (MPU6050_IMU::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);
-	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);
-	}
-	//	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);
-	//	XG_OFFSET_H_READ_OFFS_USR(Data);
-	printfloatx("", Data[0], 5, 0, ",  ");
-	printfloatx("", Data[1], 5, 0, ",  ");
-	printfloatx("", Data[2], 5, 0, "\n");
+  uint8_t AOffsetRegister = (getDeviceID() < 0x38) ? (MPU6050_IMU::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);
+  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);
+  }
+  //	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);
+  //	XG_OFFSET_H_READ_OFFS_USR(Data);
+  printfloatx("", Data[0], 5, 0, ",  ");
+  printfloatx("", Data[1], 5, 0, ",  ");
+  printfloatx("", Data[2], 5, 0, "\n");
 }