From 4edcf6b32be1689d876ad4d9de4227240588813c Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 28 Jul 2023 12:23:47 +0200
Subject: [PATCH 01/21] Add MPU6050 library and sample code

---
 Sming/Libraries/MPU6050/MPU6050.cpp           | 3656 +++++++++++++++++
 Sming/Libraries/MPU6050/MPU6050.h             | 1042 +++++
 Sming/Libraries/MPU6050/README.md             |    9 +
 Sming/Libraries/MPU6050/component.mk          |    1 +
 Sming/Libraries/MPU6050/helper_3dmath.h       |  243 ++
 samples/Accel_Gyro_MPU6050/Makefile           |    9 +
 samples/Accel_Gyro_MPU6050/README.rst         |    7 +
 .../Accel_Gyro_MPU6050/app/application.cpp    |   42 +
 samples/Accel_Gyro_MPU6050/component.mk       |    2 +
 samples/Accel_Gyro_MPU6050/mpu6050.jpg        |  Bin 0 -> 55832 bytes
 10 files changed, 5011 insertions(+)
 create mode 100644 Sming/Libraries/MPU6050/MPU6050.cpp
 create mode 100644 Sming/Libraries/MPU6050/MPU6050.h
 create mode 100644 Sming/Libraries/MPU6050/README.md
 create mode 100644 Sming/Libraries/MPU6050/component.mk
 create mode 100644 Sming/Libraries/MPU6050/helper_3dmath.h
 create mode 100644 samples/Accel_Gyro_MPU6050/Makefile
 create mode 100644 samples/Accel_Gyro_MPU6050/README.rst
 create mode 100644 samples/Accel_Gyro_MPU6050/app/application.cpp
 create mode 100644 samples/Accel_Gyro_MPU6050/component.mk
 create mode 100644 samples/Accel_Gyro_MPU6050/mpu6050.jpg

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
new file mode 100644
index 0000000000..5d174ab7a1
--- /dev/null
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -0,0 +1,3656 @@
+// I2Cdev library collection - MPU6050 I2C device class
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011
+// (RM-MPU-6000A-00) 8/24/2011 by Jeff Rowberg <jeff@rowberg.net> Updates should
+// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ... - ongoing debug release
+
+// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
+// UNDERGOING ACTIVE DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES.
+// PLEASE KEEP THIS IN MIND IF YOU DECIDE TO USE THIS PARTICULAR CODE FOR
+// ANYTHING.
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#include "MPU6050.h"
+#include <string.h>
+
+#define I2C_NUM I2C_NUM_0
+
+/** Default constructor, uses default I2C address.
+ * @see MPU6050_DEFAULT_ADDRESS
+ */
+MPU6050::MPU6050()
+{
+	devAddr = MPU6050_DEFAULT_ADDRESS;
+}
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+MPU6050::MPU6050(uint8_t address)
+{
+	devAddr = address;
+}
+
+/** Power on and prepare for general usage.
+ * This will activate the device and take it out of sleep mode (which must be
+ * done after start-up). This function also sets both the accelerometer and the
+ * gyroscope to their most sensitive settings, namely +/- 2g and +/- 250
+ * degrees/sec, and sets the clock source to use the X Gyro for reference, which
+ * is slightly better than the default internal clock source.
+ */
+void MPU6050::initialize()
+{
+	setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+	setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+	setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+	setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool MPU6050::testConnection()
+{
+	return getDeviceID() == 0x34;
+}
+
+// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
+
+/** Get the auxiliary I2C supply voltage level.
+ * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
+ * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
+ * the MPU-6000, which does not have a VLOGIC pin.
+ * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
+ */
+uint8_t MPU6050::getAuxVDDIOLevel()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
+	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
+ * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
+ * the MPU-6000, which does not have a VLOGIC pin.
+ * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
+ */
+void MPU6050::setAuxVDDIOLevel(uint8_t level)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
+}
+
+// SMPLRT_DIV register
+
+/** Get gyroscope output rate divider.
+ * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero
+ * Motion detection, and Free Fall detection are all based on the Sample Rate.
+ * The Sample Rate is generated by dividing the gyroscope output rate by
+ * SMPLRT_DIV:
+ *
+ * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+ *
+ * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or
+ * 7), and 1kHz when the DLPF is enabled (see Register 26).
+ *
+ * Note: The accelerometer output rate is 1kHz. This means that for a Sample
+ * Rate greater than 1kHz, the same accelerometer sample may be output to the
+ * FIFO, DMP, and sensor registers more than once.
+ *
+ * For a diagram of the gyroscope and accelerometer signal paths, see Section 8
+ * of the MPU-6000/MPU-6050 Product Specification document.
+ *
+ * @return Current sample rate
+ * @see MPU6050_RA_SMPLRT_DIV
+ */
+uint8_t MPU6050::getRate()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
+	return buffer[0];
+}
+/** Set gyroscope sample rate divider.
+ * @param rate New sample rate divider
+ * @see getRate()
+ * @see MPU6050_RA_SMPLRT_DIV
+ */
+void MPU6050::setRate(uint8_t rate)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
+}
+
+// CONFIG register
+
+/** Get external FSYNC configuration.
+ * Configures the external Frame Synchronization (FSYNC) pin sampling. An
+ * external signal connected to the FSYNC pin can be sampled by configuring
+ * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
+ * strobes may be captured. The latched FSYNC signal will be sampled at the
+ * Sampling Rate, as defined in register 25. After sampling, the latch will
+ * reset to the current FSYNC signal state.
+ *
+ * The sampled value will be reported in place of the least significant bit in
+ * a sensor data register determined by the value of EXT_SYNC_SET according to
+ * the following table.
+ *
+ * <pre>
+ * EXT_SYNC_SET | FSYNC Bit Location
+ * -------------+-------------------
+ * 0            | Input disabled
+ * 1            | TEMP_OUT_L[0]
+ * 2            | GYRO_XOUT_L[0]
+ * 3            | GYRO_YOUT_L[0]
+ * 4            | GYRO_ZOUT_L[0]
+ * 5            | ACCEL_XOUT_L[0]
+ * 6            | ACCEL_YOUT_L[0]
+ * 7            | ACCEL_ZOUT_L[0]
+ * </pre>
+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050::getExternalFrameSync()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set external FSYNC configuration.
+ * @see getExternalFrameSync()
+ * @see MPU6050_RA_CONFIG
+ * @param sync New FSYNC configuration value
+ */
+void MPU6050::setExternalFrameSync(uint8_t sync)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
+}
+/** Get digital low-pass filter configuration.
+ * The DLPF_CFG parameter sets the digital low pass filter configuration. It
+ * also determines the internal sampling rate used by the device as shown in
+ * the table below.
+ *
+ * Note: The accelerometer output rate is 1kHz. This means that for a Sample
+ * Rate greater than 1kHz, the same accelerometer sample may be output to the
+ * FIFO, DMP, and sensor registers more than once.
+ *
+ * <pre>
+ *          |   ACCELEROMETER    |           GYROSCOPE
+ * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
+ * ---------+-----------+--------+-----------+--------+-------------
+ * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
+ * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
+ * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
+ * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
+ * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
+ * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
+ * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
+ * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
+ * </pre>
+ *
+ * @return DLFP configuration
+ * @see MPU6050_RA_CONFIG
+ * @see MPU6050_CFG_DLPF_CFG_BIT
+ * @see MPU6050_CFG_DLPF_CFG_LENGTH
+ */
+uint8_t MPU6050::getDLPFMode()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set digital low-pass filter configuration.
+ * @param mode New DLFP configuration setting
+ * @see getDLPFBandwidth()
+ * @see MPU6050_DLPF_BW_256
+ * @see MPU6050_RA_CONFIG
+ * @see MPU6050_CFG_DLPF_CFG_BIT
+ * @see MPU6050_CFG_DLPF_CFG_LENGTH
+ */
+void MPU6050::setDLPFMode(uint8_t mode)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+}
+
+// GYRO_CONFIG register
+
+/** Get full-scale gyroscope range.
+ * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
+ * as described in the table below.
+ *
+ * <pre>
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * </pre>
+ *
+ * @return Current full-scale gyroscope range setting
+ * @see MPU6050_GYRO_FS_250
+ * @see MPU6050_RA_GYRO_CONFIG
+ * @see MPU6050_GCONFIG_FS_SEL_BIT
+ * @see MPU6050_GCONFIG_FS_SEL_LENGTH
+ */
+uint8_t MPU6050::getFullScaleGyroRange()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set full-scale gyroscope range.
+ * @param range New full-scale gyroscope range value
+ * @see getFullScaleRange()
+ * @see MPU6050_GYRO_FS_250
+ * @see MPU6050_RA_GYRO_CONFIG
+ * @see MPU6050_GCONFIG_FS_SEL_BIT
+ * @see MPU6050_GCONFIG_FS_SEL_LENGTH
+ */
+void MPU6050::setFullScaleGyroRange(uint8_t range)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
+					  range);
+}
+
+// SELF TEST FACTORY TRIM VALUES
+
+/** Get self-test factory trim value for accelerometer X axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_X
+ */
+uint8_t MPU6050::getAccelXSelfTestFactoryTrim()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);
+	return (buffer[0] >> 3) | ((buffer[1] >> 4) & 0x03);
+}
+
+/** Get self-test factory trim value for accelerometer Y axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_Y
+ */
+uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);
+	return (buffer[0] >> 3) | ((buffer[1] >> 2) & 0x03);
+}
+
+/** Get self-test factory trim value for accelerometer Z axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_Z
+ */
+uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer);
+	return (buffer[0] >> 3) | (buffer[1] & 0x03);
+}
+
+/** Get self-test factory trim value for gyro X axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_X
+ */
+uint8_t MPU6050::getGyroXSelfTestFactoryTrim()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer);
+	return (buffer[0] & 0x1F);
+}
+
+/** Get self-test factory trim value for gyro Y axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_Y
+ */
+uint8_t MPU6050::getGyroYSelfTestFactoryTrim()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer);
+	return (buffer[0] & 0x1F);
+}
+
+/** Get self-test factory trim value for gyro Z axis.
+ * @return factory trim value
+ * @see MPU6050_RA_SELF_TEST_Z
+ */
+uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer);
+	return (buffer[0] & 0x1F);
+}
+
+// ACCEL_CONFIG register
+
+/** Get self-test enabled setting for accelerometer X axis.
+ * @return Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+bool MPU6050::getAccelXSelfTest()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
+	return buffer[0];
+}
+/** Get self-test enabled setting for accelerometer X axis.
+ * @param enabled Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+void MPU6050::setAccelXSelfTest(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
+}
+/** Get self-test enabled value for accelerometer Y axis.
+ * @return Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+bool MPU6050::getAccelYSelfTest()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
+	return buffer[0];
+}
+/** Get self-test enabled value for accelerometer Y axis.
+ * @param enabled Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+void MPU6050::setAccelYSelfTest(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
+}
+/** Get self-test enabled value for accelerometer Z axis.
+ * @return Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+bool MPU6050::getAccelZSelfTest()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
+	return buffer[0];
+}
+/** Set self-test enabled value for accelerometer Z axis.
+ * @param enabled Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+void MPU6050::setAccelZSelfTest(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
+}
+/** Get full-scale accelerometer range.
+ * The FS_SEL parameter allows setting the full-scale range of the accelerometer
+ * sensors, as described in the table below.
+ *
+ * <pre>
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * </pre>
+ *
+ * @return Current full-scale accelerometer range setting
+ * @see MPU6050_ACCEL_FS_2
+ * @see MPU6050_RA_ACCEL_CONFIG
+ * @see MPU6050_ACONFIG_AFS_SEL_BIT
+ * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
+ */
+uint8_t MPU6050::getFullScaleAccelRange()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set full-scale accelerometer range.
+ * @param range New full-scale accelerometer range setting
+ * @see getFullScaleAccelRange()
+ */
+void MPU6050::setFullScaleAccelRange(uint8_t range)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
+					  range);
+}
+/** Get the high-pass filter configuration.
+ * The DHPF is a filter module in the path leading to motion detectors (Free
+ * Fall, Motion threshold, and Zero Motion). The high pass filter output is not
+ * available to the data registers (see Figure in Section 8 of the MPU-6000/
+ * MPU-6050 Product Specification document).
+ *
+ * The high pass filter has three modes:
+ *
+ * <pre>
+ *    Reset: The filter output settles to zero within one sample. This
+ *           effectively disables the high pass filter. This mode may be toggled
+ *           to quickly settle the filter.
+ *
+ *    On:    The high pass filter will pass signals above the cut off frequency.
+ *
+ *    Hold:  When triggered, the filter holds the present sample. The filter
+ *           output will be the difference between the input sample and the held
+ *           sample.
+ * </pre>
+ *
+ * <pre>
+ * ACCEL_HPF | Filter Mode | Cut-off Frequency
+ * ----------+-------------+------------------
+ * 0         | Reset       | None
+ * 1         | On          | 5Hz
+ * 2         | On          | 2.5Hz
+ * 3         | On          | 1.25Hz
+ * 4         | On          | 0.63Hz
+ * 7         | Hold        | None
+ * </pre>
+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050::getDHPFMode()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set the high-pass filter configuration.
+ * @param bandwidth New high-pass filter configuration
+ * @see setDHPFMode()
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+void MPU6050::setDHPFMode(uint8_t bandwidth)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
+					  bandwidth);
+}
+
+// FF_THR register
+
+/** Get free-fall event acceleration threshold.
+ * This register configures the detection threshold for Free Fall event
+ * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the
+ * absolute value of the accelerometer measurements for the three axes are each
+ * less than the detection threshold. This condition increments the Free Fall
+ * duration counter (Register 30). The Free Fall interrupt is triggered when the
+ * Free Fall duration counter reaches the time specified in FF_DUR.
+ *
+ * For more details on the Free Fall detection interrupt, see Section 8.2 of the
+ * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
+ * 58 of this document.
+ *
+ * @return Current free-fall acceleration threshold value (LSB = 2mg)
+ * @see MPU6050_RA_FF_THR
+ */
+uint8_t MPU6050::getFreefallDetectionThreshold()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer);
+	return buffer[0];
+}
+/** Get free-fall event acceleration threshold.
+ * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
+ * @see getFreefallDetectionThreshold()
+ * @see MPU6050_RA_FF_THR
+ */
+void MPU6050::setFreefallDetectionThreshold(uint8_t threshold)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
+}
+
+// FF_DUR register
+
+/** Get free-fall event duration threshold.
+ * This register configures the duration counter threshold for Free Fall event
+ * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
+ * of 1 LSB = 1 ms.
+ *
+ * The Free Fall duration counter increments while the absolute value of the
+ * accelerometer measurements are each less than the detection threshold
+ * (Register 29). The Free Fall interrupt is triggered when the Free Fall
+ * duration counter reaches the time specified in this register.
+ *
+ * For more details on the Free Fall detection interrupt, see Section 8.2 of
+ * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current free-fall duration threshold value (LSB = 1ms)
+ * @see MPU6050_RA_FF_DUR
+ */
+uint8_t MPU6050::getFreefallDetectionDuration()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer);
+	return buffer[0];
+}
+/** Get free-fall event duration threshold.
+ * @param duration New free-fall duration threshold value (LSB = 1ms)
+ * @see getFreefallDetectionDuration()
+ * @see MPU6050_RA_FF_DUR
+ */
+void MPU6050::setFreefallDetectionDuration(uint8_t duration)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
+}
+
+// MOT_THR register
+
+/** Get motion detection event acceleration threshold.
+ * This register configures the detection threshold for Motion interrupt
+ * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the
+ * absolute value of any of the accelerometer measurements exceeds this Motion
+ * detection threshold. This condition increments the Motion detection duration
+ * counter (Register 32). The Motion detection interrupt is triggered when the
+ * Motion Detection counter reaches the time count specified in MOT_DUR
+ * (Register 32).
+ *
+ * The Motion interrupt will indicate the axis and polarity of detected motion
+ * in MOT_DETECT_STATUS (Register 97).
+ *
+ * For more details on the Motion detection interrupt, see Section 8.3 of the
+ * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
+ * 58 of this document.
+ *
+ * @return Current motion detection acceleration threshold value (LSB = 2mg)
+ * @see MPU6050_RA_MOT_THR
+ */
+uint8_t MPU6050::getMotionDetectionThreshold()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer);
+	return buffer[0];
+}
+/** Set motion detection event acceleration threshold.
+ * @param threshold New motion detection acceleration threshold value (LSB =
+ * 2mg)
+ * @see getMotionDetectionThreshold()
+ * @see MPU6050_RA_MOT_THR
+ */
+void MPU6050::setMotionDetectionThreshold(uint8_t threshold)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
+}
+
+// MOT_DUR register
+
+/** Get motion detection event duration threshold.
+ * This register configures the duration counter threshold for Motion interrupt
+ * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit
+ * of 1LSB = 1ms. The Motion detection duration counter increments when the
+ * absolute value of any of the accelerometer measurements exceeds the Motion
+ * detection threshold (Register 31). The Motion detection interrupt is
+ * triggered when the Motion detection counter reaches the time count specified
+ * in this register.
+ *
+ * For more details on the Motion detection interrupt, see Section 8.3 of the
+ * MPU-6000/MPU-6050 Product Specification document.
+ *
+ * @return Current motion detection duration threshold value (LSB = 1ms)
+ * @see MPU6050_RA_MOT_DUR
+ */
+uint8_t MPU6050::getMotionDetectionDuration()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer);
+	return buffer[0];
+}
+/** Set motion detection event duration threshold.
+ * @param duration New motion detection duration threshold value (LSB = 1ms)
+ * @see getMotionDetectionDuration()
+ * @see MPU6050_RA_MOT_DUR
+ */
+void MPU6050::setMotionDetectionDuration(uint8_t duration)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
+}
+
+// ZRMOT_THR register
+
+/** Get zero motion detection event acceleration threshold.
+ * This register configures the detection threshold for Zero Motion interrupt
+ * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when
+ * the absolute value of the accelerometer measurements for the 3 axes are each
+ * less than the detection threshold. This condition increments the Zero Motion
+ * duration counter (Register 34). The Zero Motion interrupt is triggered when
+ * the Zero Motion duration counter reaches the time count specified in
+ * ZRMOT_DUR (Register 34).
+ *
+ * Unlike Free Fall or Motion detection, Zero Motion detection triggers an
+ * interrupt both when Zero Motion is first detected and when Zero Motion is no
+ * longer detected.
+ *
+ * When a zero motion event is detected, a Zero Motion Status will be indicated
+ * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion
+ * condition is detected, the status bit is set to 1. When a zero-motion-to-
+ * motion condition is detected, the status bit is set to 0.
+ *
+ * For more details on the Zero Motion detection interrupt, see Section 8.4 of
+ * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current zero motion detection acceleration threshold value (LSB =
+ * 2mg)
+ * @see MPU6050_RA_ZRMOT_THR
+ */
+uint8_t MPU6050::getZeroMotionDetectionThreshold()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer);
+	return buffer[0];
+}
+/** Set zero motion detection event acceleration threshold.
+ * @param threshold New zero motion detection acceleration threshold value (LSB
+ * = 2mg)
+ * @see getZeroMotionDetectionThreshold()
+ * @see MPU6050_RA_ZRMOT_THR
+ */
+void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
+}
+
+// ZRMOT_DUR register
+
+/** Get zero motion detection event duration threshold.
+ * This register configures the duration counter threshold for Zero Motion
+ * interrupt generation. The duration counter ticks at 16 Hz, therefore
+ * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter
+ * increments while the absolute value of the accelerometer measurements are
+ * each less than the detection threshold (Register 33). The Zero Motion
+ * interrupt is triggered when the Zero Motion duration counter reaches the time
+ * count specified in this register.
+ *
+ * For more details on the Zero Motion detection interrupt, see Section 8.4 of
+ * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current zero motion detection duration threshold value (LSB = 64ms)
+ * @see MPU6050_RA_ZRMOT_DUR
+ */
+uint8_t MPU6050::getZeroMotionDetectionDuration()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
+	return buffer[0];
+}
+/** Set zero motion detection event duration threshold.
+ * @param duration New zero motion detection duration threshold value (LSB =
+ * 1ms)
+ * @see getZeroMotionDetectionDuration()
+ * @see MPU6050_RA_ZRMOT_DUR
+ */
+void MPU6050::setZeroMotionDetectionDuration(uint8_t duration)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
+}
+
+// FIFO_EN register
+
+/** Get temperature FIFO enabled value.
+ * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
+ * 66) to be written into the FIFO buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getTempFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set temperature FIFO enabled value.
+ * @param enabled New temperature FIFO enabled value
+ * @see getTempFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setTempFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
+}
+/** Get gyroscope X-axis FIFO enabled value.
+ * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
+ * 68) to be written into the FIFO buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getXGyroFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set gyroscope X-axis FIFO enabled value.
+ * @param enabled New gyroscope X-axis FIFO enabled value
+ * @see getXGyroFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setXGyroFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
+}
+/** Get gyroscope Y-axis FIFO enabled value.
+ * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
+ * 70) to be written into the FIFO buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getYGyroFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set gyroscope Y-axis FIFO enabled value.
+ * @param enabled New gyroscope Y-axis FIFO enabled value
+ * @see getYGyroFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setYGyroFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
+}
+/** Get gyroscope Z-axis FIFO enabled value.
+ * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
+ * 72) to be written into the FIFO buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getZGyroFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set gyroscope Z-axis FIFO enabled value.
+ * @param enabled New gyroscope Z-axis FIFO enabled value
+ * @see getZGyroFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setZGyroFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
+}
+/** Get accelerometer FIFO enabled value.
+ * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
+ * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
+ * written into the FIFO buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getAccelFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set accelerometer FIFO enabled value.
+ * @param enabled New accelerometer FIFO enabled value
+ * @see getAccelFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setAccelFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
+}
+/** Get Slave 2 FIFO enabled value.
+ * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
+ * associated with Slave 2 to be written into the FIFO buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getSlave2FIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set Slave 2 FIFO enabled value.
+ * @param enabled New Slave 2 FIFO enabled value
+ * @see getSlave2FIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setSlave2FIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled);
+}
+/** Get Slave 1 FIFO enabled value.
+ * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
+ * associated with Slave 1 to be written into the FIFO buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getSlave1FIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set Slave 1 FIFO enabled value.
+ * @param enabled New Slave 1 FIFO enabled value
+ * @see getSlave1FIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setSlave1FIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
+}
+/** Get Slave 0 FIFO enabled value.
+ * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
+ * associated with Slave 0 to be written into the FIFO buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050::getSlave0FIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set Slave 0 FIFO enabled value.
+ * @param enabled New Slave 0 FIFO enabled value
+ * @see getSlave0FIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050::setSlave0FIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
+}
+
+// I2C_MST_CTRL register
+
+/** Get multi-master enabled value.
+ * Multi-master capability allows multiple I2C masters to operate on the same
+ * bus. In circuits where multi-master capability is required, set MULT_MST_EN
+ * to 1. This will increase current drawn by approximately 30uA.
+ *
+ * In circuits where multi-master capability is required, the state of the I2C
+ * bus must always be monitored by each separate I2C Master. Before an I2C
+ * Master can assume arbitration of the bus, it must first confirm that no other
+ * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the
+ * MPU-60X0's bus arbitration detection logic is turned on, enabling it to
+ * detect when the bus is available.
+ *
+ * @return Current multi-master enabled value
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+bool MPU6050::getMultiMasterEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set multi-master enabled value.
+ * @param enabled New multi-master enabled value
+ * @see getMultiMasterEnabled()
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050::setMultiMasterEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
+}
+/** Get wait-for-external-sensor-data enabled value.
+ * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
+ * delayed until External Sensor data from the Slave Devices are loaded into the
+ * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
+ * data (i.e. from gyro and accel) and external sensor data have been loaded to
+ * their respective data registers (i.e. the data is synced) when the Data Ready
+ * interrupt is triggered.
+ *
+ * @return Current wait-for-external-sensor-data enabled value
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+bool MPU6050::getWaitForExternalSensorEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
+	return buffer[0];
+}
+/** Set wait-for-external-sensor-data enabled value.
+ * @param enabled New wait-for-external-sensor-data enabled value
+ * @see getWaitForExternalSensorEnabled()
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050::setWaitForExternalSensorEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
+}
+/** Get Slave 3 FIFO enabled value.
+ * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
+ * associated with Slave 3 to be written into the FIFO buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+bool MPU6050::getSlave3FIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set Slave 3 FIFO enabled value.
+ * @param enabled New Slave 3 FIFO enabled value
+ * @see getSlave3FIFOEnabled()
+ * @see MPU6050_RA_MST_CTRL
+ */
+void MPU6050::setSlave3FIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
+}
+/** Get slave read/write transition enabled value.
+ * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
+ * read to the next slave read. If the bit equals 0, there will be a restart
+ * between reads. If the bit equals 1, there will be a stop followed by a start
+ * of the following read. When a write transaction follows a read transaction,
+ * the stop followed by a start of the successive write will be always used.
+ *
+ * @return Current slave read/write transition enabled value
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+bool MPU6050::getSlaveReadWriteTransitionEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
+	return buffer[0];
+}
+/** Set slave read/write transition enabled value.
+ * @param enabled New slave read/write transition enabled value
+ * @see getSlaveReadWriteTransitionEnabled()
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
+}
+/** Get I2C master clock speed.
+ * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
+ * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
+ * the following table:
+ *
+ * <pre>
+ * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
+ * ------------+------------------------+-------------------
+ * 0           | 348kHz                 | 23
+ * 1           | 333kHz                 | 24
+ * 2           | 320kHz                 | 25
+ * 3           | 308kHz                 | 26
+ * 4           | 296kHz                 | 27
+ * 5           | 286kHz                 | 28
+ * 6           | 276kHz                 | 29
+ * 7           | 267kHz                 | 30
+ * 8           | 258kHz                 | 31
+ * 9           | 500kHz                 | 16
+ * 10          | 471kHz                 | 17
+ * 11          | 444kHz                 | 18
+ * 12          | 421kHz                 | 19
+ * 13          | 400kHz                 | 20
+ * 14          | 381kHz                 | 21
+ * 15          | 364kHz                 | 22
+ * </pre>
+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050::getMasterClockSpeed()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set I2C master clock speed.
+ * @reparam speed Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050::setMasterClockSpeed(uint8_t speed)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
+}
+
+// I2C_SLV* registers (Slave 0-3)
+
+/** Get the I2C address of the specified slave (0-3).
+ * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
+ * operation, and if it is cleared, then it's a write operation. The remaining
+ * bits (6-0) are the 7-bit device address of the slave device.
+ *
+ * In read mode, the result of the read is placed in the lowest available
+ * EXT_SENS_DATA register. For further information regarding the allocation of
+ * read results, please refer to the EXT_SENS_DATA register description
+ * (Registers 73 - 96).
+ *
+ * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
+ * characteristics, and so it has its own functions (getSlave4* and setSlave4*).
+ *
+ * I2C data transactions are performed at the Sample Rate, as defined in
+ * Register 25. The user is responsible for ensuring that I2C data transactions
+ * to and from each enabled Slave can be completed within a single period of the
+ * Sample Rate.
+ *
+ * The I2C slave access rate can be reduced relative to the Sample Rate. This
+ * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a
+ * slave's access rate is reduced relative to the Sample Rate is determined by
+ * I2C_MST_DELAY_CTRL (Register 103).
+ *
+ * The processing order for the slaves is fixed. The sequence followed for
+ * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a
+ * particular Slave is disabled it will be skipped.
+ *
+ * Each slave can either be accessed at the sample rate or at a reduced sample
+ * rate. In a case where some slaves are accessed at the Sample Rate and some
+ * slaves are accessed at the reduced rate, the sequence of accessing the slaves
+ * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will
+ * be skipped if their access rate dictates that they should not be accessed
+ * during that particular cycle. For further information regarding the reduced
+ * access rate, please refer to Register 52. Whether a slave is accessed at the
+ * Sample Rate or at the reduced rate is determined by the Delay Enable bits in
+ * Register 103.
+ *
+ * @param num Slave number (0-3)
+ * @return Current address for specified slave
+ * @see MPU6050_RA_I2C_SLV0_ADDR
+ */
+uint8_t MPU6050::getSlaveAddress(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readByte(devAddr, 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)
+ * @param address New address for specified slave
+ * @see getSlaveAddress()
+ * @see MPU6050_RA_I2C_SLV0_ADDR
+ */
+void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeByte(devAddr, 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
+ * register address is stored in this MPU register.
+ *
+ * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
+ * characteristics, and so it has its own functions.
+ *
+ * @param num Slave number (0-3)
+ * @return Current active register for specified slave
+ * @see MPU6050_RA_I2C_SLV0_REG
+ */
+uint8_t MPU6050::getSlaveRegister(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readByte(devAddr, 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)
+ * @param reg New active register for specified slave
+ * @see getSlaveRegister()
+ * @see MPU6050_RA_I2C_SLV0_REG
+ */
+void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeByte(devAddr, 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
+ * cleared to 0, this bit disables Slave 0 from data transfer operations.
+ * @param num Slave number (0-3)
+ * @return Current enabled value for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+bool MPU6050::getSlaveEnabled(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set the enabled value for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param enabled New enabled value for specified slave
+ * @see getSlaveEnabled()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
+}
+/** 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,
+ * the high and low bytes of a word pair are swapped. Please refer to
+ * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
+ * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
+ * registers in the order they were transferred.
+ *
+ * @param num Slave number (0-3)
+ * @return Current word pair byte-swapping enabled value for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+bool MPU6050::getSlaveWordByteSwap(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
+	return buffer[0];
+}
+/** Set word pair byte-swapping enabled for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param enabled New word pair byte-swapping enabled value for specified slave
+ * @see getSlaveWordByteSwap()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
+}
+/** Get write mode for the specified slave (0-3).
+ * When set to 1, the transaction will read or write data only. When cleared to
+ * 0, the transaction will write a register address prior to reading or writing
+ * data. This should equal 0 when specifying the register address within the
+ * Slave device to/from which the ensuing data transaction will take place.
+ *
+ * @param num Slave number (0-3)
+ * @return Current write mode for specified slave (0 = register address + data,
+ * 1 = data only)
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+bool MPU6050::getSlaveWriteMode(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, 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)
+ * @param mode New write mode for specified slave (0 = register address + data,
+ * 1 = data only)
+ * @see getSlaveWriteMode()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
+}
+/** Get word pair grouping order offset for the specified slave (0-3).
+ * This sets specifies the grouping order of word pairs received from registers.
+ * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
+ * then odd register addresses) are paired to form a word. When set to 1, bytes
+ * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
+ * register addresses) are paired to form a word.
+ *
+ * @param num Slave number (0-3)
+ * @return Current word pair grouping order offset for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
+	return buffer[0];
+}
+/** Set word pair grouping order offset for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param enabled New word pair grouping order offset for specified slave
+ * @see getSlaveWordGroupOffset()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
+}
+/** 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
+ * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
+ * @param num Slave number (0-3)
+ * @return Number of bytes to read for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+uint8_t MPU6050::getSlaveDataLength(uint8_t num)
+{
+	if(num > 3)
+		return 0;
+	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set number of bytes to read for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param length Number of bytes to read for specified slave
+ * @see getSlaveDataLength()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
+ */
+void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
+					  length);
+}
+
+// I2C_SLV* registers (Slave 4)
+
+/** Get the I2C address of Slave 4.
+ * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
+ * operation, and if it is cleared, then it's a write operation. The remaining
+ * bits (6-0) are the 7-bit device address of the slave device.
+ *
+ * @return Current address for Slave 4
+ * @see getSlaveAddress()
+ * @see MPU6050_RA_I2C_SLV4_ADDR
+ */
+uint8_t MPU6050::getSlave4Address()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
+	return buffer[0];
+}
+/** Set the I2C address of Slave 4.
+ * @param address New address for Slave 4
+ * @see getSlave4Address()
+ * @see MPU6050_RA_I2C_SLV4_ADDR
+ */
+void MPU6050::setSlave4Address(uint8_t address)
+{
+	I2Cdev::writeByte(devAddr, 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
+ * register address is stored in this MPU register.
+ *
+ * @return Current active register for Slave 4
+ * @see MPU6050_RA_I2C_SLV4_REG
+ */
+uint8_t MPU6050::getSlave4Register()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer);
+	return buffer[0];
+}
+/** Set the active internal register for Slave 4.
+ * @param reg New active register for Slave 4
+ * @see getSlave4Register()
+ * @see MPU6050_RA_I2C_SLV4_REG
+ */
+void MPU6050::setSlave4Register(uint8_t reg)
+{
+	I2Cdev::writeByte(devAddr, 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
+ * is set 1 (set to read), this register has no effect.
+ * @param data New byte to write to Slave 4
+ * @see MPU6050_RA_I2C_SLV4_DO
+ */
+void MPU6050::setSlave4OutputByte(uint8_t data)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data);
+}
+/** Get the enabled value for the Slave 4.
+ * When set to 1, this bit enables Slave 4 for data transfer operations. When
+ * cleared to 0, this bit disables Slave 4 from data transfer operations.
+ * @return Current enabled value for Slave 4
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+bool MPU6050::getSlave4Enabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set the enabled value for Slave 4.
+ * @param enabled New enabled value for Slave 4
+ * @see getSlave4Enabled()
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+void MPU6050::setSlave4Enabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
+}
+/** Get the enabled value for Slave 4 transaction interrupts.
+ * When set to 1, this bit enables the generation of an interrupt signal upon
+ * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
+ * generation of an interrupt signal upon completion of a Slave 4 transaction.
+ * The interrupt status can be observed in Register 54.
+ *
+ * @return Current enabled value for Slave 4 transaction interrupts.
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+bool MPU6050::getSlave4InterruptEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set the enabled value for Slave 4 transaction interrupts.
+ * @param enabled New enabled value for Slave 4 transaction interrupts.
+ * @see getSlave4InterruptEnabled()
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+void MPU6050::setSlave4InterruptEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
+}
+/** Get write mode for Slave 4.
+ * When set to 1, the transaction will read or write data only. When cleared to
+ * 0, the transaction will write a register address prior to reading or writing
+ * data. This should equal 0 when specifying the register address within the
+ * Slave device to/from which the ensuing data transaction will take place.
+ *
+ * @return Current write mode for Slave 4 (0 = register address + data, 1 = data
+ * only)
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+bool MPU6050::getSlave4WriteMode()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
+	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)
+ * @see getSlave4WriteMode()
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+void MPU6050::setSlave4WriteMode(bool mode)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
+}
+/** Get Slave 4 master delay value.
+ * This configures the reduced access rate of I2C slaves relative to the Sample
+ * Rate. When a slave's access rate is decreased relative to the Sample Rate,
+ * the slave is accessed every:
+ *
+ *     1 / (1 + I2C_MST_DLY) samples
+ *
+ * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
+ * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
+ * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
+ * further information regarding the Sample Rate, please refer to register 25.
+ *
+ * @return Current Slave 4 master delay value
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+uint8_t MPU6050::getSlave4MasterDelay()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set Slave 4 master delay value.
+ * @param delay New Slave 4 master delay value
+ * @see getSlave4MasterDelay()
+ * @see MPU6050_RA_I2C_SLV4_CTRL
+ */
+void MPU6050::setSlave4MasterDelay(uint8_t delay)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH,
+					  delay);
+}
+/** Get last available byte read from Slave 4.
+ * This register stores the data read from Slave 4. This field is populated
+ * after a read transaction.
+ * @return Last available byte read from to Slave 4
+ * @see MPU6050_RA_I2C_SLV4_DI
+ */
+uint8_t MPU6050::getSlate4InputByte()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
+	return buffer[0];
+}
+
+// I2C_MST_STATUS register
+
+/** Get FSYNC interrupt status.
+ * This bit reflects the status of the FSYNC interrupt from an external device
+ * into the MPU-60X0. This is used as a way to pass an external interrupt
+ * through the MPU-60X0 to the host application processor. When set to 1, this
+ * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
+ * (Register 55).
+ * @return FSYNC interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getPassthroughStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 4 transaction done status.
+ * Automatically sets to 1 when a Slave 4 transaction has completed. This
+ * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
+ * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
+ * I2C_SLV4_CTRL register (Register 52).
+ * @return Slave 4 transaction done status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave4IsDone()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
+	return buffer[0];
+}
+/** Get master arbitration lost status.
+ * This bit automatically sets to 1 when the I2C Master has lost arbitration of
+ * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
+ * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Master arbitration lost status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getLostArbitration()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 4 NACK status.
+ * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+ * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
+ * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Slave 4 NACK interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave4Nack()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 3 NACK status.
+ * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+ * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
+ * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Slave 3 NACK interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave3Nack()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 2 NACK status.
+ * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+ * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
+ * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Slave 2 NACK interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave2Nack()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 1 NACK status.
+ * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+ * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
+ * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Slave 1 NACK interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave1Nack()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
+	return buffer[0];
+}
+/** Get Slave 0 NACK status.
+ * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+ * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
+ * bit in the INT_ENABLE register (Register 56) is asserted.
+ * @return Slave 0 NACK interrupt status
+ * @see MPU6050_RA_I2C_MST_STATUS
+ */
+bool MPU6050::getSlave0Nack()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
+	return buffer[0];
+}
+
+// INT_PIN_CFG register
+
+/** Get interrupt logic level mode.
+ * Will be set 0 for active-high, 1 for active-low.
+ * @return Current interrupt mode (0=active-high, 1=active-low)
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_LEVEL_BIT
+ */
+bool MPU6050::getInterruptMode()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
+	return buffer[0];
+}
+/** Set interrupt logic level mode.
+ * @param mode New interrupt mode (0=active-high, 1=active-low)
+ * @see getInterruptMode()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_LEVEL_BIT
+ */
+void MPU6050::setInterruptMode(bool mode)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
+}
+/** Get interrupt drive mode.
+ * Will be set 0 for push-pull, 1 for open-drain.
+ * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_OPEN_BIT
+ */
+bool MPU6050::getInterruptDrive()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
+	return buffer[0];
+}
+/** Set interrupt drive mode.
+ * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
+ * @see getInterruptDrive()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_OPEN_BIT
+ */
+void MPU6050::setInterruptDrive(bool drive)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
+}
+/** Get interrupt latch mode.
+ * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
+ * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+ */
+bool MPU6050::getInterruptLatch()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set interrupt latch mode.
+ * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+ * @see getInterruptLatch()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+ */
+void MPU6050::setInterruptLatch(bool latch)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
+}
+/** Get interrupt latch clear mode.
+ * Will be set 0 for status-read-only, 1 for any-register-read.
+ * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+ */
+bool MPU6050::getInterruptLatchClear()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
+	return buffer[0];
+}
+/** Set interrupt latch clear mode.
+ * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
+ * @see getInterruptLatchClear()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+ */
+void MPU6050::setInterruptLatchClear(bool clear)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
+}
+/** Get FSYNC interrupt logic level mode.
+ * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
+ * @see getFSyncInterruptMode()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+ */
+bool MPU6050::getFSyncInterruptLevel()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
+	return buffer[0];
+}
+/** Set FSYNC interrupt logic level mode.
+ * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
+ * @see getFSyncInterruptMode()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+ */
+void MPU6050::setFSyncInterruptLevel(bool level)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+}
+/** Get FSYNC pin interrupt enabled setting.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled setting
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+ */
+bool MPU6050::getFSyncInterruptEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set FSYNC pin interrupt enabled setting.
+ * @param enabled New FSYNC pin interrupt enabled setting
+ * @see getFSyncInterruptEnabled()
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+ */
+void MPU6050::setFSyncInterruptEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
+}
+/** Get I2C bypass enabled status.
+ * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
+ * 0, the host application processor will be able to directly access the
+ * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+ * application processor will not be able to directly access the auxiliary I2C
+ * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+ * bit[5]).
+ * @return Current I2C bypass enabled status
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+ */
+bool MPU6050::getI2CBypassEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
+	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
+ * 0, the host application processor will be able to directly access the
+ * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+ * application processor will not be able to directly access the auxiliary I2C
+ * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+ * bit[5]).
+ * @param enabled New I2C bypass enabled status
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+ */
+void MPU6050::setI2CBypassEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
+}
+/** Get reference clock output enabled status.
+ * When this bit is equal to 1, a reference clock output is provided at the
+ * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+ * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+ * Specification document.
+ * @return Current reference clock output enabled status
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+ */
+bool MPU6050::getClockOutputEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set reference clock output enabled status.
+ * When this bit is equal to 1, a reference clock output is provided at the
+ * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+ * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+ * Specification document.
+ * @param enabled New reference clock output enabled status
+ * @see MPU6050_RA_INT_PIN_CFG
+ * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+ */
+void MPU6050::setClockOutputEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
+}
+
+// INT_ENABLE register
+
+/** Get full interrupt enabled status.
+ * Full register byte for all interrupts, for quick reading. Each bit will be
+ * set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FF_BIT
+ **/
+uint8_t MPU6050::getIntEnabled()
+{
+	I2Cdev::readByte(devAddr, 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
+ * set 0 for disabled, 1 for enabled.
+ * @param enabled New interrupt enabled status
+ * @see getIntFreefallEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FF_BIT
+ **/
+void MPU6050::setIntEnabled(uint8_t enabled)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
+}
+/** Get Free Fall interrupt enabled status.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FF_BIT
+ **/
+bool MPU6050::getIntFreefallEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
+	return buffer[0];
+}
+/** Set Free Fall interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntFreefallEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FF_BIT
+ **/
+void MPU6050::setIntFreefallEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
+}
+/** Get Motion Detection interrupt enabled status.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_MOT_BIT
+ **/
+bool MPU6050::getIntMotionEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
+	return buffer[0];
+}
+/** Set Motion Detection interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntMotionEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_MOT_BIT
+ **/
+void MPU6050::setIntMotionEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
+}
+/** Get Zero Motion Detection interrupt enabled status.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_ZMOT_BIT
+ **/
+bool MPU6050::getIntZeroMotionEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+	return buffer[0];
+}
+/** Set Zero Motion Detection interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntZeroMotionEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_ZMOT_BIT
+ **/
+void MPU6050::setIntZeroMotionEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
+}
+/** Get FIFO Buffer Overflow interrupt enabled status.
+ * Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+ **/
+bool MPU6050::getIntFIFOBufferOverflowEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+	return buffer[0];
+}
+/** Set FIFO Buffer Overflow interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntFIFOBufferOverflowEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+ **/
+void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+}
+/** Get I2C Master interrupt enabled status.
+ * This enables any of the I2C Master interrupt sources to generate an
+ * interrupt. Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+ **/
+bool MPU6050::getIntI2CMasterEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+	return buffer[0];
+}
+/** Set I2C Master interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntI2CMasterEnabled()
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+ **/
+void MPU6050::setIntI2CMasterEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
+}
+/** Get Data Ready interrupt enabled setting.
+ * This event occurs each time a write operation to all of the sensor registers
+ * has been completed. Will be set 0 for disabled, 1 for enabled.
+ * @return Current interrupt enabled status
+ * @see MPU6050_RA_INT_ENABLE
+ * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+ */
+bool MPU6050::getIntDataReadyEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+	return buffer[0];
+}
+/** Set Data Ready interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntDataReadyEnabled()
+ * @see MPU6050_RA_INT_CFG
+ * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+ */
+void MPU6050::setIntDataReadyEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
+}
+
+// INT_STATUS register
+
+/** Get full set of interrupt status bits.
+ * These bits clear to 0 after the register has been read. Very useful
+ * for getting multiple INT statuses, since each single bit read clears
+ * all of them because it has to read the whole byte.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ */
+uint8_t MPU6050::getIntStatus()
+{
+	I2Cdev::readByte(devAddr, 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
+ * generated. The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_FF_BIT
+ */
+bool MPU6050::getIntFreefallStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
+	return buffer[0];
+}
+/** Get Motion Detection interrupt status.
+ * This bit automatically sets to 1 when a Motion Detection interrupt has been
+ * generated. The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_MOT_BIT
+ */
+bool MPU6050::getIntMotionStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
+	return buffer[0];
+}
+/** Get Zero Motion Detection interrupt status.
+ * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
+ * been generated. The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_ZMOT_BIT
+ */
+bool MPU6050::getIntZeroMotionStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
+	return buffer[0];
+}
+/** Get FIFO Buffer Overflow interrupt status.
+ * This bit automatically sets to 1 when a Free Fall interrupt has been
+ * generated. The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+ */
+bool MPU6050::getIntFIFOBufferOverflowStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+	return buffer[0];
+}
+/** Get I2C Master interrupt status.
+ * This bit automatically sets to 1 when an I2C Master interrupt has been
+ * generated. For a list of I2C Master interrupts, please refer to Register 54.
+ * The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+ */
+bool MPU6050::getIntI2CMasterStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
+	return buffer[0];
+}
+/** Get Data Ready interrupt status.
+ * This bit automatically sets to 1 when a Data Ready interrupt has been
+ * generated. The bit clears to 0 after the register has been read.
+ * @return Current interrupt status
+ * @see MPU6050_RA_INT_STATUS
+ * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+ */
+bool MPU6050::getIntDataReadyStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
+	return buffer[0];
+}
+
+// ACCEL_*OUT_* registers
+
+/** Get raw 9-axis motion sensor readings (accel/gyro/compass).
+ * FUNCTION NOT FULLY IMPLEMENTED YET.
+ * @param ax 16-bit signed integer container for accelerometer X-axis value
+ * @param ay 16-bit signed integer container for accelerometer Y-axis value
+ * @param az 16-bit signed integer container for accelerometer Z-axis value
+ * @param gx 16-bit signed integer container for gyroscope X-axis value
+ * @param gy 16-bit signed integer container for gyroscope Y-axis value
+ * @param gz 16-bit signed integer container for gyroscope Z-axis value
+ * @param mx 16-bit signed integer container for magnetometer X-axis value
+ * @param my 16-bit signed integer container for magnetometer Y-axis value
+ * @param mz 16-bit signed integer container for magnetometer Z-axis value
+ * @see getMotion6()
+ * @see getAcceleration()
+ * @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
+}
+/** Get raw 6-axis motion sensor readings (accel/gyro).
+ * Retrieves all currently available motion sensor values.
+ * @param ax 16-bit signed integer container for accelerometer X-axis value
+ * @param ay 16-bit signed integer container for accelerometer Y-axis value
+ * @param az 16-bit signed integer container for accelerometer Z-axis value
+ * @param gx 16-bit signed integer container for gyroscope X-axis value
+ * @param gy 16-bit signed integer container for gyroscope Y-axis value
+ * @param gz 16-bit signed integer container for gyroscope Z-axis value
+ * @see getAcceleration()
+ * @see getRotation()
+ * @see MPU6050_RA_ACCEL_XOUT_H
+ */
+void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz)
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
+	*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.
+ * Accelerometer measurements are written to these registers at the Sample Rate
+ * as defined in Register 25.
+ *
+ * The accelerometer measurement registers, along with the temperature
+ * measurement registers, gyroscope measurement registers, and external sensor
+ * data registers, are composed of two sets of registers: an internal register
+ * set and a user-facing read register set.
+ *
+ * The data within the accelerometer sensors' internal register set is always
+ * updated at the Sample Rate. Meanwhile, the user-facing read register set
+ * duplicates the internal register set's data values whenever the serial
+ * interface is idle. This guarantees that a burst read of sensor registers will
+ * read measurements from the same sampling instant. Note that if burst reads
+ * are not used, the user is responsible for ensuring a set of single byte reads
+ * correspond to a single sampling instant by checking the Data Ready interrupt.
+ *
+ * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
+ * (Register 28). For each full scale setting, the accelerometers' sensitivity
+ * per LSB in ACCEL_xOUT is shown in the table below:
+ *
+ * <pre>
+ * AFS_SEL | Full Scale Range | LSB Sensitivity
+ * --------+------------------+----------------
+ * 0       | +/- 2g           | 8192 LSB/mg
+ * 1       | +/- 4g           | 4096 LSB/mg
+ * 2       | +/- 8g           | 2048 LSB/mg
+ * 3       | +/- 16g          | 1024 LSB/mg
+ * </pre>
+ *
+ * @param x 16-bit signed integer container for X-axis acceleration
+ * @param y 16-bit signed integer container for Y-axis acceleration
+ * @param z 16-bit signed integer container for Z-axis acceleration
+ * @see MPU6050_RA_GYRO_XOUT_H
+ */
+void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z)
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
+	*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
+ * @see getMotion6()
+ * @see MPU6050_RA_ACCEL_XOUT_H
+ */
+int16_t MPU6050::getAccelerationX()
+{
+	I2Cdev::readBytes(devAddr, 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
+ * @see getMotion6()
+ * @see MPU6050_RA_ACCEL_YOUT_H
+ */
+int16_t MPU6050::getAccelerationY()
+{
+	I2Cdev::readBytes(devAddr, 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
+ * @see getMotion6()
+ * @see MPU6050_RA_ACCEL_ZOUT_H
+ */
+int16_t MPU6050::getAccelerationZ()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// TEMP_OUT_* registers
+
+/** Get current internal temperature.
+ * @return Temperature reading in 16-bit 2's complement format
+ * @see MPU6050_RA_TEMP_OUT_H
+ */
+int16_t MPU6050::getTemperature()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// GYRO_*OUT_* registers
+
+/** Get 3-axis gyroscope readings.
+ * These gyroscope measurement registers, along with the accelerometer
+ * measurement registers, temperature measurement registers, and external sensor
+ * data registers, are composed of two sets of registers: an internal register
+ * set and a user-facing read register set.
+ * The data within the gyroscope sensors' internal register set is always
+ * updated at the Sample Rate. Meanwhile, the user-facing read register set
+ * duplicates the internal register set's data values whenever the serial
+ * interface is idle. This guarantees that a burst read of sensor registers will
+ * read measurements from the same sampling instant. Note that if burst reads
+ * are not used, the user is responsible for ensuring a set of single byte reads
+ * correspond to a single sampling instant by checking the Data Ready interrupt.
+ *
+ * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
+ * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
+ * LSB in GYRO_xOUT is shown in the table below:
+ *
+ * <pre>
+ * FS_SEL | Full Scale Range   | LSB Sensitivity
+ * -------+--------------------+----------------
+ * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
+ * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
+ * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
+ * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
+ * </pre>
+ *
+ * @param x 16-bit signed integer container for X-axis rotation
+ * @param y 16-bit signed integer container for Y-axis rotation
+ * @param z 16-bit signed integer container for Z-axis rotation
+ * @see getMotion6()
+ * @see MPU6050_RA_GYRO_XOUT_H
+ */
+void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z)
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
+	*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
+ * @see getMotion6()
+ * @see MPU6050_RA_GYRO_XOUT_H
+ */
+int16_t MPU6050::getRotationX()
+{
+	I2Cdev::readBytes(devAddr, 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
+ * @see getMotion6()
+ * @see MPU6050_RA_GYRO_YOUT_H
+ */
+int16_t MPU6050::getRotationY()
+{
+	I2Cdev::readBytes(devAddr, 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
+ * @see getMotion6()
+ * @see MPU6050_RA_GYRO_ZOUT_H
+ */
+int16_t MPU6050::getRotationZ()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// EXT_SENS_DATA_* registers
+
+/** Read single byte from external sensor data register.
+ * These registers store data read from external sensors by the Slave 0, 1, 2,
+ * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
+ * I2C_SLV4_DI (Register 53).
+ *
+ * External sensor data is written to these registers at the Sample Rate as
+ * defined in Register 25. This access rate can be reduced by using the Slave
+ * Delay Enable registers (Register 103).
+ *
+ * External sensor data registers, along with the gyroscope measurement
+ * registers, accelerometer measurement registers, and temperature measurement
+ * registers, are composed of two sets of registers: an internal register set
+ * and a user-facing read register set.
+ *
+ * The data within the external sensors' internal register set is always updated
+ * at the Sample Rate (or the reduced access rate) whenever the serial interface
+ * is idle. This guarantees that a burst read of sensor registers will read
+ * measurements from the same sampling instant. Note that if burst reads are not
+ * used, the user is responsible for ensuring a set of single byte reads
+ * correspond to a single sampling instant by checking the Data Ready interrupt.
+ *
+ * Data is placed in these external sensor data registers according to
+ * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
+ * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
+ * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
+ * defined in Register 25) or delayed rate (if specified in Register 52 and
+ * 103). During each Sample cycle, slave reads are performed in order of Slave
+ * number. If all slaves are enabled with more than zero bytes to be read, the
+ * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
+ *
+ * Each enabled slave will have EXT_SENS_DATA registers associated with it by
+ * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
+ * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
+ * change the higher numbered slaves' associated registers. Furthermore, if
+ * fewer total bytes are being read from the external sensors as a result of
+ * such a change, then the data remaining in the registers which no longer have
+ * an associated slave device (i.e. high numbered registers) will remain in
+ * these previously allocated registers unless reset.
+ *
+ * If the sum of the read lengths of all SLVx transactions exceed the number of
+ * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
+ * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
+ * the slaves cannot be greater than 24 or some bytes will be lost.
+ *
+ * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
+ * information regarding the characteristics of Slave 4, please refer to
+ * Registers 49 to 53.
+ *
+ * EXAMPLE:
+ * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
+ * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
+ * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
+ * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
+ * will be associated with Slave 1. If Slave 2 is enabled as well, registers
+ * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
+ *
+ * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
+ * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
+ * instead.
+ *
+ * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
+ * If a slave is disabled at any time, the space initially allocated to the
+ * slave in the EXT_SENS_DATA register, will remain associated with that slave.
+ * This is to avoid dynamic adjustment of the register allocation.
+ *
+ * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
+ * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
+ *
+ * This above is also true if one of the slaves gets NACKed and stops
+ * functioning.
+ *
+ * @param position Starting position (0-23)
+ * @return Byte read from register
+ */
+uint8_t MPU6050::getExternalSensorByte(int position)
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
+	return buffer[0];
+}
+/** Read word (2 bytes) from external sensor data registers.
+ * @param position Starting position (0-21)
+ * @return Word read from register
+ * @see getExternalSensorByte()
+ */
+uint16_t MPU6050::getExternalSensorWord(int position)
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
+	return (((uint16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Read double word (4 bytes) from external sensor data registers.
+ * @param position Starting position (0-20)
+ * @return Double word read from registers
+ * @see getExternalSensorByte()
+ */
+uint32_t MPU6050::getExternalSensorDWord(int position)
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer);
+	return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3];
+}
+
+// MOT_DETECT_STATUS register
+
+/** Get full motion detection status register content (all bits).
+ * @return Motion detection status byte
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ */
+uint8_t MPU6050::getMotionStatus()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer);
+	return buffer[0];
+}
+/** Get X-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_XNEG_BIT
+ */
+bool MPU6050::getXNegMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
+	return buffer[0];
+}
+/** Get X-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_XPOS_BIT
+ */
+bool MPU6050::getXPosMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
+	return buffer[0];
+}
+/** Get Y-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_YNEG_BIT
+ */
+bool MPU6050::getYNegMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
+	return buffer[0];
+}
+/** Get Y-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_YPOS_BIT
+ */
+bool MPU6050::getYPosMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
+	return buffer[0];
+}
+/** Get Z-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_ZNEG_BIT
+ */
+bool MPU6050::getZNegMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
+	return buffer[0];
+}
+/** Get Z-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_ZPOS_BIT
+ */
+bool MPU6050::getZPosMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
+	return buffer[0];
+}
+/** Get zero motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_ZRMOT_BIT
+ */
+bool MPU6050::getZeroMotionDetected()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
+	return buffer[0];
+}
+
+// I2C_SLV*_DO register
+
+/** Write byte to Data Output container for specified slave.
+ * This register holds the output data written into Slave when Slave is set to
+ * write mode. For further information regarding Slave control, please
+ * refer to Registers 37 to 39 and immediately following.
+ * @param num Slave number (0-3)
+ * @param data Byte to write
+ * @see MPU6050_RA_I2C_SLV0_DO
+ */
+void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
+{
+	if(num > 3)
+		return;
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
+}
+
+// I2C_MST_DELAY_CTRL register
+
+/** Get external data shadow delay enabled status.
+ * This register is used to specify the timing of external sensor data
+ * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
+ * sensor data is delayed until all data has been received.
+ * @return Current external data shadow delay enabled status.
+ * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+ * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+ */
+bool MPU6050::getExternalShadowDelayEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer);
+	return buffer[0];
+}
+/** Set external data shadow delay enabled status.
+ * @param enabled New external data shadow delay enabled status.
+ * @see getExternalShadowDelayEnabled()
+ * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+ * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+ */
+void MPU6050::setExternalShadowDelayEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
+}
+/** Get slave delay enabled status.
+ * When a particular slave delay is enabled, the rate of access for the that
+ * slave device is reduced. When a slave's access rate is decreased relative to
+ * the Sample Rate, the slave is accessed every:
+ *
+ *     1 / (1 + I2C_MST_DLY) Samples
+ *
+ * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
+ * and DLPF_CFG (register 26).
+ *
+ * For further information regarding I2C_MST_DLY, please refer to register 52.
+ * For further information regarding the Sample Rate, please refer to
+ * register 25.
+ *
+ * @param num Slave number (0-4)
+ * @return Current slave delay enabled status.
+ * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+ * @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_RA_I2C_MST_DELAY_CTRL, num, buffer);
+	return buffer[0];
+}
+/** Set slave delay enabled status.
+ * @param num Slave number (0-4)
+ * @param enabled New slave delay enabled status.
+ * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+ * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+ */
+void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
+}
+
+// SIGNAL_PATH_RESET register
+
+/** Reset gyroscope signal path.
+ * The reset will revert the signal path analog to digital converters and
+ * filters to their power up configurations.
+ * @see MPU6050_RA_SIGNAL_PATH_RESET
+ * @see MPU6050_PATHRESET_GYRO_RESET_BIT
+ */
+void MPU6050::resetGyroscopePath()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
+}
+/** Reset accelerometer signal path.
+ * The reset will revert the signal path analog to digital converters and
+ * filters to their power up configurations.
+ * @see MPU6050_RA_SIGNAL_PATH_RESET
+ * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
+ */
+void MPU6050::resetAccelerometerPath()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
+}
+/** Reset temperature sensor signal path.
+ * The reset will revert the signal path analog to digital converters and
+ * filters to their power up configurations.
+ * @see MPU6050_RA_SIGNAL_PATH_RESET
+ * @see MPU6050_PATHRESET_TEMP_RESET_BIT
+ */
+void MPU6050::resetTemperaturePath()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
+}
+
+// MOT_DETECT_CTRL register
+
+/** Get accelerometer power-on delay.
+ * The accelerometer data path provides samples to the sensor registers, Motion
+ * detection, Zero Motion detection, and Free Fall detection modules. The
+ * signal path contains filters which must be flushed on wake-up with new
+ * samples before the detection modules begin operations. The default wake-up
+ * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
+ * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
+ * any value above zero unless instructed otherwise by InvenSense. Please refer
+ * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
+ * further information regarding the detection modules.
+ * @return Current accelerometer power-on delay
+ * @see MPU6050_RA_MOT_DETECT_CTRL
+ * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+ */
+uint8_t MPU6050::getAccelerometerPowerOnDelay()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+					 MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set accelerometer power-on delay.
+ * @param delay New accelerometer power-on delay (0-3)
+ * @see getAccelerometerPowerOnDelay()
+ * @see MPU6050_RA_MOT_DETECT_CTRL
+ * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+ */
+void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+					  MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+}
+/** Get Free Fall detection counter decrement configuration.
+ * Detection is registered by the Free Fall detection module after accelerometer
+ * measurements meet their respective threshold conditions over a specified
+ * number of samples. When the threshold conditions are met, the corresponding
+ * detection counter increments by 1. The user may control the rate at which the
+ * detection counter decrements when the threshold condition is not met by
+ * configuring FF_COUNT. The decrement rate can be set according to the
+ * following table:
+ *
+ * <pre>
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * </pre>
+ *
+ * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
+ * reset the counter to 0. For further information on Free Fall detection,
+ * please refer to Registers 29 to 32.
+ *
+ * @return Current decrement configuration
+ * @see MPU6050_RA_MOT_DETECT_CTRL
+ * @see MPU6050_DETECT_FF_COUNT_BIT
+ */
+uint8_t MPU6050::getFreefallDetectionCounterDecrement()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set Free Fall detection counter decrement configuration.
+ * @param decrement New decrement configuration value
+ * @see getFreefallDetectionCounterDecrement()
+ * @see MPU6050_RA_MOT_DETECT_CTRL
+ * @see MPU6050_DETECT_FF_COUNT_BIT
+ */
+void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
+					  decrement);
+}
+/** Get Motion detection counter decrement configuration.
+ * Detection is registered by the Motion detection module after accelerometer
+ * measurements meet their respective threshold conditions over a specified
+ * number of samples. When the threshold conditions are met, the corresponding
+ * detection counter increments by 1. The user may control the rate at which the
+ * detection counter decrements when the threshold condition is not met by
+ * configuring MOT_COUNT. The decrement rate can be set according to the
+ * following table:
+ *
+ * <pre>
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * </pre>
+ *
+ * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
+ * reset the counter to 0. For further information on Motion detection,
+ * please refer to Registers 29 to 32.
+ *
+ */
+uint8_t MPU6050::getMotionDetectionCounterDecrement()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set Motion detection counter decrement configuration.
+ * @param decrement New decrement configuration value
+ * @see getMotionDetectionCounterDecrement()
+ * @see MPU6050_RA_MOT_DETECT_CTRL
+ * @see MPU6050_DETECT_MOT_COUNT_BIT
+ */
+void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
+					  MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
+}
+
+// USER_CTRL register
+
+/** Get FIFO enabled status.
+ * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
+ * cannot be written to or read from while disabled. The FIFO buffer's state
+ * does not change unless the MPU-60X0 is power cycled.
+ * @return Current FIFO enabled status
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_FIFO_EN_BIT
+ */
+bool MPU6050::getFIFOEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set FIFO enabled status.
+ * @param enabled New FIFO enabled status
+ * @see getFIFOEnabled()
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_FIFO_EN_BIT
+ */
+void MPU6050::setFIFOEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
+}
+/** Get I2C Master Mode enabled status.
+ * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
+ * external sensor slave devices on the auxiliary I2C bus. When this bit is
+ * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
+ * driven by the primary I2C bus (SDA and SCL). This is a precondition to
+ * enabling Bypass Mode. For further information regarding Bypass Mode, please
+ * refer to Register 55.
+ * @return Current I2C Master Mode enabled status
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+ */
+bool MPU6050::getI2CMasterModeEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
+	return buffer[0];
+}
+/** Set I2C Master Mode enabled status.
+ * @param enabled New I2C Master Mode enabled status
+ * @see getI2CMasterModeEnabled()
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+ */
+void MPU6050::setI2CMasterModeEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
+}
+/** Switch from I2C to SPI mode (MPU-6000 only)
+ * If this is set, the primary SPI interface will be enabled in place of the
+ * disabled primary I2C interface.
+ */
+void MPU6050::switchSPIEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
+ * bit automatically clears to 0 after the reset has been triggered.
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_FIFO_RESET_BIT
+ */
+void MPU6050::resetFIFO()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
+}
+/** Reset the I2C Master.
+ * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
+ * This bit automatically clears to 0 after the reset has been triggered.
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
+ */
+void MPU6050::resetI2CMaster()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
+}
+/** Reset all sensor registers and signal paths.
+ * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
+ * accelerometers, and temperature sensor). This operation will also clear the
+ * sensor registers. This bit automatically clears to 0 after the reset has been
+ * triggered.
+ *
+ * When resetting only the signal path (and not the sensor registers), please
+ * use Register 104, SIGNAL_PATH_RESET.
+ *
+ * @see MPU6050_RA_USER_CTRL
+ * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
+ */
+void MPU6050::resetSensors()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
+}
+
+// PWR_MGMT_1 register
+
+/** Trigger a full device reset.
+ * A small delay of ~50ms may be desirable after triggering a reset.
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_DEVICE_RESET_BIT
+ */
+void MPU6050::reset()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
+}
+/** Get sleep mode status.
+ * Setting the SLEEP bit in the register puts the device into very low power
+ * sleep mode. In this mode, only the serial interface and internal registers
+ * remain active, allowing for a very low standby current. Clearing this bit
+ * puts the device back into normal mode. To save power, the individual standby
+ * selections for each of the gyros should be used if any gyro axis is not used
+ * by the application.
+ * @return Current sleep mode enabled status
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_SLEEP_BIT
+ */
+bool MPU6050::getSleepEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
+	return buffer[0];
+}
+/** Set sleep mode status.
+ * @param enabled New sleep mode enabled status
+ * @see getSleepEnabled()
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_SLEEP_BIT
+ */
+void MPU6050::setSleepEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
+}
+/** Get wake cycle enabled status.
+ * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
+ * between sleep mode and waking up to take a single sample of data from active
+ * sensors at a rate determined by LP_WAKE_CTRL (register 108).
+ * @return Current sleep mode enabled status
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_CYCLE_BIT
+ */
+bool MPU6050::getWakeCycleEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
+	return buffer[0];
+}
+/** Set wake cycle enabled status.
+ * @param enabled New sleep mode enabled status
+ * @see getWakeCycleEnabled()
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_CYCLE_BIT
+ */
+void MPU6050::setWakeCycleEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
+}
+/** Get temperature sensor enabled status.
+ * Control the usage of the internal temperature sensor.
+ *
+ * Note: this register stores the *disabled* value, but for consistency with the
+ * rest of the code, the function is named and used with standard true/false
+ * values to indicate whether the sensor is enabled or disabled, respectively.
+ *
+ * @return Current temperature sensor enabled status
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_TEMP_DIS_BIT
+ */
+bool MPU6050::getTempSensorEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
+	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
+ * rest of the code, the function is named and used with standard true/false
+ * values to indicate whether the sensor is enabled or disabled, respectively.
+ *
+ * @param enabled New temperature sensor enabled status
+ * @see getTempSensorEnabled()
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_TEMP_DIS_BIT
+ */
+void MPU6050::setTempSensorEnabled(bool enabled)
+{
+	// 1 is actually disabled here
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
+}
+/** Get clock source setting.
+ * @return Current clock source setting
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_CLKSEL_BIT
+ * @see MPU6050_PWR1_CLKSEL_LENGTH
+ */
+uint8_t MPU6050::getClockSource()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set clock source setting.
+ * An internal 8MHz oscillator, gyroscope based clock, or external sources can
+ * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
+ * or an external source is chosen as the clock source, the MPU-60X0 can operate
+ * in low power modes with the gyroscopes disabled.
+ *
+ * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
+ * However, it is highly recommended that the device be configured to use one of
+ * the gyroscopes (or an external clock source) as the clock reference for
+ * improved stability. The clock source can be selected according to the
+ * following table:
+ *
+ * <pre>
+ * CLK_SEL | Clock Source
+ * --------+--------------------------------------
+ * 0       | Internal oscillator
+ * 1       | PLL with X Gyro reference
+ * 2       | PLL with Y Gyro reference
+ * 3       | PLL with Z Gyro reference
+ * 4       | PLL with external 32.768kHz reference
+ * 5       | PLL with external 19.2MHz reference
+ * 6       | Reserved
+ * 7       | Stops the clock and keeps the timing generator in reset
+ * </pre>
+ *
+ * @param source New clock source setting
+ * @see getClockSource()
+ * @see MPU6050_RA_PWR_MGMT_1
+ * @see MPU6050_PWR1_CLKSEL_BIT
+ * @see MPU6050_PWR1_CLKSEL_LENGTH
+ */
+void MPU6050::setClockSource(uint8_t source)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
+}
+
+// PWR_MGMT_2 register
+
+/** Get wake frequency in Accel-Only Low Power Mode.
+ * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
+ * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
+ * the device will power off all devices except for the primary I2C interface,
+ * waking only the accelerometer at fixed intervals to take a single
+ * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
+ * as shown below:
+ *
+ * <pre>
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * </pre>
+ *
+ * For further information regarding the MPU-60X0's power modes, please refer to
+ * Register 107.
+ *
+ * @return Current wake frequency
+ * @see MPU6050_RA_PWR_MGMT_2
+ */
+uint8_t MPU6050::getWakeFrequency()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
+					 buffer);
+	return buffer[0];
+}
+/** Set wake frequency in Accel-Only Low Power Mode.
+ * @param frequency New wake frequency
+ * @see MPU6050_RA_PWR_MGMT_2
+ */
+void MPU6050::setWakeFrequency(uint8_t frequency)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
+					  frequency);
+}
+
+/** Get X-axis accelerometer standby enabled status.
+ * If enabled, the X-axis will not gather or report data (or use power).
+ * @return Current X-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_XA_BIT
+ */
+bool MPU6050::getStandbyXAccelEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
+	return buffer[0];
+}
+/** Set X-axis accelerometer standby enabled status.
+ * @param New X-axis standby enabled status
+ * @see getStandbyXAccelEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_XA_BIT
+ */
+void MPU6050::setStandbyXAccelEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
+}
+/** Get Y-axis accelerometer standby enabled status.
+ * If enabled, the Y-axis will not gather or report data (or use power).
+ * @return Current Y-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_YA_BIT
+ */
+bool MPU6050::getStandbyYAccelEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
+	return buffer[0];
+}
+/** Set Y-axis accelerometer standby enabled status.
+ * @param New Y-axis standby enabled status
+ * @see getStandbyYAccelEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_YA_BIT
+ */
+void MPU6050::setStandbyYAccelEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
+}
+/** Get Z-axis accelerometer standby enabled status.
+ * If enabled, the Z-axis will not gather or report data (or use power).
+ * @return Current Z-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_ZA_BIT
+ */
+bool MPU6050::getStandbyZAccelEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
+	return buffer[0];
+}
+/** Set Z-axis accelerometer standby enabled status.
+ * @param New Z-axis standby enabled status
+ * @see getStandbyZAccelEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_ZA_BIT
+ */
+void MPU6050::setStandbyZAccelEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
+}
+/** Get X-axis gyroscope standby enabled status.
+ * If enabled, the X-axis will not gather or report data (or use power).
+ * @return Current X-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_XG_BIT
+ */
+bool MPU6050::getStandbyXGyroEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
+	return buffer[0];
+}
+/** Set X-axis gyroscope standby enabled status.
+ * @param New X-axis standby enabled status
+ * @see getStandbyXGyroEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_XG_BIT
+ */
+void MPU6050::setStandbyXGyroEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
+}
+/** Get Y-axis gyroscope standby enabled status.
+ * If enabled, the Y-axis will not gather or report data (or use power).
+ * @return Current Y-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_YG_BIT
+ */
+bool MPU6050::getStandbyYGyroEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
+	return buffer[0];
+}
+/** Set Y-axis gyroscope standby enabled status.
+ * @param New Y-axis standby enabled status
+ * @see getStandbyYGyroEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_YG_BIT
+ */
+void MPU6050::setStandbyYGyroEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
+}
+/** Get Z-axis gyroscope standby enabled status.
+ * If enabled, the Z-axis will not gather or report data (or use power).
+ * @return Current Z-axis standby enabled status
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_ZG_BIT
+ */
+bool MPU6050::getStandbyZGyroEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
+	return buffer[0];
+}
+/** Set Z-axis gyroscope standby enabled status.
+ * @param New Z-axis standby enabled status
+ * @see getStandbyZGyroEnabled()
+ * @see MPU6050_RA_PWR_MGMT_2
+ * @see MPU6050_PWR2_STBY_ZG_BIT
+ */
+void MPU6050::setStandbyZGyroEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO buffer size.
+ * This value indicates the number of bytes stored in the FIFO buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO buffer
+ * and it is directly proportional to the number of samples available given the
+ * set of sensor data bound to be stored in the FIFO (register 35 and 36).
+ * @return Current FIFO buffer size
+ */
+uint16_t MPU6050::getFIFOCount()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
+	return (((uint16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO buffer.
+ * This register is used to read and write data from the FIFO buffer. Data is
+ * written to the FIFO in order of register number (from lowest to highest). If
+ * all the FIFO enable flags (see below) are enabled and all External Sensor
+ * Data registers (Registers 73 to 96) are associated with a Slave device, the
+ * contents of registers 59 through 96 will be written in order at the Sample
+ * Rate.
+ *
+ * The contents of the sensor data registers (Registers 59 to 96) are written
+ * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
+ * in FIFO_EN (Register 35). An additional flag for the sensor data registers
+ * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
+ *
+ * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
+ * automatically set to 1. This bit is located in INT_STATUS (Register 58).
+ * When the FIFO buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO buffer is empty, reading this register will return the last byte
+ * that was previously read from the FIFO until new data is available. The user
+ * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO buffer
+ */
+uint8_t MPU6050::getFIFOByte()
+{
+	I2Cdev::readByte(devAddr, 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_RA_FIFO_R_W, length, data);
+	} else {
+		*data = 0;
+	}
+}
+/** Write byte to FIFO buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050::setFIFOByte(uint8_t data)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
+}
+
+// WHO_AM_I register
+
+/** Get Device ID.
+ * This register is used to verify the identity of the device (0b110100, 0x34).
+ * @return Device ID (6 bits only! should be 0x34)
+ * @see MPU6050_RA_WHO_AM_I
+ * @see MPU6050_WHO_AM_I_BIT
+ * @see MPU6050_WHO_AM_I_LENGTH
+ */
+uint8_t MPU6050::getDeviceID()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
+	return buffer[0];
+}
+/** Set Device ID.
+ * Write a new ID into the WHO_AM_I register (no idea why this should ever be
+ * necessary though).
+ * @param id New device ID to set.
+ * @see getDeviceID()
+ * @see MPU6050_RA_WHO_AM_I
+ * @see MPU6050_WHO_AM_I_BIT
+ * @see MPU6050_WHO_AM_I_LENGTH
+ */
+void MPU6050::setDeviceID(uint8_t id)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
+}
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+
+uint8_t MPU6050::getOTPBankValid()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
+	return buffer[0];
+}
+void MPU6050::setOTPBankValid(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050::getXGyroOffsetTC()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+	return buffer[0];
+}
+void MPU6050::setXGyroOffsetTC(int8_t offset)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// YG_OFFS_TC register
+
+int8_t MPU6050::getYGyroOffsetTC()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+	return buffer[0];
+}
+void MPU6050::setYGyroOffsetTC(int8_t offset)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// ZG_OFFS_TC register
+
+int8_t MPU6050::getZGyroOffsetTC()
+{
+	I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
+	return buffer[0];
+}
+void MPU6050::setZGyroOffsetTC(int8_t offset)
+{
+	I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// X_FINE_GAIN register
+
+int8_t MPU6050::getXFineGain()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
+	return buffer[0];
+}
+void MPU6050::setXFineGain(int8_t gain)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050::getYFineGain()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
+	return buffer[0];
+}
+void MPU6050::setYFineGain(int8_t gain)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050::getZFineGain()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
+	return buffer[0];
+}
+void MPU6050::setZFineGain(int8_t gain)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050::getXAccelOffset()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setXAccelOffset(int16_t offset)
+{
+	I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050::getYAccelOffset()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setYAccelOffset(int16_t offset)
+{
+	I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050::getZAccelOffset()
+{
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setZAccelOffset(int16_t offset)
+{
+	I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050::getXGyroOffset()
+{
+	I2Cdev::readBytes(devAddr, 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_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050::getYGyroOffset()
+{
+	I2Cdev::readBytes(devAddr, 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_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050::getZGyroOffset()
+{
+	I2Cdev::readBytes(devAddr, 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_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050::getIntPLLReadyEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+	return buffer[0];
+}
+void MPU6050::setIntPLLReadyEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050::getIntDMPEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+	return buffer[0];
+}
+void MPU6050::setIntDMPEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050::getDMPInt5Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getDMPInt4Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getDMPInt3Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getDMPInt2Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getDMPInt1Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getDMPInt0Status()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
+	return buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050::getIntPLLReadyStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+	return buffer[0];
+}
+bool MPU6050::getIntDMPStatus()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
+	return buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050::getDMPEnabled()
+{
+	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
+	return buffer[0];
+}
+void MPU6050::setDMPEnabled(bool enabled)
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050::resetDMP()
+{
+	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050::setMemoryStartAddress(uint8_t address)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050::readMemoryByte()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
+	return buffer[0];
+}
+void MPU6050::writeMemoryByte(uint8_t data)
+{
+	I2Cdev::writeByte(devAddr, 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_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;
+
+		// read the chunk of data as specified
+		I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
+
+		// increase byte index by [chunkSize]
+		i += 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);
+		}
+	}
+}
+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_DMP_MEMORY_CHUNK_SIZE);
+	if(useProgMem)
+		progBuffer = (uint8_t*)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+	for(i = 0; i < dataSize;) {
+		// determine correct chunk size according to bank position and data size
+		chunkSize = 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;
+		}
+
+		I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+		// verify data if needed
+		if(verify && verifyBuffer) {
+			printf("VERIFY\n");
+			setMemoryBank(bank);
+			setMemoryStartAddress(address);
+			I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+			if(memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+				printf("Block write verification error, bank \n");
+				/*Serial.print("Block write verification error, bank ");*/
+				printf("bank %d", bank);
+				printf(", address ");
+				printf("%d", address);
+				printf("!\nExpected:");
+				for(j = 0; j < chunkSize; j++) {
+					printf("%#04x", progBuffer[j]);
+				}
+				printf("\nReceived:");
+				for(uint8_t j = 0; j < chunkSize; j++) {
+					printf("%#04x", verifyBuffer[i + j]);
+				}
+				printf("\n");
+				// free(verifyBuffer);
+				// if (useProgMem) free(progBuffer);
+				// return false; // uh oh.
+			}
+		}
+
+		// increase byte index by [chunkSize]
+		i += 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(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, false);
+}
+bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, bool useProgMem)
+{
+	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) {
+			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_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(useProgMem)
+		free(progBuffer);
+	return true;
+}
+bool MPU6050::writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize)
+{
+	return writeDMPConfigurationSet(data, dataSize, true);
+}
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050::getDMPConfig1()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
+	return buffer[0];
+}
+void MPU6050::setDMPConfig1(uint8_t config)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050::getDMPConfig2()
+{
+	I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
+	return buffer[0];
+}
+void MPU6050::setDMPConfig2(uint8_t config)
+{
+	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
+
+/**
+ * calibration
+ *
+ */
+
+/**
+  @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
+*/
+void MPU6050::CalibrateGyro(uint8_t Loops)
+{
+	double kP = 0.3;
+	double kI = 90;
+	float x;
+	x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
+	kP *= x;
+	kI *= x;
+
+	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);
+}
+
+/**
+ *
+ * @param ReadAddress
+ * @param kP
+ * @param kI
+ * @param 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;
+	for(int i = 0; i < 3; i++) {
+		I2Cdev::readWord(devAddr, SaveAddress + (i * shift),
+						 (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::readWord(devAddr, ReadAddress + (i * 2),
+								 (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::writeWord(devAddr, SaveAddress + (i * shift), Data);
+			}
+			if((c == 99) && eSum > 1000) { // Error is still to great to continue
+				c = 0;
+			}
+			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);
+		}
+		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::writeWord(devAddr, SaveAddress + (i * shift), Data);
+		}
+	}
+	resetFIFO();
+	resetDMP();
+}
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
new file mode 100644
index 0000000000..0e0bda8f62
--- /dev/null
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -0,0 +1,1042 @@
+
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011
+// (RM-MPU-6000A-00) 10/3/2011 by Jeff Rowberg <jeff@rowberg.net> Updates should
+// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ... - ongoing debug release
+
+// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
+// UNDERGOING ACTIVE DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES.
+// PLEASE KEEP THIS IN MIND IF YOU DECIDE TO USE THIS PARTICULAR CODE FOR
+// ANYTHING.
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _MPU6050_H_
+#define _MPU6050_H_
+
+#include "helper_3dmath.h"
+#include <Libraries/I2Cdev/I2Cdev.h>
+
+// supporting link:
+// http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 also:
+// http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
+
+#undef pgm_read_byte
+#define pgm_read_byte(addr) (*(const unsigned char*)(addr))
+
+//#define PROGMEM /* empty */
+//#define pgm_read_byte(x) (*(x))
+//#define pgm_read_word(x) (*(x))
+//#define pgm_read_float(x) (*(x))
+//#define PSTR(STR) STR
+
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
+#define MPU6050_ADDRESS_AD0_LOW 0x68  // address pin low (GND), default for InvenSense evaluation board
+#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
+#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
+
+#define MPU6050_RA_XG_OFFS_TC 0x00  //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_YG_OFFS_TC 0x01  //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_ZG_OFFS_TC 0x02  //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
+#define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
+#define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
+#define MPU6050_RA_XA_OFFS_H 0x06   //[15:0] XA_OFFS
+#define MPU6050_RA_XA_OFFS_L_TC 0x07
+#define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
+#define MPU6050_RA_YA_OFFS_L_TC 0x09
+#define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
+#define MPU6050_RA_ZA_OFFS_L_TC 0x0B
+#define MPU6050_RA_SELF_TEST_X 0x0D  //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Y 0x0E  //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Z 0x0F  //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_A 0x10  //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
+#define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
+#define MPU6050_RA_XG_OFFS_USRL 0x14
+#define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
+#define MPU6050_RA_YG_OFFS_USRL 0x16
+#define MPU6050_RA_ZG_OFFS_USRH 0x17 //[15:0] ZG_OFFS_USR
+#define MPU6050_RA_ZG_OFFS_USRL 0x18
+#define MPU6050_RA_SMPLRT_DIV 0x19
+#define MPU6050_RA_CONFIG 0x1A
+#define MPU6050_RA_GYRO_CONFIG 0x1B
+#define MPU6050_RA_ACCEL_CONFIG 0x1C
+#define MPU6050_RA_FF_THR 0x1D
+#define MPU6050_RA_FF_DUR 0x1E
+#define MPU6050_RA_MOT_THR 0x1F
+#define MPU6050_RA_MOT_DUR 0x20
+#define MPU6050_RA_ZRMOT_THR 0x21
+#define MPU6050_RA_ZRMOT_DUR 0x22
+#define MPU6050_RA_FIFO_EN 0x23
+#define MPU6050_RA_I2C_MST_CTRL 0x24
+#define MPU6050_RA_I2C_SLV0_ADDR 0x25
+#define MPU6050_RA_I2C_SLV0_REG 0x26
+#define MPU6050_RA_I2C_SLV0_CTRL 0x27
+#define MPU6050_RA_I2C_SLV1_ADDR 0x28
+#define MPU6050_RA_I2C_SLV1_REG 0x29
+#define MPU6050_RA_I2C_SLV1_CTRL 0x2A
+#define MPU6050_RA_I2C_SLV2_ADDR 0x2B
+#define MPU6050_RA_I2C_SLV2_REG 0x2C
+#define MPU6050_RA_I2C_SLV2_CTRL 0x2D
+#define MPU6050_RA_I2C_SLV3_ADDR 0x2E
+#define MPU6050_RA_I2C_SLV3_REG 0x2F
+#define MPU6050_RA_I2C_SLV3_CTRL 0x30
+#define MPU6050_RA_I2C_SLV4_ADDR 0x31
+#define MPU6050_RA_I2C_SLV4_REG 0x32
+#define MPU6050_RA_I2C_SLV4_DO 0x33
+#define MPU6050_RA_I2C_SLV4_CTRL 0x34
+#define MPU6050_RA_I2C_SLV4_DI 0x35
+#define MPU6050_RA_I2C_MST_STATUS 0x36
+#define MPU6050_RA_INT_PIN_CFG 0x37
+#define MPU6050_RA_INT_ENABLE 0x38
+#define MPU6050_RA_DMP_INT_STATUS 0x39
+#define MPU6050_RA_INT_STATUS 0x3A
+#define MPU6050_RA_ACCEL_XOUT_H 0x3B
+#define MPU6050_RA_ACCEL_XOUT_L 0x3C
+#define MPU6050_RA_ACCEL_YOUT_H 0x3D
+#define MPU6050_RA_ACCEL_YOUT_L 0x3E
+#define MPU6050_RA_ACCEL_ZOUT_H 0x3F
+#define MPU6050_RA_ACCEL_ZOUT_L 0x40
+#define MPU6050_RA_TEMP_OUT_H 0x41
+#define MPU6050_RA_TEMP_OUT_L 0x42
+#define MPU6050_RA_GYRO_XOUT_H 0x43
+#define MPU6050_RA_GYRO_XOUT_L 0x44
+#define MPU6050_RA_GYRO_YOUT_H 0x45
+#define MPU6050_RA_GYRO_YOUT_L 0x46
+#define MPU6050_RA_GYRO_ZOUT_H 0x47
+#define MPU6050_RA_GYRO_ZOUT_L 0x48
+#define MPU6050_RA_EXT_SENS_DATA_00 0x49
+#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
+#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
+#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
+#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
+#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
+#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
+#define MPU6050_RA_EXT_SENS_DATA_07 0x50
+#define MPU6050_RA_EXT_SENS_DATA_08 0x51
+#define MPU6050_RA_EXT_SENS_DATA_09 0x52
+#define MPU6050_RA_EXT_SENS_DATA_10 0x53
+#define MPU6050_RA_EXT_SENS_DATA_11 0x54
+#define MPU6050_RA_EXT_SENS_DATA_12 0x55
+#define MPU6050_RA_EXT_SENS_DATA_13 0x56
+#define MPU6050_RA_EXT_SENS_DATA_14 0x57
+#define MPU6050_RA_EXT_SENS_DATA_15 0x58
+#define MPU6050_RA_EXT_SENS_DATA_16 0x59
+#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
+#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
+#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
+#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
+#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
+#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
+#define MPU6050_RA_EXT_SENS_DATA_23 0x60
+#define MPU6050_RA_MOT_DETECT_STATUS 0x61
+#define MPU6050_RA_I2C_SLV0_DO 0x63
+#define MPU6050_RA_I2C_SLV1_DO 0x64
+#define MPU6050_RA_I2C_SLV2_DO 0x65
+#define MPU6050_RA_I2C_SLV3_DO 0x66
+#define MPU6050_RA_I2C_MST_DELAY_CTRL 0x67
+#define MPU6050_RA_SIGNAL_PATH_RESET 0x68
+#define MPU6050_RA_MOT_DETECT_CTRL 0x69
+#define MPU6050_RA_USER_CTRL 0x6A
+#define MPU6050_RA_PWR_MGMT_1 0x6B
+#define MPU6050_RA_PWR_MGMT_2 0x6C
+#define MPU6050_RA_BANK_SEL 0x6D
+#define MPU6050_RA_MEM_START_ADDR 0x6E
+#define MPU6050_RA_MEM_R_W 0x6F
+#define MPU6050_RA_DMP_CFG_1 0x70
+#define MPU6050_RA_DMP_CFG_2 0x71
+#define MPU6050_RA_FIFO_COUNTH 0x72
+#define MPU6050_RA_FIFO_COUNTL 0x73
+#define MPU6050_RA_FIFO_R_W 0x74
+#define MPU6050_RA_WHO_AM_I 0x75
+
+#define MPU6050_SELF_TEST_XA_1_BIT 0x07
+#define MPU6050_SELF_TEST_XA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_XA_2_BIT 0x05
+#define MPU6050_SELF_TEST_XA_2_LENGTH 0x02
+#define MPU6050_SELF_TEST_YA_1_BIT 0x07
+#define MPU6050_SELF_TEST_YA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_YA_2_BIT 0x03
+#define MPU6050_SELF_TEST_YA_2_LENGTH 0x02
+#define MPU6050_SELF_TEST_ZA_1_BIT 0x07
+#define MPU6050_SELF_TEST_ZA_1_LENGTH 0x03
+#define MPU6050_SELF_TEST_ZA_2_BIT 0x01
+#define MPU6050_SELF_TEST_ZA_2_LENGTH 0x02
+
+#define MPU6050_SELF_TEST_XG_1_BIT 0x04
+#define MPU6050_SELF_TEST_XG_1_LENGTH 0x05
+#define MPU6050_SELF_TEST_YG_1_BIT 0x04
+#define MPU6050_SELF_TEST_YG_1_LENGTH 0x05
+#define MPU6050_SELF_TEST_ZG_1_BIT 0x04
+#define MPU6050_SELF_TEST_ZG_1_LENGTH 0x05
+
+#define MPU6050_TC_PWR_MODE_BIT 7
+#define MPU6050_TC_OFFSET_BIT 6
+#define MPU6050_TC_OFFSET_LENGTH 6
+#define MPU6050_TC_OTP_BNK_VLD_BIT 0
+
+#define MPU6050_VDDIO_LEVEL_VLOGIC 0
+#define MPU6050_VDDIO_LEVEL_VDD 1
+
+#define MPU6050_CFG_EXT_SYNC_SET_BIT 5
+#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
+#define MPU6050_CFG_DLPF_CFG_BIT 2
+#define MPU6050_CFG_DLPF_CFG_LENGTH 3
+
+#define MPU6050_EXT_SYNC_DISABLED 0x0
+#define MPU6050_EXT_SYNC_TEMP_OUT_L 0x1
+#define MPU6050_EXT_SYNC_GYRO_XOUT_L 0x2
+#define MPU6050_EXT_SYNC_GYRO_YOUT_L 0x3
+#define MPU6050_EXT_SYNC_GYRO_ZOUT_L 0x4
+#define MPU6050_EXT_SYNC_ACCEL_XOUT_L 0x5
+#define MPU6050_EXT_SYNC_ACCEL_YOUT_L 0x6
+#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L 0x7
+
+#define MPU6050_DLPF_BW_256 0x00
+#define MPU6050_DLPF_BW_188 0x01
+#define MPU6050_DLPF_BW_98 0x02
+#define MPU6050_DLPF_BW_42 0x03
+#define MPU6050_DLPF_BW_20 0x04
+#define MPU6050_DLPF_BW_10 0x05
+#define MPU6050_DLPF_BW_5 0x06
+
+#define MPU6050_GCONFIG_FS_SEL_BIT 4
+#define MPU6050_GCONFIG_FS_SEL_LENGTH 2
+
+#define MPU6050_GYRO_FS_250 0x00
+#define MPU6050_GYRO_FS_500 0x01
+#define MPU6050_GYRO_FS_1000 0x02
+#define MPU6050_GYRO_FS_2000 0x03
+
+#define MPU6050_ACONFIG_XA_ST_BIT 7
+#define MPU6050_ACONFIG_YA_ST_BIT 6
+#define MPU6050_ACONFIG_ZA_ST_BIT 5
+#define MPU6050_ACONFIG_AFS_SEL_BIT 4
+#define MPU6050_ACONFIG_AFS_SEL_LENGTH 2
+#define MPU6050_ACONFIG_ACCEL_HPF_BIT 2
+#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH 3
+
+#define MPU6050_ACCEL_FS_2 0x00
+#define MPU6050_ACCEL_FS_4 0x01
+#define MPU6050_ACCEL_FS_8 0x02
+#define MPU6050_ACCEL_FS_16 0x03
+
+#define MPU6050_DHPF_RESET 0x00
+#define MPU6050_DHPF_5 0x01
+#define MPU6050_DHPF_2P5 0x02
+#define MPU6050_DHPF_1P25 0x03
+#define MPU6050_DHPF_0P63 0x04
+#define MPU6050_DHPF_HOLD 0x07
+
+#define MPU6050_TEMP_FIFO_EN_BIT 7
+#define MPU6050_XG_FIFO_EN_BIT 6
+#define MPU6050_YG_FIFO_EN_BIT 5
+#define MPU6050_ZG_FIFO_EN_BIT 4
+#define MPU6050_ACCEL_FIFO_EN_BIT 3
+#define MPU6050_SLV2_FIFO_EN_BIT 2
+#define MPU6050_SLV1_FIFO_EN_BIT 1
+#define MPU6050_SLV0_FIFO_EN_BIT 0
+
+#define MPU6050_MULT_MST_EN_BIT 7
+#define MPU6050_WAIT_FOR_ES_BIT 6
+#define MPU6050_SLV_3_FIFO_EN_BIT 5
+#define MPU6050_I2C_MST_P_NSR_BIT 4
+#define MPU6050_I2C_MST_CLK_BIT 3
+#define MPU6050_I2C_MST_CLK_LENGTH 4
+
+#define MPU6050_CLOCK_DIV_348 0x0
+#define MPU6050_CLOCK_DIV_333 0x1
+#define MPU6050_CLOCK_DIV_320 0x2
+#define MPU6050_CLOCK_DIV_308 0x3
+#define MPU6050_CLOCK_DIV_296 0x4
+#define MPU6050_CLOCK_DIV_286 0x5
+#define MPU6050_CLOCK_DIV_276 0x6
+#define MPU6050_CLOCK_DIV_267 0x7
+#define MPU6050_CLOCK_DIV_258 0x8
+#define MPU6050_CLOCK_DIV_500 0x9
+#define MPU6050_CLOCK_DIV_471 0xA
+#define MPU6050_CLOCK_DIV_444 0xB
+#define MPU6050_CLOCK_DIV_421 0xC
+#define MPU6050_CLOCK_DIV_400 0xD
+#define MPU6050_CLOCK_DIV_381 0xE
+#define MPU6050_CLOCK_DIV_364 0xF
+
+#define MPU6050_I2C_SLV_RW_BIT 7
+#define MPU6050_I2C_SLV_ADDR_BIT 6
+#define MPU6050_I2C_SLV_ADDR_LENGTH 7
+#define MPU6050_I2C_SLV_EN_BIT 7
+#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
+#define MPU6050_I2C_SLV_REG_DIS_BIT 5
+#define MPU6050_I2C_SLV_GRP_BIT 4
+#define MPU6050_I2C_SLV_LEN_BIT 3
+#define MPU6050_I2C_SLV_LEN_LENGTH 4
+
+#define MPU6050_I2C_SLV4_RW_BIT 7
+#define MPU6050_I2C_SLV4_ADDR_BIT 6
+#define MPU6050_I2C_SLV4_ADDR_LENGTH 7
+#define MPU6050_I2C_SLV4_EN_BIT 7
+#define MPU6050_I2C_SLV4_INT_EN_BIT 6
+#define MPU6050_I2C_SLV4_REG_DIS_BIT 5
+#define MPU6050_I2C_SLV4_MST_DLY_BIT 4
+#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5
+
+#define MPU6050_MST_PASS_THROUGH_BIT 7
+#define MPU6050_MST_I2C_SLV4_DONE_BIT 6
+#define MPU6050_MST_I2C_LOST_ARB_BIT 5
+#define MPU6050_MST_I2C_SLV4_NACK_BIT 4
+#define MPU6050_MST_I2C_SLV3_NACK_BIT 3
+#define MPU6050_MST_I2C_SLV2_NACK_BIT 2
+#define MPU6050_MST_I2C_SLV1_NACK_BIT 1
+#define MPU6050_MST_I2C_SLV0_NACK_BIT 0
+
+#define MPU6050_INTCFG_INT_LEVEL_BIT 7
+#define MPU6050_INTCFG_INT_OPEN_BIT 6
+#define MPU6050_INTCFG_LATCH_INT_EN_BIT 5
+#define MPU6050_INTCFG_INT_RD_CLEAR_BIT 4
+#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT 3
+#define MPU6050_INTCFG_FSYNC_INT_EN_BIT 2
+#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT 1
+#define MPU6050_INTCFG_CLKOUT_EN_BIT 0
+
+#define MPU6050_INTMODE_ACTIVEHIGH 0x00
+#define MPU6050_INTMODE_ACTIVELOW 0x01
+
+#define MPU6050_INTDRV_PUSHPULL 0x00
+#define MPU6050_INTDRV_OPENDRAIN 0x01
+
+#define MPU6050_INTLATCH_50USPULSE 0x00
+#define MPU6050_INTLATCH_WAITCLEAR 0x01
+
+#define MPU6050_INTCLEAR_STATUSREAD 0x00
+#define MPU6050_INTCLEAR_ANYREAD 0x01
+
+#define MPU6050_INTERRUPT_FF_BIT 7
+#define MPU6050_INTERRUPT_MOT_BIT 6
+#define MPU6050_INTERRUPT_ZMOT_BIT 5
+#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT 4
+#define MPU6050_INTERRUPT_I2C_MST_INT_BIT 3
+#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT 2
+#define MPU6050_INTERRUPT_DMP_INT_BIT 1
+#define MPU6050_INTERRUPT_DATA_RDY_BIT 0
+
+// TODO: figure out what these actually do
+// UMPL source code is not very obivous
+#define MPU6050_DMPINT_5_BIT 5
+#define MPU6050_DMPINT_4_BIT 4
+#define MPU6050_DMPINT_3_BIT 3
+#define MPU6050_DMPINT_2_BIT 2
+#define MPU6050_DMPINT_1_BIT 1
+#define MPU6050_DMPINT_0_BIT 0
+
+#define MPU6050_MOTION_MOT_XNEG_BIT 7
+#define MPU6050_MOTION_MOT_XPOS_BIT 6
+#define MPU6050_MOTION_MOT_YNEG_BIT 5
+#define MPU6050_MOTION_MOT_YPOS_BIT 4
+#define MPU6050_MOTION_MOT_ZNEG_BIT 3
+#define MPU6050_MOTION_MOT_ZPOS_BIT 2
+#define MPU6050_MOTION_MOT_ZRMOT_BIT 0
+
+#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT 7
+#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT 4
+#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT 3
+#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT 2
+#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT 1
+#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT 0
+
+#define MPU6050_PATHRESET_GYRO_RESET_BIT 2
+#define MPU6050_PATHRESET_ACCEL_RESET_BIT 1
+#define MPU6050_PATHRESET_TEMP_RESET_BIT 0
+
+#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT 5
+#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH 2
+#define MPU6050_DETECT_FF_COUNT_BIT 3
+#define MPU6050_DETECT_FF_COUNT_LENGTH 2
+#define MPU6050_DETECT_MOT_COUNT_BIT 1
+#define MPU6050_DETECT_MOT_COUNT_LENGTH 2
+
+#define MPU6050_DETECT_DECREMENT_RESET 0x0
+#define MPU6050_DETECT_DECREMENT_1 0x1
+#define MPU6050_DETECT_DECREMENT_2 0x2
+#define MPU6050_DETECT_DECREMENT_4 0x3
+
+#define MPU6050_USERCTRL_DMP_EN_BIT 7
+#define MPU6050_USERCTRL_FIFO_EN_BIT 6
+#define MPU6050_USERCTRL_I2C_MST_EN_BIT 5
+#define MPU6050_USERCTRL_I2C_IF_DIS_BIT 4
+#define MPU6050_USERCTRL_DMP_RESET_BIT 3
+#define MPU6050_USERCTRL_FIFO_RESET_BIT 2
+#define MPU6050_USERCTRL_I2C_MST_RESET_BIT 1
+#define MPU6050_USERCTRL_SIG_COND_RESET_BIT 0
+
+#define MPU6050_PWR1_DEVICE_RESET_BIT 7
+#define MPU6050_PWR1_SLEEP_BIT 6
+#define MPU6050_PWR1_CYCLE_BIT 5
+#define MPU6050_PWR1_TEMP_DIS_BIT 3
+#define MPU6050_PWR1_CLKSEL_BIT 2
+#define MPU6050_PWR1_CLKSEL_LENGTH 3
+
+#define MPU6050_CLOCK_INTERNAL 0x00
+#define MPU6050_CLOCK_PLL_XGYRO 0x01
+#define MPU6050_CLOCK_PLL_YGYRO 0x02
+#define MPU6050_CLOCK_PLL_ZGYRO 0x03
+#define MPU6050_CLOCK_PLL_EXT32K 0x04
+#define MPU6050_CLOCK_PLL_EXT19M 0x05
+#define MPU6050_CLOCK_KEEP_RESET 0x07
+
+#define MPU6050_PWR2_LP_WAKE_CTRL_BIT 7
+#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH 2
+#define MPU6050_PWR2_STBY_XA_BIT 5
+#define MPU6050_PWR2_STBY_YA_BIT 4
+#define MPU6050_PWR2_STBY_ZA_BIT 3
+#define MPU6050_PWR2_STBY_XG_BIT 2
+#define MPU6050_PWR2_STBY_YG_BIT 1
+#define MPU6050_PWR2_STBY_ZG_BIT 0
+
+#define MPU6050_WAKE_FREQ_1P25 0x0
+#define MPU6050_WAKE_FREQ_2P5 0x1
+#define MPU6050_WAKE_FREQ_5 0x2
+#define MPU6050_WAKE_FREQ_10 0x3
+
+#define MPU6050_BANKSEL_PRFTCH_EN_BIT 6
+#define MPU6050_BANKSEL_CFG_USER_BANK_BIT 5
+#define MPU6050_BANKSEL_MEM_SEL_BIT 4
+#define MPU6050_BANKSEL_MEM_SEL_LENGTH 5
+
+#define MPU6050_WHO_AM_I_BIT 6
+#define MPU6050_WHO_AM_I_LENGTH 6
+
+#define MPU6050_DMP_MEMORY_BANKS 8
+#define MPU6050_DMP_MEMORY_BANK_SIZE 256
+#define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
+
+// note: DMP code memory blocks defined at end of header file
+
+class MPU6050
+{
+public:
+	MPU6050();
+	MPU6050(uint8_t address);
+
+	void initialize();
+	bool testConnection();
+
+	// AUX_VDDIO register
+	uint8_t getAuxVDDIOLevel();
+	void setAuxVDDIOLevel(uint8_t level);
+
+	// SMPLRT_DIV register
+	uint8_t getRate();
+	void setRate(uint8_t rate);
+
+	// CONFIG register
+	uint8_t getExternalFrameSync();
+	void setExternalFrameSync(uint8_t sync);
+	uint8_t getDLPFMode();
+	void setDLPFMode(uint8_t bandwidth);
+
+	// GYRO_CONFIG register
+	uint8_t getFullScaleGyroRange();
+	void setFullScaleGyroRange(uint8_t range);
+
+	// SELF_TEST registers
+	uint8_t getAccelXSelfTestFactoryTrim();
+	uint8_t getAccelYSelfTestFactoryTrim();
+	uint8_t getAccelZSelfTestFactoryTrim();
+
+	uint8_t getGyroXSelfTestFactoryTrim();
+	uint8_t getGyroYSelfTestFactoryTrim();
+	uint8_t getGyroZSelfTestFactoryTrim();
+
+	// ACCEL_CONFIG register
+	bool getAccelXSelfTest();
+	void setAccelXSelfTest(bool enabled);
+	bool getAccelYSelfTest();
+	void setAccelYSelfTest(bool enabled);
+	bool getAccelZSelfTest();
+	void setAccelZSelfTest(bool enabled);
+	uint8_t getFullScaleAccelRange();
+	void setFullScaleAccelRange(uint8_t range);
+	uint8_t getDHPFMode();
+	void setDHPFMode(uint8_t mode);
+
+	// FF_THR register
+	uint8_t getFreefallDetectionThreshold();
+	void setFreefallDetectionThreshold(uint8_t threshold);
+
+	// FF_DUR register
+	uint8_t getFreefallDetectionDuration();
+	void setFreefallDetectionDuration(uint8_t duration);
+
+	// MOT_THR register
+	uint8_t getMotionDetectionThreshold();
+	void setMotionDetectionThreshold(uint8_t threshold);
+
+	// MOT_DUR register
+	uint8_t getMotionDetectionDuration();
+	void setMotionDetectionDuration(uint8_t duration);
+
+	// ZRMOT_THR register
+	uint8_t getZeroMotionDetectionThreshold();
+	void setZeroMotionDetectionThreshold(uint8_t threshold);
+
+	// ZRMOT_DUR register
+	uint8_t getZeroMotionDetectionDuration();
+	void setZeroMotionDetectionDuration(uint8_t duration);
+
+	// FIFO_EN register
+	bool getTempFIFOEnabled();
+	void setTempFIFOEnabled(bool enabled);
+	bool getXGyroFIFOEnabled();
+	void setXGyroFIFOEnabled(bool enabled);
+	bool getYGyroFIFOEnabled();
+	void setYGyroFIFOEnabled(bool enabled);
+	bool getZGyroFIFOEnabled();
+	void setZGyroFIFOEnabled(bool enabled);
+	bool getAccelFIFOEnabled();
+	void setAccelFIFOEnabled(bool enabled);
+	bool getSlave2FIFOEnabled();
+	void setSlave2FIFOEnabled(bool enabled);
+	bool getSlave1FIFOEnabled();
+	void setSlave1FIFOEnabled(bool enabled);
+	bool getSlave0FIFOEnabled();
+	void setSlave0FIFOEnabled(bool enabled);
+
+	// I2C_MST_CTRL register
+	bool getMultiMasterEnabled();
+	void setMultiMasterEnabled(bool enabled);
+	bool getWaitForExternalSensorEnabled();
+	void setWaitForExternalSensorEnabled(bool enabled);
+	bool getSlave3FIFOEnabled();
+	void setSlave3FIFOEnabled(bool enabled);
+	bool getSlaveReadWriteTransitionEnabled();
+	void setSlaveReadWriteTransitionEnabled(bool enabled);
+	uint8_t getMasterClockSpeed();
+	void setMasterClockSpeed(uint8_t speed);
+
+	// I2C_SLV* registers (Slave 0-3)
+	uint8_t getSlaveAddress(uint8_t num);
+	void setSlaveAddress(uint8_t num, uint8_t address);
+	uint8_t getSlaveRegister(uint8_t num);
+	void setSlaveRegister(uint8_t num, uint8_t reg);
+	bool getSlaveEnabled(uint8_t num);
+	void setSlaveEnabled(uint8_t num, bool enabled);
+	bool getSlaveWordByteSwap(uint8_t num);
+	void setSlaveWordByteSwap(uint8_t num, bool enabled);
+	bool getSlaveWriteMode(uint8_t num);
+	void setSlaveWriteMode(uint8_t num, bool mode);
+	bool getSlaveWordGroupOffset(uint8_t num);
+	void setSlaveWordGroupOffset(uint8_t num, bool enabled);
+	uint8_t getSlaveDataLength(uint8_t num);
+	void setSlaveDataLength(uint8_t num, uint8_t length);
+
+	// I2C_SLV* registers (Slave 4)
+	uint8_t getSlave4Address();
+	void setSlave4Address(uint8_t address);
+	uint8_t getSlave4Register();
+	void setSlave4Register(uint8_t reg);
+	void setSlave4OutputByte(uint8_t data);
+	bool getSlave4Enabled();
+	void setSlave4Enabled(bool enabled);
+	bool getSlave4InterruptEnabled();
+	void setSlave4InterruptEnabled(bool enabled);
+	bool getSlave4WriteMode();
+	void setSlave4WriteMode(bool mode);
+	uint8_t getSlave4MasterDelay();
+	void setSlave4MasterDelay(uint8_t delay);
+	uint8_t getSlate4InputByte();
+
+	// I2C_MST_STATUS register
+	bool getPassthroughStatus();
+	bool getSlave4IsDone();
+	bool getLostArbitration();
+	bool getSlave4Nack();
+	bool getSlave3Nack();
+	bool getSlave2Nack();
+	bool getSlave1Nack();
+	bool getSlave0Nack();
+
+	// INT_PIN_CFG register
+	bool getInterruptMode();
+	void setInterruptMode(bool mode);
+	bool getInterruptDrive();
+	void setInterruptDrive(bool drive);
+	bool getInterruptLatch();
+	void setInterruptLatch(bool latch);
+	bool getInterruptLatchClear();
+	void setInterruptLatchClear(bool clear);
+	bool getFSyncInterruptLevel();
+	void setFSyncInterruptLevel(bool level);
+	bool getFSyncInterruptEnabled();
+	void setFSyncInterruptEnabled(bool enabled);
+	bool getI2CBypassEnabled();
+	void setI2CBypassEnabled(bool enabled);
+	bool getClockOutputEnabled();
+	void setClockOutputEnabled(bool enabled);
+
+	// INT_ENABLE register
+	uint8_t getIntEnabled();
+	void setIntEnabled(uint8_t enabled);
+	bool getIntFreefallEnabled();
+	void setIntFreefallEnabled(bool enabled);
+	bool getIntMotionEnabled();
+	void setIntMotionEnabled(bool enabled);
+	bool getIntZeroMotionEnabled();
+	void setIntZeroMotionEnabled(bool enabled);
+	bool getIntFIFOBufferOverflowEnabled();
+	void setIntFIFOBufferOverflowEnabled(bool enabled);
+	bool getIntI2CMasterEnabled();
+	void setIntI2CMasterEnabled(bool enabled);
+	bool getIntDataReadyEnabled();
+	void setIntDataReadyEnabled(bool enabled);
+
+	// INT_STATUS register
+	uint8_t getIntStatus();
+	bool getIntFreefallStatus();
+	bool getIntMotionStatus();
+	bool getIntZeroMotionStatus();
+	bool getIntFIFOBufferOverflowStatus();
+	bool getIntI2CMasterStatus();
+	bool getIntDataReadyStatus();
+
+	// ACCEL_*OUT_* registers
+	void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx,
+					int16_t* my, int16_t* mz);
+	void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+	void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+	int16_t getAccelerationX();
+	int16_t getAccelerationY();
+	int16_t getAccelerationZ();
+
+	// TEMP_OUT_* registers
+	int16_t getTemperature();
+
+	// GYRO_*OUT_* registers
+	void getRotation(int16_t* x, int16_t* y, int16_t* z);
+	int16_t getRotationX();
+	int16_t getRotationY();
+	int16_t getRotationZ();
+
+	// EXT_SENS_DATA_* registers
+	uint8_t getExternalSensorByte(int position);
+	uint16_t getExternalSensorWord(int position);
+	uint32_t getExternalSensorDWord(int position);
+
+	// MOT_DETECT_STATUS register
+	uint8_t getMotionStatus();
+	bool getXNegMotionDetected();
+	bool getXPosMotionDetected();
+	bool getYNegMotionDetected();
+	bool getYPosMotionDetected();
+	bool getZNegMotionDetected();
+	bool getZPosMotionDetected();
+	bool getZeroMotionDetected();
+
+	// I2C_SLV*_DO register
+	void setSlaveOutputByte(uint8_t num, uint8_t data);
+
+	// I2C_MST_DELAY_CTRL register
+	bool getExternalShadowDelayEnabled();
+	void setExternalShadowDelayEnabled(bool enabled);
+	bool getSlaveDelayEnabled(uint8_t num);
+	void setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+	// SIGNAL_PATH_RESET register
+	void resetGyroscopePath();
+	void resetAccelerometerPath();
+	void resetTemperaturePath();
+
+	// MOT_DETECT_CTRL register
+	uint8_t getAccelerometerPowerOnDelay();
+	void setAccelerometerPowerOnDelay(uint8_t delay);
+	uint8_t getFreefallDetectionCounterDecrement();
+	void setFreefallDetectionCounterDecrement(uint8_t decrement);
+	uint8_t getMotionDetectionCounterDecrement();
+	void setMotionDetectionCounterDecrement(uint8_t decrement);
+
+	// USER_CTRL register
+	bool getFIFOEnabled();
+	void setFIFOEnabled(bool enabled);
+	bool getI2CMasterModeEnabled();
+	void setI2CMasterModeEnabled(bool enabled);
+	void switchSPIEnabled(bool enabled);
+	void resetFIFO();
+	void resetI2CMaster();
+	void resetSensors();
+
+	// PWR_MGMT_1 register
+	void reset();
+	bool getSleepEnabled();
+	void setSleepEnabled(bool enabled);
+	bool getWakeCycleEnabled();
+	void setWakeCycleEnabled(bool enabled);
+	bool getTempSensorEnabled();
+	void setTempSensorEnabled(bool enabled);
+	uint8_t getClockSource();
+	void setClockSource(uint8_t source);
+
+	// PWR_MGMT_2 register
+	uint8_t getWakeFrequency();
+	void setWakeFrequency(uint8_t frequency);
+	bool getStandbyXAccelEnabled();
+	void setStandbyXAccelEnabled(bool enabled);
+	bool getStandbyYAccelEnabled();
+	void setStandbyYAccelEnabled(bool enabled);
+	bool getStandbyZAccelEnabled();
+	void setStandbyZAccelEnabled(bool enabled);
+	bool getStandbyXGyroEnabled();
+	void setStandbyXGyroEnabled(bool enabled);
+	bool getStandbyYGyroEnabled();
+	void setStandbyYGyroEnabled(bool enabled);
+	bool getStandbyZGyroEnabled();
+	void setStandbyZGyroEnabled(bool enabled);
+
+	// FIFO_COUNT_* registers
+	uint16_t getFIFOCount();
+
+	// FIFO_R_W register
+	uint8_t getFIFOByte();
+	void setFIFOByte(uint8_t data);
+	void getFIFOBytes(uint8_t* data, uint8_t length);
+
+	// WHO_AM_I register
+	uint8_t getDeviceID();
+	void setDeviceID(uint8_t id);
+
+	// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+	// XG_OFFS_TC register
+	uint8_t getOTPBankValid();
+	void setOTPBankValid(bool enabled);
+	int8_t getXGyroOffsetTC();
+	void setXGyroOffsetTC(int8_t offset);
+
+	// YG_OFFS_TC register
+	int8_t getYGyroOffsetTC();
+	void setYGyroOffsetTC(int8_t offset);
+
+	// ZG_OFFS_TC register
+	int8_t getZGyroOffsetTC();
+	void setZGyroOffsetTC(int8_t offset);
+
+	// X_FINE_GAIN register
+	int8_t getXFineGain();
+	void setXFineGain(int8_t gain);
+
+	// Y_FINE_GAIN register
+	int8_t getYFineGain();
+	void setYFineGain(int8_t gain);
+
+	// Z_FINE_GAIN register
+	int8_t getZFineGain();
+	void setZFineGain(int8_t gain);
+
+	// XA_OFFS_* registers
+	int16_t getXAccelOffset();
+	void setXAccelOffset(int16_t offset);
+
+	// YA_OFFS_* register
+	int16_t getYAccelOffset();
+	void setYAccelOffset(int16_t offset);
+
+	// ZA_OFFS_* register
+	int16_t getZAccelOffset();
+	void setZAccelOffset(int16_t offset);
+
+	// XG_OFFS_USR* registers
+	int16_t getXGyroOffset();
+	void setXGyroOffset(int16_t offset);
+
+	// YG_OFFS_USR* register
+	int16_t getYGyroOffset();
+	void setYGyroOffset(int16_t offset);
+
+	// ZG_OFFS_USR* register
+	int16_t getZGyroOffset();
+	void setZGyroOffset(int16_t offset);
+
+	// INT_ENABLE register (DMP functions)
+	bool getIntPLLReadyEnabled();
+	void setIntPLLReadyEnabled(bool enabled);
+	bool getIntDMPEnabled();
+	void setIntDMPEnabled(bool enabled);
+
+	// DMP_INT_STATUS
+	bool getDMPInt5Status();
+	bool getDMPInt4Status();
+	bool getDMPInt3Status();
+	bool getDMPInt2Status();
+	bool getDMPInt1Status();
+	bool getDMPInt0Status();
+
+	// INT_STATUS register (DMP functions)
+	bool getIntPLLReadyStatus();
+	bool getIntDMPStatus();
+
+	// USER_CTRL register (DMP functions)
+	bool getDMPEnabled();
+	void setDMPEnabled(bool enabled);
+	void resetDMP();
+
+	// BANK_SEL register
+	void setMemoryBank(uint8_t bank, bool prefetchEnabled = false, bool userBank = false);
+
+	// MEM_START_ADDR register
+	void setMemoryStartAddress(uint8_t address);
+
+	// MEM_R_W register
+	uint8_t readMemoryByte();
+	void writeMemoryByte(uint8_t data);
+	void readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0);
+	bool writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0,
+						  bool verify = true, bool useProgMem = false);
+	bool writeProgMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0,
+							  bool verify = true);
+
+	bool writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, bool useProgMem = false);
+	bool writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize);
+
+	// DMP_CFG_1 register
+	uint8_t getDMPConfig1();
+	void setDMPConfig1(uint8_t config);
+
+	// DMP_CFG_2 register
+	uint8_t getDMPConfig2();
+	void setDMPConfig2(uint8_t config);
+
+// special methods for MotionApps 2.0 implementation
+#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
+	uint8_t* dmpPacketBuffer;
+	uint16_t dmpPacketSize;
+
+	uint8_t dmpInitialize();
+	bool dmpPacketAvailable();
+
+	uint8_t dmpSetFIFORate(uint8_t fifoRate);
+	uint8_t dmpGetFIFORate();
+	uint8_t dmpGetSampleStepSizeMS();
+	uint8_t dmpGetSampleFrequency();
+	int32_t dmpDecodeTemperature(int8_t tempReg);
+
+	// Register callbacks after a packet of FIFO data is processed
+	// uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+	// uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
+	uint8_t dmpRunFIFORateProcesses();
+
+	// Setup FIFO for various output
+	uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
+	uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
+	uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+	// Get Fixed Point data from FIFO
+	uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
+	uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity);
+	uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q);
+	uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);
+	uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0);
+	uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0);
+
+	uint8_t dmpGetEuler(float* data, Quaternion* q);
+	uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
+
+	// Get Floating Point data from FIFO
+	uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0);
+
+	uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData);
+	uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL);
+
+	uint8_t dmpSetFIFOProcessedCallback(void (*func)(void));
+
+	uint8_t dmpInitFIFOParam();
+	uint8_t dmpCloseFIFO();
+	uint8_t dmpSetGyroDataSource(uint8_t source);
+	uint8_t dmpDecodeQuantizedAccel();
+	uint32_t dmpGetGyroSumOfSquare();
+	uint32_t dmpGetAccelSumOfSquare();
+	void dmpOverrideQuaternion(long* q);
+	uint16_t dmpGetFIFOPacketSize();
+#endif
+
+// special methods for MotionApps 4.1 implementation
+#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
+	uint8_t* dmpPacketBuffer;
+	uint16_t dmpPacketSize;
+
+	uint8_t dmpInitialize();
+	bool dmpPacketAvailable();
+
+	uint8_t dmpSetFIFORate(uint8_t fifoRate);
+	uint8_t dmpGetFIFORate();
+	uint8_t dmpGetSampleStepSizeMS();
+	uint8_t dmpGetSampleFrequency();
+	int32_t dmpDecodeTemperature(int8_t tempReg);
+
+	// Register callbacks after a packet of FIFO data is processed
+	// uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+	// uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
+	uint8_t dmpRunFIFORateProcesses();
+
+	// Setup FIFO for various output
+	uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
+	uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
+	uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+	uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+	// Get Fixed Point data from FIFO
+	uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetMag(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
+	uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity);
+	uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q);
+	uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);
+	uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
+	uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0);
+	uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0);
+
+	uint8_t dmpGetEuler(float* data, Quaternion* q);
+	uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
+
+	// Get Floating Point data from FIFO
+	uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0);
+	uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0);
+
+	uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData);
+	uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL);
+
+	uint8_t dmpSetFIFOProcessedCallback(void (*func)(void));
+
+	uint8_t dmpInitFIFOParam();
+	uint8_t dmpCloseFIFO();
+	uint8_t dmpSetGyroDataSource(uint8_t source);
+	uint8_t dmpDecodeQuantizedAccel();
+	uint32_t dmpGetGyroSumOfSquare();
+	uint32_t dmpGetAccelSumOfSquare();
+	void dmpOverrideQuaternion(long* q);
+	uint16_t dmpGetFIFOPacketSize();
+#endif
+
+	void CalibrateGyro(uint8_t Loops = 15);  // Fine tune after setting offsets with less Loops.
+	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
+	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
+
+private:
+	uint8_t devAddr;
+	uint8_t buffer[14];
+};
+
+#endif /* _MPU6050_H_ */
diff --git a/Sming/Libraries/MPU6050/README.md b/Sming/Libraries/MPU6050/README.md
new file mode 100644
index 0000000000..19ff23e2ab
--- /dev/null
+++ b/Sming/Libraries/MPU6050/README.md
@@ -0,0 +1,9 @@
+# MPU6050 Six-Axis (Gyro + Accelerometer)
+
+Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
+
+- Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
+- Removed map function in favor of the Sming built-in one.
+- Adapted include path and applied clangformat
+
+
diff --git a/Sming/Libraries/MPU6050/component.mk b/Sming/Libraries/MPU6050/component.mk
new file mode 100644
index 0000000000..725f8abd75
--- /dev/null
+++ b/Sming/Libraries/MPU6050/component.mk
@@ -0,0 +1 @@
+COMPONENT_DEPENDS := I2Cdev
diff --git a/Sming/Libraries/MPU6050/helper_3dmath.h b/Sming/Libraries/MPU6050/helper_3dmath.h
new file mode 100644
index 0000000000..4e8ac2d51b
--- /dev/null
+++ b/Sming/Libraries/MPU6050/helper_3dmath.h
@@ -0,0 +1,243 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D
+// math helper 6/5/2012 by Jeff Rowberg <jeff@rowberg.net> Updates should
+// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+#include <math.h>
+#include <stdint.h>
+
+class Quaternion
+{
+public:
+	float w;
+	float x;
+	float y;
+	float z;
+
+	Quaternion()
+	{
+		w = 1.0f;
+		x = 0.0f;
+		y = 0.0f;
+		z = 0.0f;
+	}
+
+	Quaternion(float nw, float nx, float ny, float nz)
+	{
+		w = nw;
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	Quaternion getProduct(Quaternion q)
+	{
+		// Quaternion multiplication is defined by:
+		//     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+		//     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+		//     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+		//     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+		return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z,  // new w
+						  w * q.x + x * q.w + y * q.z - z * q.y,  // new x
+						  w * q.y - x * q.z + y * q.w + z * q.x,  // new y
+						  w * q.z + x * q.y - y * q.x + z * q.w); // new z
+	}
+
+	Quaternion getConjugate()
+	{
+		return Quaternion(w, -x, -y, -z);
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(w * w + x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		w /= m;
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	Quaternion getNormalized()
+	{
+		Quaternion r(w, x, y, z);
+		r.normalize();
+		return r;
+	}
+};
+
+class VectorInt16
+{
+public:
+	int16_t x;
+	int16_t y;
+	int16_t z;
+
+	VectorInt16()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+	}
+
+	VectorInt16(int16_t nx, int16_t ny, int16_t nz)
+	{
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	VectorInt16 getNormalized()
+	{
+		VectorInt16 r(x, y, z);
+		r.normalize();
+		return r;
+	}
+
+	void rotate(Quaternion* q)
+	{
+		// http://www.cprogramming.com/tutorial/3d/quaternions.html
+		// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+		// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+		// ^ or:
+		// http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+
+		// P_out = q * P_in * conj(q)
+		// - P_out is the output vector
+		// - q is the orientation quaternion
+		// - P_in is the input vector (a*aReal)
+		// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z],
+		// q*=[w,-x,-y,-z])
+		Quaternion p(0, x, y, z);
+
+		// quaternion multiplication: q * p, stored back in p
+		p = q->getProduct(p);
+
+		// quaternion multiplication: p * conj(q), stored back in p
+		p = p.getProduct(q->getConjugate());
+
+		// p quaternion is now [0, x', y', z']
+		x = p.x;
+		y = p.y;
+		z = p.z;
+	}
+
+	VectorInt16 getRotated(Quaternion* q)
+	{
+		VectorInt16 r(x, y, z);
+		r.rotate(q);
+		return r;
+	}
+};
+
+class VectorFloat
+{
+public:
+	float x;
+	float y;
+	float z;
+
+	VectorFloat()
+	{
+		x = 0;
+		y = 0;
+		z = 0;
+	}
+
+	VectorFloat(float nx, float ny, float nz)
+	{
+		x = nx;
+		y = ny;
+		z = nz;
+	}
+
+	float getMagnitude()
+	{
+		return sqrt(x * x + y * y + z * z);
+	}
+
+	void normalize()
+	{
+		float m = getMagnitude();
+		x /= m;
+		y /= m;
+		z /= m;
+	}
+
+	VectorFloat getNormalized()
+	{
+		VectorFloat r(x, y, z);
+		r.normalize();
+		return r;
+	}
+
+	void rotate(Quaternion* q)
+	{
+		Quaternion p(0, x, y, z);
+
+		// quaternion multiplication: q * p, stored back in p
+		p = q->getProduct(p);
+
+		// quaternion multiplication: p * conj(q), stored back in p
+		p = p.getProduct(q->getConjugate());
+
+		// p quaternion is now [0, x', y', z']
+		x = p.x;
+		y = p.y;
+		z = p.z;
+	}
+
+	VectorFloat getRotated(Quaternion* q)
+	{
+		VectorFloat r(x, y, z);
+		r.rotate(q);
+		return r;
+	}
+};
+
+#endif /* _HELPER_3DMATH_H_ */
diff --git a/samples/Accel_Gyro_MPU6050/Makefile b/samples/Accel_Gyro_MPU6050/Makefile
new file mode 100644
index 0000000000..ff51b6c3a7
--- /dev/null
+++ b/samples/Accel_Gyro_MPU6050/Makefile
@@ -0,0 +1,9 @@
+#####################################################################
+#### Please don't change this file. Use component.mk instead ####
+#####################################################################
+
+ifndef SMING_HOME
+$(error SMING_HOME is not set: please configure it as an environment variable)
+endif
+
+include $(SMING_HOME)/project.mk
diff --git a/samples/Accel_Gyro_MPU6050/README.rst b/samples/Accel_Gyro_MPU6050/README.rst
new file mode 100644
index 0000000000..d43abbcfff
--- /dev/null
+++ b/samples/Accel_Gyro_MPU6050/README.rst
@@ -0,0 +1,7 @@
+MPU6050 Six-Axis (Gyro + Accelerometer) 
+================
+
+MPU6050 sensor reader.
+
+.. image:: mpu6050.jpg
+   :height: 192px
diff --git a/samples/Accel_Gyro_MPU6050/app/application.cpp b/samples/Accel_Gyro_MPU6050/app/application.cpp
new file mode 100644
index 0000000000..d49c231127
--- /dev/null
+++ b/samples/Accel_Gyro_MPU6050/app/application.cpp
@@ -0,0 +1,42 @@
+#include <Libraries/MPU6050/MPU6050.h>
+#include <SmingCore.h>
+
+constexpr float main_loop_interval = 0.02; // sec
+SimpleTimer main_loop_timer;
+MPU6050 mpu;
+
+void print_accel_gyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
+{
+	Serial.print("a/g:\t");
+	Serial.print(ax);
+	Serial.print("\t");
+	Serial.print(ay);
+	Serial.print("\t");
+	Serial.print(az);
+	Serial.print("\t");
+	Serial.print(gx);
+	Serial.print("\t");
+	Serial.print(gy);
+	Serial.print("\t");
+	Serial.println(gz);
+}
+
+void main_loop()
+{
+	int16_t ax, ay, az;
+	int16_t gx, gy, gz;
+	mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+	print_accel_gyro(ax, ay, az, gx, gy, gz);
+}
+
+void init()
+{
+	Serial.begin(SERIAL_BAUD_RATE); // 115200 by default
+	Serial.systemDebugOutput(true); // Enable debug output to serial
+
+	Wire.begin(DEFAULT_SDA_PIN, DEFAULT_SCL_PIN);
+	mpu.initialize();
+	Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
+
+	main_loop_timer.initializeMs(static_cast<int>(main_loop_interval * 1000), main_loop).start();
+}
diff --git a/samples/Accel_Gyro_MPU6050/component.mk b/samples/Accel_Gyro_MPU6050/component.mk
new file mode 100644
index 0000000000..54e9cf0c23
--- /dev/null
+++ b/samples/Accel_Gyro_MPU6050/component.mk
@@ -0,0 +1,2 @@
+ARDUINO_LIBRARIES := MPU6050
+DISABLE_NETWORK := 1
diff --git a/samples/Accel_Gyro_MPU6050/mpu6050.jpg b/samples/Accel_Gyro_MPU6050/mpu6050.jpg
new file mode 100644
index 0000000000000000000000000000000000000000..3d478a9db958f526902945c929042c9d68b8446e
GIT binary patch
literal 55832
zcmb5V1z20%(gvDPpjeAL#jOx5IEB#S6nA$51b2rj?(Wdy?rs%a0tJd&aV->SixqC*
zYx#Q4f9~_#yOZq9nt5lI&CFVRXXk3_>I(qpsg$e~00;yEWDqaF)zTfXq$kV*08mf>
zumAu6Gyo<L8-R2}0RXW;oEsV)QBDQo0q!8afru9XNCrT;p#gwgAo(p_3S|C`L0ABC
z-RNQ=aJ4}24NZWc-y`AwYbOW>K>ls#b1=gn8YvgS1Gt#jxwzR|0@M`Po^W&XiCy!q
z+k9RBKi|@_3d+=Ab^&%U7y#x3a|?0u3xNfwxwwRQxP^Fm0RW1Zsejsr#PTxZmIepY
z{@c!ArhjPQV*nr;aMMmi-;ji&@BC>q(&K34TUtF92*CKK6-ZXGNVhf}W07z9p0Oym
z{O7TEf7gW=y;!v0G{QhE`d>80Ej~Cl`lc>qZcbi6Y|LMD?7#FI0N^gd1TrTVC+8dB
zZ##%_d4u$?%{O=crE~w{=l;z{zO6Sp7v(pe6fvo&f9X=+)Qgx$q+~>$*GDe^z=Q(;
zNGV@`bArK~fd3kQZhj#yp8v^1XkX7C81X4We9RG_>*M<ScNsYHPx-nACY+}{0+MP<
zPgJ=O4hld;=wACKk|N@SpssxoF{((1i1Q6!@|yoopAh^z*Q0t}+x6#K`?t^Dp}i>s
z0El+q$PhIlz7HXl0Bo_x2paRI3;<l?fIxxb;eYr@_pi%WOBlOWuC9(k931v8>?Y<8
zrcib>2RjZ=6Gsj%c1{j}u&Ae_iJ1-5mD&_)39}br+HZNsL=7_+VbbDN;8buFhg!j8
zyq%$H-iqpG-Zo}}=1igx3}H_pPdi6DsH+LJr=6|6i;$-X(~WT<1bxlsU_yYL%`Jpf
zC8T~s5G4_&-)-^m@L>1gW_NJ5<lqt%6y)FpbAZ8Y2n3spm%XcrC!4(s9l`<z^^FY)
zsEe61%+VF*VE@||6p;pQjr~W*t|d1;L#R_z3ttz7WbI5Wp;|C=S1Sk)Cnu*c$A5GG
z6Dbe1w?sg>{|ow?eTx-$uyb^Vy0{=(1$nA0DNT(S*FTKkRs0RP)l)FBfkJ-gi{H2#
z*1rG;bEqxkPbT>TxnbU7l$>Eu#6VpS2}Iq&k<Aur;rbi-x9AqHtqOH<uywn^^MijQ
z|7PAIb^aIfZ{{uXU!aCP%oQSQZ)Ww|@PA2eHPjp|Ts=&jp^%##sjaQSC$1&+r!}|2
zTeyUYD^wk32Zj7+zPyRo-!<JrZk2SzO~sv+9b917eTCUuLc|dR1$Bm+|JL|F@>@+w
zPe%tAH|Kw<<l^Avq;`RtvHw>458<tfl%kuRDbyK)a5<2`Z{#1|E$EM`va^Gwvxyx<
z(ZSiy#FpCepW=Vn{{y<!mQ_<ybA&?8RS}bIZ|QRF)&E3=8r12g@K4DW2H^%HT>Coa
z5cCb|rtll~yPE%D?VlKtL<EW^)cM+*sX6`)oj>D?a4JN0FtPprFcr7Z+b;YuB7v~s
z3iF1V|9`OZFZR}qnw6U?VkU1!L(|084GQ4|ae}EOHL01oIJj;m>*4Qi{$J>=l_!o+
zXII25|6_p*#3y)T;bsQ^sQ)MCR`t(Bior}=ZX3XfQ2(zv{8xUfs$k;zpGN<8#s3HP
zuK_5c>j(!>K;!|4nxn&?Xjk}~e~XrfSz5WOIk-8SK_MFUHuer4_P^Wsx8xQsWozPM
zMSbl$jwa47P<1ays0h<_5MLYkCz1Y>GjD+ZdWV#QnThT1yvPIMr?#{EUCZBs+gjAz
zOc4Qf6VQn5!k?M?m-#nP?H}N+Fa3wYe+&PI;=k_mzcl}q-m2d8Od00+I|d+s%G7^?
z0<l{D>D={b{RO?XA?{%7fN)O}#Gt#VN{ju5|HZvUBDA5l+7gJ|W`DDO{Ix9ox9C50
zbpF-(Ykj)W`Ac-G^T%Uvyyl6Us{=Lkcar<vxc>m$s%crlT%lqnwkGz7uH2}3Tyv;R
z|6%>s{SVZw?#<Ym*y=hs*j=YA2M8Akv5@{5yZ<A-)%(k3WbL_m?d+srwssKkA3you
z;#+MsGpIdO+{E!Zvaj72qULI1Z*JoJC+q(o`K{*7<^ZvPIk`b)B_Mz8;C}b=Kjr_b
zT`wvQh+JY~3bTc|ddb?GLp>pif0X{G^#4`)kJIr8fT+3n5FcKU06#B3-=9&F{V#KW
z@V5q~mDS`O>=D`3%^a!<wSY+eZpVM~|EB_kiwi`}%>$wabAosTIR$wI|A+R!L65Nc
z#Mbq;DmADHwJp>gYWuI!-{QY({KJRvg8BY08d1YP%hzAqJ;WC9A0A>GiP%tb+-^G&
zh1-oQ)E=>0b-vz~Ud;l;0e4YQP*ITYqN1Xr-Mx#3iHC`afq_YcjdKr=jEJ0!l!%ms
zf|{9@f|8Mvgp`hxo{@!>jh&607R<{9;$;T0fv$}J@1mh$qGJ+bViJNLkUjwY|30oh
z0&vlQtVloa0C53GxWGHOz^hIGIb!>N=lXi~-vfwr2N?zRE<#9(5L_Ss$^bXQs~G^+
z9mJ&s&K<<gA9z^r1Mx{I23xZr2Scad0$5o_r)WPC8gQc3bHGDri``Y8Fpq<ln*-E~
zafs=X<kR8mQZd}V;njk=rEc_QKs8c&fEe%-&Q}7GPP9@0TJU2TV;=EboMBw1(bIhv
zdqayn0(7L4=}-=_J84C|vqectL}{E`LvLKAk|z^(-V8x&?!TO6Y;4w^z$hfdi?fBv
z$o0$zBhi)NW(&Hi(aRVMISZMRpo!%Q#KwP+0I1&sq9JoV??eMo(ufSRmGBKjsS>=P
z6&L}cixEoYRnNzV7N;^lZ{~%9>m+O-ITQ_2$>ElxY3j7lfLL7@y8yJX$k(R)OpN0)
zcyjbHG?CH4Z!kVSCt(E;qG1^$;Q+en0RU`lrqQOo*HbQ~1sVW`!=3)#PXZgTfw@h&
z=?xf{wX=IXz$zlfLb4pZ75j0zJ9!$1Qv(A{>Ns6Pe^ysU(o&{@LBBZzjE+-;AM%M7
zNrJOhimQ<G0W}GhDUkXO2>{JhSUB@1x$LJT)I4`;?boSmuW2RS(qy{_rzLrllna2i
zw(xpxn5{;>F1l>KftRM)XqYKVUaS>`b1m9f1d(g@wD^l`GHt0h67@_nA(-?655tjJ
z0H}r0BjEr*@O$eKlsjnOM*ZkA-@rJnHDr*D$c~<*(y=-7^Y^G_4C@?kr~%j`ylRj?
zHQF!$U}4?qJ2uK_1yU43Ya<de5w>D_Y8ke{*+uxQxW@gxQ_D6iXu0TE!ASiv^wwC&
zvOttK62VjNQ9Zpxt^jN-n*&3ii$5s8<XdBopl$9c67(k30@+aJ0N5g$3_z%srK!j(
z@~}|(y2MZS&~0Hn(wJ6wxL)Jo>nBIvEHOOg>GC0%b`qKq7BT@6(Id=Yo#rOjvEjUv
z`KHf@nq(XI(>Hg9@5-i$C1c?LNSER^(~rsMi1O6YH4>pUJUoxh$I^;w_;XT8K^wL`
zKwFzcCxX^4`=_QH5lG=~x^Ld{cZAvyJ`GkUHx*O#t49lg21_jS<yIL-gGhB~&6&fv
z?AD4bl}Gs=f%F(gGE>+0?D~Y)3_ZOM)_)B7J>Ws+6n^TnpqG#VYSK%XQ6-3tNGYe2
ze=#&yFO)nMewO}=q*ISW8WRm4toZ<GYMdOLUpb-on1@M^HGJc)F^K4>B4s$K%rbQ(
zIQ9+W<Af*lvNlrMb-BTbPLtA6zURrmDDIiRiG77uu6MX;<|mct(~%_ITes1(wKT9F
zAOkoc7Ja<df|<jf$D_=UsQV?XhX6M4xmrl465~aXG|<)xp2C&FD)O?MC)Jcu&Kela
zPyckVjv51Vq(Af_oeGkK?6~;^I!zh0%iMYE)Mu~9U%F%<sezo3MwK4$<~6eObXY@{
zLR`78H}g^($r6=iT-;WdcQ~TjCo4@49(w5}SjC`K^Q~7auN9~zHF4JsO_b&j*+&M~
z3u)I&gfVQ`yVPQuSr`Zm=qqSS+(Y*&m#H8Y%fqOM{A?9z=Tt{l6)p~()}4xGVyR?}
zjEWkYPCL4M$7q<6{8gg+4)A$25I26nh;3MeYh^qu$bRmyXMSbFeK>f+Z;@>i#67Nw
zgd;U=n#3AflGc>N9%zTmN|~d{T+Ji#7Q}+;O+%C_fA~J42q(_!wOPUAPF*Rv@Y>HL
z?{TAx2on>^`eYn;wmrQCAQhDM)#%0{c*@brVCGIPw5JRMiWEbB)!%;nyqK)nJzZq~
z;q>#u!>e7;C*BH81rdJfvKDHVRQYb}$ETa}-Y!4sF|vDiEV_BREJOsGX{Qzw8($2K
z_X<TMb;p&&gO0o(X%DP^#Z6N85=rIMG6n+iG>}Gt8M9x`M6k8uVm6NysO2S|r}n=#
zrhY@COiinZ8^@op@$-kTf7F8?5RUq_HfU9rYGp2sRIf4`E1^EDAE*O+p54{*5Tq)J
zDT%}*8LjD79!;-M1SBns1`^ad(S|Yu!^KEq@U`PYz0SNTA!mM>{#p*4G@<$6_arDl
zz~GOVB%SJq205gpo~<Ij4oX1_P7A#gE~W6dGAgcieeY#WEa|dOc8}@^Cs}hH(Q5cB
z0-Id0Hnv_#6QzN7Rw>x@bpiUR1FMx^wwp~kA{Vq!GSy^(G|ET-WYo7t98$xgitl7M
zei=%Ho0cHU#NH`o5FG0+8miH4IsC9lPCC-R)7@ezV!`8l;*(`L>|=Vwn=tY?xVupW
zOmD-OC{;QWtC1!Rz)u`ZLF$7Qf5v5QEexOm#OcVw5|cYw;uYwdN~1$ClF<OjXcN9d
zi)&|u`vW!O!zq))^zVez*T;ixhFjK{s0tKXo4*Sl53BF<`ZT(?2gS}c8HtX$yM6T<
z79HZG_l!*aDeN&CwnCX)_kc$7pn*3@{9!qV{0@B2Og)dVXNv?`(Xg8(LZ?R2aITEh
z))l-3#Sbb;dMx#Nl02L6{iqnBRa{*XUw&&F*&P57g_?xg{>ateA!q3Lr1|0(Nn0jp
zGE02;pv=o<gLidQnz&)CZ*shUvY=?7q>L;L)Z$`U(*ys$5%%S~tJ&nClkYPoCSf&x
zKHrK@=a8DjGxTiz39;w6_?l1iFkUgpkEH70i`>B#wCxKfs%|!J=m?HieG4SZu2vn%
zk@*w@3j5$ZCX4nMKyRAQ6|>=D-2q!`-hZ%?(|0Hog-jZKPf(=2hpJ+vBFVnpjWt1&
zu0~7$pmx>ffr3bn;JR=Y69>d~-Zs6p;N89t<k3!7Oqx7d08UuW+f*W4Rz=9nO4(3%
z%_OTMfGKUVtD2=oe^OK}AvcUp(?Gk09b4*w7QF)FRJu(Ce-2PH<dIIDj>$%?8P4<g
zwGxO-M_uq;eZ|b+<1KIXdTN8E(_gl}XP_K|<sz?n|47Qr42l-l2{0?$9S3<A?s9B@
zKZ9YrkoAl9J6UP>wldnO8n(Rc^|n%T*jRz8!KvWKRkE7i7UAO<g`bUSou{Lbs4Md^
z{f!y&qp?^v)8os$pJT+>8W1FGa{+J_^e>2yPHTBmJF-f~S{)|)D~}BP;~$-GW5lD6
ze7h*A`AEL^OH@afBD~PX5|a9c)R|?v^w8q%VuGemiEsl?ybe>;?8L#MGpRxPS0Gxm
z5NR>vW7deK3jT&LmDnN8l{%n>f&O&&lqHj*VJusOpu4l>UE9RxhW2rJ@z}hd01-@P
zAgSgrRwGEcvLj0eCYgfsFb5*PUrCr}zNZCD{tHGqTaq?B`P`$Y{_0+bQ4ZP7XCJhf
z*lqm$b*GT%1MDU|y~Z<6HHyBL9yR93W7AnS9PMi!ByQI-)6xLm0#NWONeY4Y8PMdI
z)d6hLkF_2Hh;#(?Q)-zB&}|#0Rma4radNjDPuv8@gf#f8wq9i>2{sJ1r$dG{h_YAo
zp=-;V-wxE?!4~HV1;CazWj*=tF-c`x)DqI}U({LqU<&tZ@mjc2N)7w0EtI#Ode1&a
zmO##?1_GW~0hj>*^oLl+4AcPNTd|jqAEOmLe~(OqVdjAyq7Qd*<Inb}hSTQ_cssT&
zG|sAUa%^mDd-?lm)%uC5@<`ufNga=vgi6w5vg*8Je`fEowP){3(ZZh@#Wp!$C|B=i
z3D4MvScxfmjBZKW!2-?7oI>lQbdjyWNdx{4X4D}xh<op*D5j5Bzh;;|S6?RR1RzH%
z8waD(f_9F4W#}189H<-&4E(r5M=c;}88W>Q-uf56vN+)7_7z%_<=cGDsv(5~vNQ=A
ziK+$`Y6XV-ItNo~6W$K|#a95vOTS~`%(ykPv5T!|ep&+*N@cB?@E?LZ`i>2mLv9=2
zvy6s)^)g$Viq}R+hMdpBH7!9>RkEqHNX1050)Uh?Kaxz|*IQ`4Zj(bhg0-mBz+hw~
zJo+s!e+diI;{Axp*^+`$MMwE&cmtW5AlZP+w!rC;m#^Nw@UQj^kv2}jnb37iBas>*
z%ef!ugF~%fV;gcachrVg3~AdY@6SI=I`;SYaVcsBf7I?}|8#JnO=D7`3ORNnae(%1
zqL>bORr!J<Yjve2_&qH$rb;AZyG!lsI&EC`nDPhGGR8E|(Zq0q^Y=OW8EHrBMcN*S
zYBdjTw^{Nl^3Camk%Nyn9`QRn`#U5K9%Q@v{Ynu!7aUjo#$N7Im|ru!w^|`|kQ!Iw
z26<u0Pj-~B>HkdW$?EY%+Se0zi}OXkGfK1;CBb8t$c3o*N;JFS;|96$wbFCAG>7xW
z&fBmw)zEg>+>uj-<l(PaIOymKpw(lf4_lipTK<u!GMU6by_(2B_RSCTXqMR4<cDup
zxu3t@T!^3n6aVAKco<kn!N~OE>!&sX5uXX&zfS-3yO-YP)zHI~v!Uk|q7bjSn?u@e
zgtg%Bj@!oVeVKnx7g}!sUXj<e{QOykd__{r@)JL&b1Tcv#ju8cdqL2-=-q<hXR@ao
z_?+HcYri~3F6@ln6L<`#p5GI8$UY^D7Zi9WC+O$htjF$kEY~`)e(b}MHIr@+AAY!Z
zWcuTD5!)#D^quSu344=~-m+fD$gZ!5tGB1Qx3EV}xW~r#cKZyuc*nDiLAOYZ$K}A6
z1PPDnq|??P4%L^~ITV;}`B5&0E%t4+wnt?I6`;7h(=H&-wiX&{r{d3A*%*<xF{;^G
zTQu~o0WqS^RZR<7$A+F^%df5ZUQFt(opb#-_FQ~>*x<4_dg+%*$K_qK?&tC8LN4b;
z{Lf6AuL=-9Z_yCtA<@s>M)oC5eZ>qZld52^HpoCv%cCQm$*7%l!$sb75re3#@44^?
zC(f5a{a%Z2PTr~2?44dZP^l&CpY(5h`nIS9Zc|i}?btyR&lSKyc_@a%aQQ5Y;;_it
zzRh*dkILebN;iB5`OtJR#UW!}(3N51%(vy_drMH(u&01K$(p6+S=-#+Lg{Glw(M|e
z2^V2kZs_sPHn4LmW8KA~z6i-d`!IXDk<Nf$1+Hs4;^(8;n(msrn6{LC!hEMJffmPd
zLe}SkXTf6;TZ-=9KQ5KpFXy(_+7Vu&wU{~Nv%ONnx$V;I=)V2U@1CK9<)hiPsFL*@
z4-unOoiMMS)=<Q?u&k+s%=3nz=RFv@#e0e8?O9MtBZs`LfskWg*A*XMzmxU~h_6i!
z-$iEDlXs#i!vhUjS*hcfSq`EN1!;JZ6ApYug{dXDFcZ7Xcex|*j-kVg{hYb|r9;R9
zU)gG&>JyD<v;^u15B|AMi7`rVS94UB{?>`~B3%~aJJ0VYHr2*YJ5vlE1CVK+cgvWE
zAeY8+vahUUiLxoExk)aXRTL>H<y(dwU+|ua@Lya33`H%|hP)e(g%`>f1t%5Nza4pc
zp8oV{qns0YwDd43j~y@UfrFwiL&)cvN!QfMqcNS<Ea|3|#UcNm{+HWf{o$K0bAhV#
zar82=6WWRZ24zxe9IV(zA{8)QqLhL~B@pfTV*vF7<oD5?8I%zJimWcld16-srHX=*
zS-<&}xV;0N2)PH6EBT5d-WNYdC%i77-KXLg{^@?!c9C)1$05o6wD|L8nI<$s1>Rtn
zx#0qGg63rDUy7D(ATIofP^wBpVoQL*4|7O4sZpwD<!Llcr~zF4J*@N%Y}Io*6cNj5
z03=#qxG4|=M-8CR+T>Hg(LXftXqKo&t^DVH#WRS04P0Tt&aK3NIdvAlP3Yu6JS!_M
zL&L4>yC2EC8@Gql@igdV$>*hk#RXpt%Ls3uv@G`4x%tayZ{Ht#qP_4kd7^+sf~E<U
z2>z%?D_$5}stLdV3dCnu+ql9Ez(>5{{0vAq&ry+pSYiN*hgok=A&}FL^#?*n3uzpR
z9X)qbF~zm_?KO^P_FMPN*4M+~L6JZFbobhb*tkaGnldn>yjs-aBHV;5`>ahA^DV39
zj#49y1T&^H9tUHI;h@l<1e;Nm-USrVKU4(*mY?=8%f~kp>(|X4@&>B`(SrNX!~g)C
z`x4$tqOWG1PXmw&P@*M}>YECMK+_<L!qjT#FWw?2LT4LjdvLvsfYgk88@*D^9cGa;
zmcx|(NylH;lAf_A5D|tW18ANrQ<FSV1iaDIMC_uG!K!uWczSfIGcJ2vQ>p+QEMyuq
zGm<Z4I_YkLr~1Q6+~gw2gM+zMWU{8T_oS@l(2hO7b?BZBSM14DPG{_-#;}RpRrDAB
zSk8b}la=MWF+cq2FoSENIRuOn0*%G##nBE0s?%cP;?g2Z%H*#Xi;>`~l5|&d7=XuI
z!gO*0xGYkI!GJKt5AiVo={G~Ihu)!w6ZFhLq#^(v)#HN;2XWzc?6Duh`$mRc)>s)n
zk*!NinM)s2ShPWA?p_{(zaBa{w^iorTPr5Oa}y;3W#aDuG&ylP5x-==5!W#0!g{Qw
zuA^^R+HFP}-9?J=G8meZEAf;LCwp!?eo}ikMY)q2c%Rlgin#y#$>~h(=|;hx%b9mZ
zDJJNrNYm!v<k95=?&nteF@hx<umZ%*Ffig-4kX|mB$PWSh&y7y>t`J90FeQ>c=$L3
zD1_8BTy$V=T6#{NCy&L&P#Jh7lo5By&~A`O*k3y^#bV79BKqdJDPPPtCZ7u0g+R0p
z2dF?Ml^aA-s2|M~DfYxoINLB`^yd@K9uaF{;j3mLr8dx>arWh*jU5lrx|yKPqyd-R
zGO`X2UfJ_v*ZB#oY+T3jVDA@COz!Ai-g~wa^;K-;B_zWFaF8xdtbLjsP(3%OQ^QU4
zD|ngq>CeVB>|P1Z9tWa@m_%<@$^ks(3S5uTCQK!#S^EZ8KHW`BMnYHX_b5HV;{=1X
z?vDyfhgqCBqvN^Uc1mN-sxsW;Bg`LemGaJ@>5_?lgIxjipPB2!^PT(BxIQqdLSCf2
ze8eWmYDb&KMz@y+vVqSNjLO36@o9Hx$~<_^2!<F58yUN{*21{%LF4Fr@^-flp0Bf;
ze){P+a}i<5>K%s0r4WGPuBA`=i37ufO2L`p12Qifn(aNw>Q;5QkWK3PyAS1vb5q4H
zqsxz@;OH~A5V_~f_u1cVA;0;W80md#CnO1a9thdBA(eCBM;o9<wKYNuIo3)Jihg<V
zWVu!cYO<H2?K5D^QuRq9(x$)%wfYnVGfWXp?`(@Z;mQ5fO|?8(oeJKq0tt=P$7(Ot
z)qd5Qd%GR)>_o{cU1XgH1U}|UTWO?RjD`9KXXLu~CGV*2?hP|8?c0U88dG-c%f+>^
z?LFi1CJ?Ky{;0j#OwnZ~RlS2=Z#2iBOYpjQfXLtrzSn4vNP5L~1DO*RA$ty9VQHt7
zQkCy_a%5(O4*>ynAH_z0xW3#ekjadjG}1c#x;1$!I+S`mS#Y^k&X^$8`HjQ&{ejyR
zK-T<a-J6UI;80BXvQG7KH*tKY!hE3|xhB?LA(7$h-E{^KN0UtXk217p7VSO{1Gw{3
z4D<&vw)d^aQJ1<WyjdAIIw$m;)Eihw@>#05D9ApC>J~HQr|g$E9i_TP#vN^?Y+YKO
zXZ{i$yaI@(U3|RguU4+-b`<Y<6{q1%>&iGo^h6);VJ+Th14Ttuzd-eK^M$kU2JS@A
zK+mTlppBkR{i9mBx8vu9l~sO8;!$rn<Db^sFgrgz`EfC!bFXS#iRUAhdb0%v!IWXg
zFZs#*l>0LQy6SnXl<!G!_2f_B-mn-Eo-L{YTm~-fYSv;fQ8Ju3HP8=zh$Syh7A0V5
zw?j@V^$LLNuls!|Sh*8f6a7I<$7VDGJVqP-LYp%>Ikp)29EP){VLIt<L-&;sRR2ON
zPmdu#p?pU<zA5;r-ZVCE{72GpjHHGa$_IsNb~)Yie&{8=xt|lu2xX`DQ#a~;)|+Pf
zn5|`Bk3ra63{%&eSG#TUZQ^1?m~y?{wlE*UpOeB#2XzJqgSa0Bn(HO^Hn!sFaobB>
z1obY3k_Rj;YxjRu`bM=XSypUDt9g&|!9myNJU6>{GFxS$$+UWuCk;m>YOBs;u!Gd)
zcSlV_ENrp*!={933tLV8LoHS(6BX0hCj}$6^s#ok%t9Nspo8WS^y%&VgL{_JNEB^#
zrA#`flRU$o@TK=L37R1Ro}QG!lYAT9zUvFAHd&-cab)I7+Q;(eNWGQ#4P+mpmPVO?
za6^=-_g0nWcj!q;_!#MzTO2ouaH5soTmcw#zjZ!zdZkJ4xe+!}FJ;SSg8g_~y#pil
zr*1h0*1gsMWuF4|;2w?Vk7NkE36bP^0D4f3qKv5VUH`Jz->B3J$zx<1x@6X@PtrCO
z46QI;_mXunPq@tTz`c>=6L@txn2nicTo$<Xli{wz=_fbu<S|-jGYAtC?@tBWAGmfA
z)3_H|ctpu0L`i}zzIG3uDhLu&2U@dzGM38>`Z-<$Gzbq-jTC6WCeGHDjI>rzsBCNd
zk;6}nZ~C6Bl5d~&VGwu~75uZV1nJ9mS@mSUj@7h5`5wUb=hL<34<^TY0-?3rn~u)=
zTFJPwR2r24>&-l6Jhb8zdY>Kx7w1OF5TQJ}E)2_rt*@osK8ilwHs!GV>w(N7Emx|S
zSY1`5_!?T2{BkN!uoH1H@={b$e7Zmwl{3&KI_Y71L)ccY6!(5%r0%w6F0V%q#lacR
zsA0nMHY6^d9I;%XsZth02H&z&uGSq8*Ji-ow`Qmv6+rW1I}e!17)!L^))xm2=JXGR
zQs3!QSMCK(&Jrp_;1-Sn3qNWp2z?w(0+_cmiMhgE6kJw#CL!F4?*tFB1P??-sJ%dp
zciAAe+=$I!upBy93DNsrizdZWMgwg_w(~7*+)_JAdU?E`cd6c_O5f*6C0P?YABj$T
z8dCi|*10NlZWb7Bt(UkeH`*9t7ksD5_(jEBRqL!Asv#%8tIqoeKCE~o_y_8hI6JD`
zHicxbSvG4YlIiKZg3dGNAxZa(S%h*68x&G4+IZnYzOEsCKmM89k0%9GZC)~C>ETy^
zXUi`)KVv$+iB;djlU;P9m$NC;OXdyk39mmBop|%auABy|45xlPp=i7@AymI9Nb?T4
zo;jp7TLkmI6ro-HBf2$~SkP`i*=j6yv9jWa+)>WlQCf5zx1aCx*UMRiG<oM_f23al
zygR8V%3fv%iPh+z!o7WuPll5b<7D8ng25Y-_36cO^~tZO(+6fJ8?y}ClX3bQjc)bR
zgyy>QT>B3&kh{WIR`*k}0fn}dLPyACk&90R!pS=|bUw=303F>ws+sISZDnNy^XtX6
zA3n0v!{1ciw@cV>>BrgdlWKZm0iVB^ClF)Ork^h_jdE!!@ECW_NbpZBei){KLqqkm
z^rNR`RF%if6`<?um}08skni!B{Ey63Se77(Q=-ySI@;-&<mA4~dtCJsFAWyd*1-0s
zuo{!HA7t3E-2J#$fH_-O`y=SxAPPvjTXG1o?}&VT^Emy890nh8!7n?%FsR>44N*Dk
z{;q_+r(LgWhf+H0Tl4kwqan}ltEJ{)rv}~os+3jjz^W?4o{iT*R>mh5X+TG7_@eYk
z+l5GM@!@gzi4M7EU&YFB<hutDZ(rBbtpn~?XEpY9#qs^65`tc$fE=zb7LMEkN&-;=
zQS7zbg)I8DD7zZ|Q>3rdUjul-`PLhw-|BU<I9U8?9KL5jG0Mh^n@eU4c;QZ`OJ%@(
zc@nZC)HbtGg}Hh(I3F6^{)z6)7od-3f0M2$2wwud6s4<1C+AaToLz2=K{dEX-maz8
z;oVPH{nvz=^hz?cYg#@wiaW(8p4)?E8Vve8Z{C<~l)gOCy|cDQ$ARI5d*Y##ne_nT
zvQf~Z)IyiXyYJs(pINtf7u_!YThgh-ufgfkG#3|V^d{uF16gyElZ1wHV*1Vk<1Xsn
z;^g<*-LNoPLgtB{saIhH%VP7^0~N9Rrd5LNmZ(ZpnUvK~C;d1{dw9S$)C(m8222gW
zaf3SdcfR>R3T6o;<;8;5!MQd8iBz^j{v(wOBdu*EUdh!eZo1mL7<+WcJVU0YemF_7
zo#X(JZN*sQ64d0dsdmgT)!Sdl_xs5(B1W7v`|i8FWb7jy<~q0+@>QaaC9b{$axuo!
zJK&DdV!mQ3!TZ2<>nMwa45m@EVESO3H+Z&8Ll*v?+M+ZN_QDKg89;JRmL{ZvQ!eF~
zVTf@*{8=@x{8)xm3#u$1l7<oMGt=HZy(GpGQ#qrzW{k74;O66jsnzPPn3VN*<GwyS
zfg1#SzME?9rgq=a)C0t+6}+rR42cQb@ZPLg*MFMK3>&L{?fu+?7_fo1fo$oJPWnM`
zc8KI4&hNedVn_(w@B6gLSZ<ie>U>Mg^x$sgj}WkV0Ga&Y>aa|dL%P<qdg%aF4oE{L
zqBwylu-qN1(s33)ZG$S4q8jRnl!(@7X}hx})QKt73DA&7E+H{vB@05O4=%1XFX2Hi
zGHs)#xBR&0KmTok_{i~H^7{!t_4^=zZ|aBHe4Z?+iT;2I8k1JDF)SKAP~8hu<qFlD
z8B6-wAu9^IvY~Rln3zn4;;CU@iC=h;kC>V{mDKChVx6B*xvpM3i!cL5in@vOf4>57
zJ&UYPYb8HV?Za(See<^CDAgh+^5ok~+!A`>gbn8nLnDc3r~a@i27*!MS%o5S`8Nj5
z{>3R1+o1bvx-WIxFy`56Ytrf^a<2dgYirMnPkw4el?Z+C3DLh?R3_}-uKe(DERt5B
z4KI1>*!Im=Bh7w3)$h`kLqzg60fufyTZT=f%fR+$+7DTsWMjzIS{^iN)>i7iJh=jJ
zUxf2lR|#P#aB@LHK3p(kRDLkpi!C&0c4Ht)Ah+##r*+{8ChF%WV&ODF^&S3ZBUn0Y
zU}?}*zMLg5AAU&LGT^>E>13ahl9_Em0^*9XLGt%|G!U-;L0=^Mozg=m@CaEG=Bx2b
zMWv^U$$r5rQqMM=X_Yl~H8iD?uK?>mtB3AoGMqRbyfF7`qxG9noZ5nz%gDQ(^jS%v
zznCna`PBQg>5^?M-)-^7g60-`)Vc88GYH+O=w;{XvBc&|RPnIa6`;G*z9cu1;Ol*j
zty0Uui40KEpzp@HK#z#AZ`*F$D^RVew0WIbgIv`oF>3o_ja^)>o(?O80Q>CS$*N@w
z*#n_&{jQ_Pb79g>LZzSh9i$%x_CLW-F-=(Tri~b%>h<)!ZooL@_IEaW98Zy~{DCZN
zPiL(vQzuhze=P+<p6_M8k*N63$;2{|$m`|=+epmxf~`e=O~Y^I?{)}Z4S)GdI3{i7
znw}GE=61E%gm<Ln@P^NmmbztyXO{bxg;x&oPTBUCLB#9?QoF+Cl<8j()(}OHg==ny
zq5CeOPAB(uTsWjXVL;^@w&zJ|g5wP^N`hdV<n09PT#<)^7F%>@kyY|##TyY;e7+(x
zPvfwj7b^Gowd4MBJf*VbNE1zBc|qtpyqSP2W@_4e1z;`sab{irb=s5$#=8dwf)4O!
z^*%lms_jb&_*K(vO1hh|)^)a|f{z4!B&Xd+JBn8Kk`~^>8uu{e74?DAo(iLcG%4Jz
z<b_6}DL2Z}3k^I9QR&mVA1OOt^lBzp$}dxRtRI#cA10(3ui~qJQE;+h*1Rip4$ZbM
zW46JNM)ep?flsNm>REKkvC3&{C<C);<!S?~KFy^|2L)_aE%_22LZGof$7}a?x;4P2
zstg8s7Ok=vIzLWaLTvWJXok}Fo^nq2>a_4%w2!)QrKWMEzl|MDUK7k*8+$nspPKF^
z{bM)GI((lv&I_~mK&Xn+Qj;6bvg(<jAUs;{l>v+5Dk?}(|9F5A05MUOa`>E__T7T6
z(?-hB3ex|TOD+iKOKUgHIxD#aMp#7nX@(Fi)gmPyCV2CP1fN@$b}Di{{+!pTRo+O1
zH#w(|#z=u}sVoJq4B`#G?+&H|-49Z9Q*1Ur52If7N*|>)v?qP7DW7jzJ*5z6z7aGd
zrsMZUU9(q*xK+-BFDp18?f`qw9X5=~Tr5erYx1kMeWp(0PNEr;S$3CK`5;D1PhI#|
z&K*07vQx75rNM7C^J`HZ=sFV@dGz+KsXBw&1&TXa){`J}Re5QP@ZGqaq>ppzESB|B
z?nOOsP1A!Xv@|Vzr0ndR@7KY^wbuHF*);Wvc+a*bm5kbL1gRVpT#Uw0@O32byf~AM
zYuqKNL|&<WA~TewJLLegwyN5X-;l27s}=dyu+=wsRvlrRVNV~pN-(*w-of<CHl#bn
zRMpMvC3B7SyutU<;ovPI=XjGus_7+_rg!I3F>$$Tjf--)C$4b12UOCyWX-5|dAEGl
z8`c<|^cY$CD5rQY^|I`gb{{$_Prb-^r?~&a$tcB)`}N24XUkoePpiRASp}}oRYcv@
zFY9B{%s7U>K1+oRX2v8;C}1$sBrePH8!CV0ManXsgHVqX!<_pFw=6%iL1`Yb=|*5N
zi8&>BwC@U^d#=O>k!qp6l)(%zx%ar(oUqAJsNxDhds!qlv8tKBav(Yozp<&*=a)&T
zhv~lgBYje)QQr;xZfSGBBjF0buQ#oCICF7!thL_sWBl?X_S8!YU1gxlIZ1S^eYZtq
zce$sxmN{_iQCTy^LP*wZQCeQaK-~GWb*gG-XBQ*yYQ$elXgEgMwBeot&nNf>q-B+V
zR^I7X=dJuMkMCD?1t=n0()(&R<vovLqv3-8ykrjSLm7s&60%`YGN*01XLekVH5stQ
z@@;|XBz=4LTlIt@&&%_NP`%jnpb24CxA6$y#_u2Ms71Ccy|p!0gOKZ!JiQhuJ@O)4
z8*q$NKMFE6Sh#NlS8%#tjwg16u@0R$+i{TihWGD($pT{?Q4jFF?HI8eCHc6LvRxo?
zm(!BdZ1yhIhf8niuYG-e!}o*dh#v(_lD-x~E#=VSuVQmR|A6miXIo(zoj`{2`GCti
zgEU>^y`K3F+RJ5&ck@K{vf8wEF=N<^ERh>vNVTehPXcf6XQzZD|DuyZKidj$fqsOE
zEGl4hHbx_5ra`B4?(43Uqo_buy1Ps0XUhkL@Wd)5=Z+=|4`{~RhKoSUy}Mp9mMMx}
z@!ejTI*qoGOEDRJ8?HL@lpc~9BbM$u3yL0b8G9a${Ka<rMH_xfkG6IAVOxt*9vvCq
zHoQp}0zKYlY+8D@?h|J$AGH{I*bHrX%~d>lK4d(kJtPA^X+m_fZy4LNG^_*8raTxG
z9JCkz9>psd=CtGe)D;`MCL?wjz~m4@lBt>!%>gUT)gBO|k*D@7@sV%Y;e1oNle&f%
z>?1M*7SB9X5D?B=e$BnyKJr>$L0sD6U4cFl+_8Y);$k&@U3gu1!$LA}!*Om;FJr)^
z*}@}YXellRnljJ-D1VD+mUQ9Kqo0<^{K8#Liw%aqQh)9ab6R+8WE^bx>MRUyxMU28
zfB$H_#m|$u&D}(?C;cd3XQ)h2BRvGZ1y@VWfdu+tjkd##__T(Ej==GXj&zS4N0Y@;
zxZ>jCp4ang=$WAOAL6TpFH<2o&zomDtEas2^h@BxIqig3kz??#na-S3W$@xUIX9Bo
zj73M5e=44MKjBpWZZn`TlQ@2+6p5Y7hOaI?a5N*Ke4;js3^Ykj-m%U6?BvUFI`gIW
zv7*vJ=c>7sybqWsYv~Fg;(eSy^hnR`qalgBiRn+NdoB8%6FY4)__o#q?%!I)dXPN>
zGR2#JDEYxc3pegr5M6NojLz_4k`;_W3doj!^i-|P7)^gr>NzLw$}E~}>yxELEzlLf
zQoo|YX@zSK-YMGLzsm)0{{&gY$NyBLv@$kQZgWv3z?~&<a0Q4n;O>~cUsAK66{M%X
z#L%ySZz^WSXG!_Aa(gGd)GRc@`L50^E(&Qr&N+d9@&`g*B3Tm!YRRQvJP#XItV7ID
zyj83~vt)Blc0%5Yo|7vR#vniQpLa<_sdmq5xAI|WF7jhM-w}&LR=)suM1-lC>=hvD
z!+ce9Wac~%9PE9)Y}A7vJJTtqTFkxpZ2M{ULCTj2i=PwVu4?PaJ*7vl-dBD1)pD}L
zl+Deao)O5mB84kq;|vT3QnfR>#-8VVh4V(8T8PGinII;wF3v1c3N+G-VwOR{N2DLB
zj?Fq>5t_v|q;k-8PG(w^9x-gBa0qvKL1;<W=`u|+O-R@A%V4V-x#FAq%$Z`nwC}5p
zD>&`F!kjrs!QGjRnv)i3j{-NG=k_w<U`K>k0ClX`^;YIAFMQppH{1<qnZWej6}pbq
zn*L$o!lQmAx{pyzA2TvF%!JE_yrAiavw~|A&Q{vtyQP*evqxGR_Pbx~9cvKRa>IN?
zGKzNTqb`b7ly7@G`l3!t)7`cAXRiRqSAc^Pn=61Fx$6NHmeAT3RddkF>w68vF9qcK
zL6!;AAHNN7y;A%{aObcMpJBRWo$yRb>Td59peNTNCQX_T)hW=4LqH>=*`;35y>Mup
zbVh4vsNG$orn)RntXwJZ^DtXcz|y$0mFCAhBaC7*bj)ySlD@ty30H*Ae3k^K*P2UG
z^(-yEW;%}aY<|K23;0v{{Nb}xB7emfX$7wGs|4#!h`aJ2#IvkOKqO>T#Pg_#_w@lp
z!o7oohm5!*k59k}eqv1fSXqUR+tlgiemvq?S5ctR-$&lFsi$2LRmQ?7mzhY{Cw)(n
z10Joy_83WGsUHp&>kgK|T?t`a?L$$G2L)4}?>=-@6cmOL_c0Mp+5ckTa}ew8Vs2lp
z%$LJFq0s#$blHa6Ez;O$?)<$xASNcmoyh38P^k(+bwNRPCMDn-m*LKF+<-}gwXwp@
zi2co}BD<T4exdi%6+l;zxhF*~2W+Cc`}CgZ+Yk1x7NHSz+&kwO>ZZe&aoyFdXH@t0
z1wt;R9D>fZYqXxd{q~E|QUlx|eg()Z-nQe5mmO)W@?*j;uyAXB6t0?Y(Nni0TovUq
z`1puZxD%W5ZWNLrK?^pI{cCYO#z*!sM0?3~i}>FBbk1iK^5DgL1(sr0fY?Vm0u$+S
z7u^G(8W#Z#tEl^NRWvLUq1?+Z23Xqe)`JtdjPRb2W1h|MIiLG>-}$R`_&vsgqf`#R
z!6st#2g~JuHbzE@&i2Kb*1$9EDD5b%j;>!$G`tXdx+19&$3v^bQQ_KfoT)<n{iNWC
zD#<W%VbQM#&L_-oC6*j>%hE0Ao69`yHO>93q=bD^2<2ipR3i#!w0<1*uVv!~2|bNu
z;=2s?cATq)M#w3WH_#Hl{U$1P1t>o!6`5h<WY4|=)D)%iatDXz4)QEP4-YUm6dxQ`
zJUJ}r`^J6+m`pM?IMy}!iN6)StffAfmB=b!WQUo=^Xvg<a}o<VN16wR9o|Uw06+R!
zZ+0l_xYBp%RP0Xn7#2t2fsv~A=ax*ovRtl0_w}JO*xk=zb-fwnVW8>(jw~X!UhNX;
z61web(d?EZt@6I}Q0APV1Z-XwGwagL5u&UowHbIYDkAAag7*6~7X)-flIn)_Oj}&A
zNPCfR)y2i9d}#{VhSjUpza)v_{w-jiCN3c+k%71G4zlrQKdBXwGEMZs1)_UXb11ge
zpK5UP#K%ZmFiBCo!}3koY%{}S_dIug_r$pj)JBT7XZbn`of40+Xe*LDgHngmfRxit
zD_3(v!l{F*@xXB!#5u6b@SwalJM<S;=AH}*P0O1TYp}_Tf(qoL9~!wq+$-~(lPxyt
zD*)X)hBG;NnE{o+7>Jv1g=BxgVodU*6ib6}XXicpi$eIKBKe$of-4=$UQF7oxA9L3
zvBVvyn}7z>BWWDOcC7!GZ~LB%hXBkjrmKK44_DPFCiY>kqts`2d?690QHj^?)L*>^
z<PAnThh;MAd@EAIVz8FZQ{Em*xtc|8>14I1KT=7k>FRdqbRsofk&_W}hen0mK|FcG
z?5W)x>%O0GcIr2i%?OF?-Pn>?&6Sk`gEh98XN76`8^6s@Cy`eLTi!qO?v*y9uaGdx
zgC-gx?02!1E)r>S`<j|3ZsaNka2&fPHoBA#%8~g*GKI{K*gz9Sjrjd=UQ1V@$n?=M
zN)2)*iocIu{&JQUb&oxNTuNp-mS;F72We}E#)EG!gh6-O+vt%{e7~guTjBt;ibw}~
zr~^&p8;Dt!uV=ghs0Hh?8K2?yDoW|(4w8-v56VvyyoOH%L_33PA!jCEci04AGx^XD
zM;9#2hzZgERjW%YwA%BKOH3x;O_YXDhw&0Mbx#(pXLdr@EP=~>>%!wKMWeGyMwOP)
zHQObp+cF#!xCB0iCuY$3BnAiBOXfNA4~OwTeb)1=aByUnwu@HKN=Idxj$Lk3r%?xO
zoi2kY<>3gEtuE8&D0|7?S{pr%Q>E{og}QCF)!nCR1T*{%*(1#sSm({;5nVC94Fh~)
zM*Yu<n2YH8yE0WM<qs|6g>w+E(kNEC0h#M?j$$81$;1?9RC?Q?m{*2Yee94%LnO%<
z*koHhfQXLk0MYv;eeqNvK$Te9f|5oqX9F>fp_^Wfls2U6RZ)EWhx*UPF!b#E1b*>{
z?{QHGr$rfw^S8uy_72IdE({}5J@ZYfzjNp2_t%?Df1T=a?%+|=aB?BzD-+OyA3sqs
zzDe^)*J(b{CWAmltb@5+K0B0I->Tfsx3tEuE-0bvTa{z=IgwGnOnDc9iqr@Q9LgHj
zMWEa`Y0<8d6!uj+lN1_u9n-|*rxPlY&e^X+`9e(Mm6M1=ncq5Qx4R|k_?6&yfT|$Z
zJ<})WsDfS)CK_!DPhL+SUI$-1L)U>3**vxewnpY^#3;Me7UgO{X;<mP>~r~6Bi=B6
z^rf_jkG`eV1NbKmJVAwvuWT&qeWYuBEbDEVXQCV~$gI??bU&v01!`)MU7H*lvq~cF
za`ZAnVKe&XY#W$yN}#$Sc^`EmjIS#Sv6I2D(uLzr$}2!P_=g7g$zLBqle!R1dlwvE
z=<F<fEaAE{?DqEkc9_w~R>7CVG_lX|GTu?gs!E{paq_2l9n9IU8zHPqYixr@BRG+Z
zBaZUk&z3&Jz`CYsFYXykL6fEtE$skRpCX)^Bk*&Q81rks%e?z&jtu!~QR*=3seb0S
zF4sM0#SB|+6xQ1B8)b(xjQt>E$QQ7qvK|0c34;eX!nzW*vp(*K)`sLQzk52OU)=ZJ
zY_C+f=US!wY=7IIp0I?5qosMC9KKtPGZ&C8pvqj%8-^I&L_-~D(!trnJ#@8v1vMJB
z0d`sPm^Si}Hu6uo_MHb(6IQg|m7`H%49weGGvbnX{Nxe6h<ToVi<q>=cGg$@dkdm%
zzf7%DqbD|}@75YPF-<0>)|h;;9{|lsxr$jU?J*U`3lV$9ut$kFp7ZHIzqLCYXYI<#
zIejgD`-DNdJ^+I`DO1<eAzr5`>|Wea*P44Gr516Qi&5!x<D5Nz8hrhb{iNF8r7lgU
zF2>;X(8~@#^mrBRXEc(8L)xt}IzbP;I?YV?bgN93Ls_v|Yby_Lnzf$Ay4#MWbM0!K
z7b{9FwqoS0VSW-?LWnts6Apzkm#b?Du_h2`F+yZQs06Y@yR*Bqh^KZ;aTdtFT!d{z
zw`Prat`r4(Es5hw5_VXH4#ylbv1orMi^Pg5ii6}^j?0XsrMq~x8u4u|I3SwYz2eon
zV2>D{?iHtJ4QSJK<hWJj#1B`_KqHb`FC+2@RCL5Y@-o$>*37R^)`k$p6s;b=3#24A
z?T!6K_{H;jQT&;F@$GrS^5%9z=nQ`KS>(t?wV$R5JGoS3E2y0?L$^Gy_MjxUVD(HU
z>uFR@7|-z!7DSL~m#Rn-h7hPoBI0c{6wy8A>@!4(T;biDi*KWvy|Axl0WzP*E~6Be
zVd8l>*YtVm=doHTVk9y@GWw!0oFwv(tD(b1t^?ZU1rlv??SWZE4!s%6x}S#&A-R$D
z8-eyY-u&6SF<-I+Ut7yrSDh76Y0!8Vk*O_Ikj2hK6<W5#+3bcg>3@_^wq(g9fQ8B-
zGA-K9{Ar#0?HAuho;k5c_R#sod=t;1Z{Oj)*B>F6>ncB?0jgk1e_P5f(~zaSCqWl-
z!g#r2N};l~X2xqEGpCb%xoaaNK5p4K?39_R5MDBXyZ!8rxU5_WZ|!Ga%K7)lS&_W&
z@VLnRadS@1^uF=1b~C8MPCAZ~Jr*-@Hv8Vy?wEVv+R=HnMHQ3b<rI~_Yo%F9IFy`#
zo|4(%@tRExh{uWu!>}eVYTllj867Nbp@OO?3l4agE*S20=f=}X=Uq6JjFZ#brt%n%
zHH5GS?mH+Oct`9-S03HJX#FM~4+*#e1WZ-t)ln&jXwHg^*GFhI(+wp>5>H890XX|E
zrQ#j?#Co=NgYspb1}cpeac`*{2L~0*90=@5kJ4@79tV$&DwCu@BZ5B%ewQADLjtTv
zcDLpF@(QYX14AFiwC<ew&8radei|JuuV7~!gAp2>o-XHo`W(lTQh>5L5l`)>j;1IS
zvuVe`|0O-8fYxa!31b(;mAasS{==$t5`&B{O{I=pr8fK8`4HYANe~h9kU-Psn5mn-
zb~iACTZ%7A<?Tc4QTgLD)u5PI@v&;o95v2^L=}UGh72p*xK}57zw9Kt1=Ph=xfqyh
z-u_4;dRj%+p5JC9-_i4^>)X!7V4tLD+UK2ac=r#V@CEqpoK90(5FfvnJ|fHW>gY3J
zr<tjrdt7Isw@bnm49h`BsabjGX_Y~U2yX3*j(H|Do;m$B?OtGqNmUx@b%>|w)Xv!Q
znNmJw&W^|*kwAEJsj#8~E&klQ73fIt>+^f}EgK!Ajs=;5=+AHxhfn5>XgjF{J*hH#
z5X%?@2Y`5r=ng6}3K9x3()Dvhh=0=I;?dmUdV+&bO$%1xHgSqUrW1SdmQ$7Iv9hVN
z_{*vf1WVOwW-hTKzn>{W6-7EPEG(`Gc#MW*E8ykwn5>vmZoAM6G;fsM`dK(>FlAc6
ztK5s;izW9nTWnI=w~49q;-#eO1(|+22_u^s9*se7mihP47f17gdmT0fiUVFM#C^mW
zo}elKv1f>Q@+V0AdRstOw`}?dZO6+y;e>c{k1G|iZPGW^H8-mkoGOlon{zkh-|%*7
zN95#c=ZnjToUo_0_}Sd+5KwUZRTMcio&N}2ImfY4H8#!L8EG*+fIj7Dxa7JxPrPK8
zZ<~oiO?gVw^6@^0!?Y>nE~~>7%NHU^{pK7b)ERC1Y$ueIWTlVpBy56SAgNZ)XggIG
z(_}$MGBSe)(H;Kxq<=njdL!`Jm{<(zY479{d<7s_{~?S=X5!lZ%s)7Y>u_Qqd|D0O
z8p8DC7x!Lv3C$`YaqKJE%B7f8d_3W3MtU#mt;WMAV~$O=(`w3YW!V);wGXMW70HV3
zYorq`a+b^o2j8h?ClO0RbMbkjbNEF9MWrEeR!2_X_hO{m2mR}6x*|H*oc8j9B9iS`
z`(4{d;FDpqNIJ!kAvZUCPOxN;AyRi^U<QuxcWK3t-3Jc*$LgBvjo%*+pV{ItG7wvz
zGu4OH)jZ5q&}ZYl3u2lv;bu>GeK>2L76P79=)}EiQX)`oGl$%@ge5}|o%{tkeI=ld
z?~QFbd{#yfDX6bF|M;y;pc?ywcp!<ftYjGuUx%)~Z%jGd-iAz9pUfDL^;&yln|c*L
z8SBLd<5!eaMv1cU(+m|l>Z&JBWpP`byV7LFGDiwYM~&*JVV;NE*~ZvgZ@d`8i&VhN
zG^kbrP;?vHfLCV9Y8J<Dz5pU{Z~zbLH3+?^zkC9mDbqaX(TyKbU~@ooNmIsXFiu;4
z6^WZy_PpdnQCxa)GOW6oXBI?HMQ{9_6kNqx90VoJ2|nxjnkV>0;v3lm{M0T{z1O^V
zTX2_VYOSln=AZaSmpQ$6e68vmFA`Bh?Uf&U!iFnhk$Mz@QkEZi1t@da=nU(I4MSew
zdcT6+)qu|il)HkM0@sPiUvZOy`1iV`B|2nm>d+fsylvJq7vwrtn=c9|%Xa~S1K-oV
z)}$wJ+$?HFX0Zv=7g8=1sDno@^lr%5jujIeH!A#?HiVPLU)&X3oT*|diSA)>aj?Ij
zDff9b^qh8nsa%Fga^H#arDs+MPvE8IiLl<*li3eCcCM$-7?O^j@V$Qz=gt09zN9XO
z74;6=bunQy)x&^U)GZd<o`QgkTxN<wBELg{uGxND8_D=rD|I{UL4rACD-Z5ugN)P=
zSvK*Ri7Zq1z<&wDTbK6S+T>+;U~>+mn81)5HPm)2qusGJ7=4(O@-AxEc|hpaY0N#0
znkc0`OV{BPsohM&&6o;c$J_r0+dw40x7SqWmPI&mo@wKHz+HHh2JySS+(xtfyfhNI
zoYt$k?)0+oXVuI2b}s>*%J>al9C|KXM+M5}zwo*6$o)WW3VYq9loxYEs_MUP5MJ9X
zH#LzttTGv6qQh}$Y^Kt{8RUbQ$QvQat_`GW(wJa?bWJT3&YU1(O((KAMxxy#G%Itc
zB7D%T?%lbvJE*A9O<rgKY((FJoQ9=84-*!cFfE#P4A^~1%+3!E@z11bWF5Q>tE6{N
z`f1{+uVFDXgLP2F!!RU?Rr1VLPO<#1OPRo^6AHm-vuf#Lz8|qyslK6dxsLm-TlrRp
zdKGgY<i2JX+$q7u`#E#4$A8cXoU;KnqtvRM6A-DU{rE*hxF(7ZDsLg>L6jO3MjK5b
zP{2T$zQ9b!l57H6GnCK+sU6c;$+oA2R$;4oym0daB*OAYVCo|^UM4L<EoAp^iLJ3z
zbf(g3q(xo!)YzXi>X_0VWT)d7G!fG}`J%^DiX7>!In2v}!*<72+$=G@4Beh!bwZ3r
zF`>qwRTRWc=QUw%mf1$=Rr`~wb{z$|j-+F+6#f?X5v&Jy6FKo;^=BdGy!_MyQT-g3
zE+Iav=CVA(<#>4P<*pI@DS-a~Hs+$QvzN?f?Dq<Ao*q9c9@5k2!V_-=JijKcsBsdj
zo)KG2EH^Y*=CJoesbR*srja0OgPS8@uO!wqnn;wxhcM$8SYd{0O{C250P_H&gy))j
zge%1(42hjoSMuqqRBt&cs!$bF2#|OUSfPpG?+gsp95>?zI$&EgSv5xpj9f5J)2g%C
zKele2iX1s~;&vo*wQp&*XkwAdcHLX`U<oHhQ@X$5RW!Hv6-z}e{GZjfYdV7e0O^%*
z+)?>ahl@e|uTolY{{XqS3Nc#TMt2ugd^`UD+Ep(UouM!gZP`-YOq3q&1SqGQzX%R=
zgw~3=Q#WMP-z3==LL`MtWZrMERg!Dk>Co9Xb<Z8v8Dm>e%`gBYXTaK>Nba|T9z6Rj
zJ?0=Z4zB6biqh9MBz-D3kV9ZU%`%r%K)D+2s+PO_ld(oCFkRT>v3Gc_Ym8mlC*ss0
zqDPrU$6f~qVF%B3i;Q}*YOI_(4aw9jcq5*}hA2DF-763@xHYWG5yXyN-&g%3W1RiJ
zkd;%q-S}1Fbd5Wzj#>`i$k@lChf|0<Ia$xa{{SnsROZlsejZAupZ7Ix{vb-P51pTp
zuqK+JI=q{jnxb+=`VJ9LASOAa!;=mfJjtBbIiX98Ochf}?R}KYCeq^tHQUdYbxnI6
zQ(eLuT}V2u)3G3TOm6d)`8hiZzL7GR#+vK6DuQQ#F{$`fG#0Jc%@!QT2y<M^`J&*_
zWxVAB%<Q_h*E|kZ=23VCfU7NVXLV(DZMXd_JU=;3#3mX);Z98%59J`KZz=LsHva(Q
z{{ZoJDjaOLzYm_RL1BY7?2S3Rrnruj;d-ZpHOAhFtq-_NEPT0|Cdj;@%~k!CA=R1o
z*bA~u0~So@*)t8;fQgbqZ8(i^8V(l84IRo4ZaW2*2;R0^)a?`By=OaYWeNGNrVS-D
zvi939V1JQL2MmhNnj?tPA-Ty%3H?rYP$~4~JPzJr)O0ejej~7@s{z&_nVIQdF|7Xp
z$AfiZuea;oeikEHiOL(<VmA*EPyo?lo6|;mD=Vujt1JGDrvCthtR{?|pYt7^-9!D>
zc0n}EBG+k}kn3oRv>Z%$bb^@R`#D#J`jcHUPKmFpJ(PqF@>!IomO8NH(p@!hhRrK<
z=d^254Uj<-QVnz30TLaept+z#)uN@}VmJ&rqCas?ZG{=ib6_@`T^2gTaECRb51Os5
z<y*knXA_JXHC01{53=VE-ToDo)s@w@N9g<}A<5#D-uQ2W{Dtl6qj)FE>z5A@{{XfY
z9t9CosghG{<2{#5f4N(2F3ERc$ppY`N0NDNZz+WRDd4!w@P-(K){IdhVYhlLo4b2l
z2hy5p=A4iKb52_U1`}|c_Gcy&NG*E@(h6lc@U&!u+V*9tYfGD1WLqVM)*L$6C&@55
z?d5MU2vrdljkAc#$7FFZ*vS2^Sk%TLk;+yr5sGUZq^H521H29{(iSc``gvRGzw$Yp
zewL|r84?dX<-^qN=V-EfF!38e0b=z<A7zf)$Y{@}?ZJKvt8J0Nf(CeIq({^74WGI#
zIgit%$v#MivXBJHDUA*kxwS|O0EfIc6EQ0c&^%gG*vHb**>gF^)USvC0P+Qd9BG!D
z{{XnG+J7tJ;jjcpw*(Pn5QRavbZB=fHp@OHzPXggS0Tt2zNJt#Ao6qBNJ0p7*#RNM
zrjs1pG>v<$(WZOon}^HIk<lC&5<Qmg22(mHcjJIDqDf9*ZQGjN2J@oRnBkT+)RaRi
zH0K3X58eJaZjCcZ%?7->gc|2Mp2v#YAncLBI~>E+hK>){hs?x2C7ShZmaM-kFUrgE
zzxJ~Hp=fipLG-wT`<$b)$3OkF$Hyqb&3rSCBLHB$l+fppTZvvt&tqK#_E$6ku4@Wh
z2pEB2fvEogv^SVz6`)BA;x|vNn1Z3?x=*(YL9tkmX}hva{?&=k7t6m0Wh2!F?;9a8
zwYJHj&uR2hVbh>i&T`l!!fixM=z+wCnY1QE&kI06h&I(=G(d38qeY27>NdzS)dqB?
zcaD*!-<Z<j<ek*Q<84+uS}S!x#!14c#3T%p;LZ3B^18Hzi;RAe4fRyF5L8-A)I+*W
zF8X*u7@YqAwiY8W-pWK?Q%mp6u*<WRb#0g9k+Kvje3SbV3Ze8L!mIrpY{IqLdQQg=
zHI~g*t5t;PvvZMTS|OuaLZoIEASbLvmCK6o{neTq_Sni?MNe@2Cc6_GnCK3vxJD(G
z1S!OB_8TR9Ny5Z9jeBbJL(grA*eO(Mh{Glkwp>B-2q~sPsYoFDA<WyVq!Hz^k(wD@
z46mp_x-Z*?mgcumDqFg5n%&cQmAj{#HA}=E14CvLgIa2CXoj@Qb5L{P<!k6wwXTlg
z7PZbS8gON>x9B;l3%Pv4q0Z&>*=@11+i$jZ7d3&=-Br_F?75s=w^pEKWN@C^?_-o8
zBQ;MIVWrHbmuf!gyp%NF1=T^-IEg+>C4LWw{nfwz69B*_Z84Ik4bs`MT*9`2H?qcd
z3+iqwW(mnO9@{LgHe7)3HHnH0&4O*>lCinz;iq;dx&lIIkg1k~q$+VpsN{o3PR7cM
z1691!Fw8O2qH80NYr@dn&>@l|F6#+{lGkX_Hx3IJ(4NA`<c5yx3A%>u`L$Ezi<yS!
z;m%VF7Y>&=NHX0PB_g9($fw5vt<Db80>s5GFu@HIsyMGMEQo76r!|jd*)HI*O?L1X
zAEiBvrW+TW(}SH;t(b~YV?c(S`>SwYW^1UfzryPLF0aDsT|spO=A9gYoQm|=<Sl7H
z`Ji?fA!fhyw}rYykNZNgOkT>^WtQ!q1nj`K*8c$hDV&WBfiiSN;jpp2kdWtyk}_DY
z!o;bVt;p!3mF>)VFAb{LYb}D6MyR(8<c+f8540fbT~%V6G0dttY=N2<%?TNySmB0d
zw|B4AQ3k(NL^{7!K@P1$#rO{Dud;ApeOkhB!w!+0E9b)iI*k06E1gJ0wqWPPS@fH#
zTzOAmw_~Pfg8M9q@3!hMKvOe&ZF}3~P-}H<S6>h&OHALg!saIp+8eAw(m78|EH{4j
zAplpG1F2|q_R$UD0JLbGy{4!cabSO&{{Z-=6Y&H<1_Cblnp+u!*{SV{j+R)MxOkP}
z;x@ywbY(m_fbTM&iU=?eybx}&Vc>f3)4S&Gp)?{41%?#>TT`$gVlVE48Vg+Fn}-~u
zUKWPtfej==!KJ4zi9`H4HC>MHr&WYCr*F+94<t)li0nDi%NX6<!p+TMo4e)?l>Y!;
z0$qtqv=Ff|3(YJL2b#fbm@LaenS$Dc;mnM<TH%P<Iwvu$>4QDi1%PodscSk8d{#r#
zfSEy=UUs+pnnJ<Gdml?12FLzW_c%uqrbDAdLzr8Mf?^Z5xLA!i-?_1Mx^sg|9)W3{
zIG*M;rhcT?IE(gArRNY%b39lPop1jDgvorHMfFjQ4*kTT4ts~yPh|W-ig7Tp8_PN@
zYuKI;T-h?38DK)?Do^O<R%dQyO-6lH$v&rsL4u~Yd#b84-c?l@?<%T{_mw2i2P=zx
z5iNUxK4{YS3F4x8bA`JPH1<>nns|;Lgv#Byt&c^!`et}6t;7M>YYGnR<bD{DL0pe>
zeF`u7j%7D|>Zvj;jvnQY2J&-H!kO2J?Q9lgT-43%TeoE6hT2(Ggm7J5?e|z)yg=uN
z9K6D3$~t{2mXhYW^(-(SK#!$4`|aivo6F>}TRE8lICVz{-80krZNEpc;%BOG^FaA8
zLa_&wq33ljFg({-S%qt%{npq<+~IDyLmWhr^(K=K#OxO{KrTJyQ(Hd43UNvNAxVGS
zq4LXpbwaE<KFPSQYg~IKwYUZ>5oQI3iwt5?%B?+>4|RAE%HWH~?n4#5FR|btcU;<S
zfw1BXf~*T{uA6MEvzKOD8sqhBlEMNln1yUr;x83=iukL<ULx@qh`d0&K)gV_K)gV@
z!s`l$UsWNb@E1XW$C}M%vstXxdoHiS#?3RT#dC2C=X}$CVf&*vuFW}<0-Tl`fYuGu
zz!w5@OqK|Q;O2u4g-b*=&7AdBUR!V{U^;C#k20Hr_JG3S5q7x=ds^T+ikJLO8{_E-
zuWMWA6JF3N>;xvraCp#+keM#?K)W}6QlMr&tyhfYQM;BHa1AEGH{r2|%ZncC5u|2h
z6wo#0aQ^_bWz>}<f;_f6*NYR)OWPEuVvd<cL|dv;XS#7YQm{T)0&_={a;lopT=KA7
zS(52!Q7yW6qI7pg>G0`JgFa}%seg3e$vXavGUA8RB{Wr;%~{!Mn%ebE6-IYWKJc0w
zFhr)_3jY9TQ&l)D&&^}FwcpzsFNY4plZzmghWO=WLH__{j}4}i-!#y0?m)lU7w;Kw
zrf>Y1#clM27z~GD)pXo@1H7b&$zwB=76RdH))dq802A3grVj5C-pdK%k}+_}%kZ%=
z8)Jk(g!~RrTuGkm4yFvh?LNM!{A>rd($<!TAKj9|z{LXzjCDdyz1H&)>=W3+X{x7H
za2=w9hBduw5=kw!G^X-JHlF(!*F3fa**h0`RC#S=HvN_#!sFqVmW;~A<()xNG+V|4
z%hehnYBe2(7f4pZS7MminNfX^Tw~W|GUMchbqL8_?n9l{$#||%F&w$1qF&Q=Y&Ux@
zhkGlv!hDs1_FDTbeUu6IS-aU@eUx537Qg<eT-(V-<NQ=#*+HLW)P0p~Hibi81AP<W
z{wGpoKf~!yV0SC?Td_qxh;ub&7N4P)5cJ>y*Akf6ag9h%4HD6!<)|MkrxMz4?3+-N
z8TVN@w$URbLt1~hMUH4%m(7aiZ2th?n!^q<Nzbr^;GWHs*>W~dWY)!-Fbd~_>bao`
zJCh$|*FC^U?y*|%OgbA0u)xYhTU~*##cNDzQ^0UH?y968RlX28vP^Si5U}uG-<#b%
zq*$zxV=(%&S<>yH_<~8%SOBTSXi=L^TP9ah>Qh$C;Wd+G^30DlqAhlN1`{l(E?KB3
zAknJ0mC~|Bn*eYmmC$|HL%r2ZkVeQ>=2N>;MKiXym<J`)<<#UH47C29Yc-i!p}<o|
z)DwZop)+(&Ua6GlnH`gu;{;{qonw}P0vk-3YN!me@>Ij6fE=<@-8gp%!4SS%4j~tY
zqm0}zDHo~Pr#luJj_0pMO)VcyPJAp%<<6+YCepyj5VHxl6Rd1wSi&AYuIONHwVH}y
z!;35|OAm-dJ2E64j(1_S4>STN%^PH=R_q&Huqz1K-IZpm#dRHkT!vfbf(R)p%2(TM
zQvp!WKtyZ`omW<>hh5rj!xI)=KXv<8-}T1~jJu~d63%yZRveJW0s<wxwJ}ZAk$h6e
zgxEL1f8tXZ(B`$mB6C~5VE&k&!V}aiWia`mdxR5lDSS5Mw?&DF--o0Y(;*&iW{?lv
zHQ<<%J#3ih&9fhPT#c+4i+LxqKDfW4AhChhFrN*x7@uxY2$PjoP6zi{VUR7TSbrCQ
z10FO8ciD^Ry*5?ZHxy*7^lv1pb#}qE+Xn-9T7n7|ik0@+WszE^PSE1FkRenCTUqF-
zIrl1%zW)HqSM`YzqMAS+YZ9hInkUs^<1jiLZlZQ!12?q(6R<0S*3=AUnJvlME$jyK
zP1Yf3<TnaaU$Lde&Sk_{tX;>$xtMPDl}KnD)f5^7TFgG@y^~(%g3v6gd!_EaSt4o@
zOx;a)^i&tp)LO?HJlBg?_M+|W$I_cou`3H70~EgmXjMK_u(0c7@A8d>Qf}2X)9S2i
zDyM9L3m~!zAZ(S8Sp|?+Ktidxtd~lu)<FWgp?HdFlwGqaT?KR%ND;OQrmkKJU1(iF
zbp=Sv_5BYHBi;1f7r*_!N?v(fU<LmGj@*HeWgE6gyo3#GxAviji0Y!@3_|=#h?%-L
znUEzDi0LF+bW`-?h|t_GO^(9H4So><T35twrjmIBlsVL+%OqZBC76g`H>x)CCl53k
zHtwB<TpfKAL!C0P*y!AocqxwZ*;bO9h&o(n?tyk7^jx^jUOQKg)ogaJ6`t$FYU*mx
ztrkIag$Loo*YJ(^U&0S{{2KlZRB&zCe+uRNGneqrU&B2La2yrz%IGVhbQRFL3g|1L
zbS{O`gtHQLT{w#@MgF3;_Etq?Rz+&HTCG?9mTNWCx|dj8VRa96)E0YB3_QSfm&+it
zMTQ7~%dhr@g(e;(eU$$I(qRXi{<dFB6;>0Vc1&iCGwM?qFHGG%o_jZc5Wy26<28!h
zIH!0(^pxycz#|}aPRlKQQ8L{#DeeZ_Zfu-I#I9LaOn_{e@c7Miy24X0uo$zmRG51Y
z9&0kXis~z@uClty>no{sE~V7Ems09ktk!E!Wv8;!*;*EhAi7$*TDls#7`hs|8oC<1
zHm?b*!fNoEycVwnF9a_Mcx$0_mC{yDTCCi<%JC1f@qY@~{{V&jK=xMnhN3rt^>*Pq
zu{4{iX>+j8;bd4ml+v059biYg4sHl*;w1{Oc}9xuRWt+SyfQxN!|Wvr^C38Q3I3`E
zChHr!x=N}ARYag))2igdz4KDWlHTayGYq-oK%CcLZ%_Oxneg~vA2o+o8*sTJl;(qX
zauu*96vZ%`<eR>vJY=wj+Szvh09yrHE(vRkNbITKIPVR@k%;LEH;&G8MDL$*8K(T#
za^(wjUUQOlDtFGDk#WQ#Ou#^I2-4Y=&4}YBLb;VAVH0E<HX0^!$v3(<wDgdgd!k!n
z?ABSTTVSw(>I{&W0MSc^ZBO;th5kWdC~s2B@S_pG*5oho;L6B4I#u@NvcX*d(VBs_
zqz;=f!F4W$x?!rLGO)1OVq*JR;gdzz!;||^YphfQHiZ8GRR|w7fbTWEZiqV%Jl22L
z3r<Tgvmd3>6^8?Y1*8TOV00@)D!3XUK!nlO1Pd<(%fTs)a|>!sY;$4$Rc^ZE6uC*w
zUKHXw%Xd_Us0gt3NBZS$UK<yL-*vn!kGk;P?y)hzb6bbzztvq>fq1X>UNQDwH{p20
z*?6Pbc#U2oS6Egd2X|PEafs%X6(4ofeb-U<RAwetiy)dv$!|IEj6$W)Bpi=*)>m5U
zQM@WPHmk~e4q1f-<OSAG*>!#wiTGY1_Ff}bh}G8BcQmi3-F-gmA?~_=E2HwfKg#fm
z@SMCCFA3hu!hf>x-R!(KdoG82E|cuKPqN6YzoH6qe#|EQSjK--%}L2S7YI%fybq`J
zF)%SQ`h1ZUwS6ll`>ud~>P8V<*bY?!T1G&GBo<<~Qa_o=WEV(XBiVE}*>pGAcy0Dx
z6MdJ2&t>5=*?3I$UJE^!g4N))ctLnAUI1PQUI_PI7v*?=mC^ZKKgcvqfja|9vqTRs
z)Eh{{Z6Jx`azCN}!~iG|0RRF50Rsd91pxs7000000RRypF+mVfVIXmVk)g4{(eUx%
zF#p;B2mt{A0Y4#!gW9W$k%Nv1c0%^7R0l!Z(}ACDL`Q%k;4xPg(vZ+N{t#-o#tg_*
zaFx>7KGNl=aNIs6NTc{Yrf;;>dUrAOjhA4yFwgjEc7V)+&xF1#3N1k!(Qj=-s1P#0
z2wxU-yCsgn3K|&OV!G|<7R2^ua($pmr4yDIfc9!dz}VPyUWiEG7<kwSYt|FEL%G{l
zfm-TH>0K!FS4)=4OTM=)_466WN`$?7c$e1`o)K)ySBXS7ejzJL{IbZed>=p(Dm%wP
z_HhDDJb9OSk#tJ#hLM@TnYN{H<7Pc6c(C8J$zYW8GeaV*d&*v`obL}q;jFdG9LmQC
zu_$A^yajf&az@1@fPRPpwgjWDtb41MQ*}{x_KAgPuwsf|9WKz~anXYJB9~Cz1y>Ca
z(^FV0h~TVI;}wo#9THqPIjlq~KyKJDWF3l(UTx`1moKgQIF~P`;!GfXPG!sKa_V2A
z`f=;LSAS0Ck7N-#D^Y<lRwEd&Q*zdtPe{FON>66YrEp^L$b>l6z!4)K0IrtDIz~8r
zbi~Fu9s)79Ju19M3{u5u9r@HPjx&&NnL@~3kO4T93rfpfMRt|kYF5h85#k|?5HuMw
zMtG}ht;;-y?Nc2-${fT{1axNRsZ71%?ulh~>E$;a7&5i1<X~i(ZUHTDdHpNx4OC+%
zK?Gd6a^>{Cjpx6ueQsR2a^?E?m+88fE;^TwxdV*za|NYYN%=sG1wn7Opez?x3^ucg
zL>55muFsi3YN-gi2&)w>qQzm;jCUR7Z*O=d>Q_i}0!UkG!_iMH!GL@l2q1VqHs*+c
zx%Nv{o>LJdLJYa!2n&HwuY7z>>+NT}G%gm4JIj?CCCiOWiyG`)cm%mxN-&WE!Nttl
zFs=j^x;MDx91*DU4}@kz9Db-qaHi0E#bi+>O+l`K>2ZA<oOM+%rLyHaH!cjnP4vFK
z{Carl$J~a>#V(^zP*lw01x0u`JvGEJIm+Ug71^#->_@%aSdWWSel1-DNQ0DX!BsUX
zBpV}&H37JKH-wZ&X$FFu?=(;zERO3(9PVB*p-3`-ASp~HxQDycqUC^6yM;&)RwAWs
zQ7za*sgZb`u$gRoTbWhGzTuH-gB`NP?3BF>LJKDAaN1LeY-~Aj*!!RKOu-a(aT5B~
z+?$y?7i~nuQ7@qCWfxv%ZK!C>+&dw<j6yF#azVLy=3X9ww-W4^nR4agUM1#UCHI%O
zy%*>>jf3J^K<xrIEUWKR1}u(!10e4P1Ujh7Cd!EY&N-fLBLub!2!w^Vbi7COxUhMa
zL9o%OOyooz)elA{{ZvNV;$CKBLsugXDFAYz$jO^$iJfHuvEm&p;4iT-uj|nQfYmvI
z-r7YoD#D7e&r9go+_YYJ5$Zew=Zj}HjT~uLjwL3gxbZNN!ZT=8xB@Wj4Ms3%KbmFC
z0gEOUh8gHtGBH98;miyWvs02+JWB~{71=Q_2~FZ;>!xt1Z{l{#)F-(+$5Wa7$2C*D
zS<L3TZ=;TYab*I6Qk(f<mBovhqVV5$rjc~vgpD{hO~ZK;c>83ScK2?{h9$U1WDe4X
z?X_zh2fF4W1g3n$&}Xcj(Zxv4DIXIjAqBTB8qZ+E)gF^4iJEu^XRf+>y10S?#O;`0
z$qmseiI^zYpui7&lYh^Y+Y4mf5yk%iWZotwp;$o@#(2fC`=%c=UU~gEN7BNoRKu-n
z?MftW4*6FaM4OChnGuW@5n|1-I@H9w7qDDs)vdooJL&+MrSqT~`XztF{wMKpo%7yF
z`3<HVzt1MTyVaj$ZP0pOZ%xHUJgk^uvt|!@YzDl<qbl?_GpzZ$hOlby1aNuogk4me
zGQ|fgaFQ9T8N}liU3ZESw*o<T7Ylop!_ateMj3t(r)_b;?JAM(BrV;;A&0e`iE`!R
zFx9EgV1vV;<qAS?@R>78=0>H>ON!vWvNsh<Xqm!pz`d`2&_vfH&zXj&nFkE7&_=l9
zG9Ytz2gxjPGu%S3OAs;Gg4z})JWP`8l$^yDTr!hHZ7ya?d1l9mjTt?fB^{4><R376
zp5zCU?s81p)A_<@J%)^-&%J~LKT>|8)QhmM%mV<w-pEA<`fQpd*vSUWdNJN#L6R|>
zF*h)`m=fiF^t9KV^}QO$I9r49f3EOS{{VNq0oH99NhQtA?}MVE!{rapQ)XD&>vop8
z9rDA`OR+a(*wEM&DpH#nOpHB_hA;s@?3}}rE31s9VPwHHuBtV?@IdROcdzY1P!q>K
zVB%C~+GSMT-La&#pd0n#8s<)XV+PTD4-$!~oCGgPUw%k$aQTlBwN)hSC5we8KMIAg
zlZ8uQL8z;;3Pl8N2T5{M7~z|-)JjNk49aE2-0k%^XXJjTx}&ibT*Ti)Ff|D4qTxmN
z)YHdo{{S-9U+d%enNNSE!8Un=ae)W9QC?-*rY*Jcesw+`P9w$O!==lPvmBAErX^8a
z2QDBcZB#X0mEa%dEW=73z~)VnwO&&sQ$c`A-6PLSg4c5wkSg~X*#~4){JtU=LFdrD
zdBh%CSQfy#7Dv)ng6l^$0Oo4fHSH-a7<9RDfm5q4<)zSLcY1+8Xk`sww`9JHWp<2b
za*>ByG&U*<3AOV8;#%DBWw|3zFW;~|kCtn_LT}!=P^eYYq87x&=4^u0u*(-Nov1*`
zF_)q=gvcn4c=k-(UOArxYrFcE^fXHQ>0K+WN|gg8j^h&-PbAo1aM$r1e;dSPE3;$y
ziT?mpxRw^Y5BVyrdzO4M-b24fB&;tIKPk&e2n4*zYP%~v4NLMKBl&LzTezL!DWY3(
z4D;6C67Rc{gePj3ao^Oc+J^;_)<Wn8jp9{kd?j@l32p|C=A5M*xmE`R6se+`f@5Gt
zA)ssZl;T`|6{;3Yl}#BRG55laj`Gv)1N21F)pLo=xWGLXRT6<xu5-*6olwc);cRO2
z8`0ue*LkB1wPc6vY5J7CgZjQ^1Ky?gio@P7HGho`253DB(Ew^&lr4rNSR)`wnp=o^
zMyo_NU$%n5+T`Y#jT#cIgl45mguaKNC1#~DTHDP`ec6(wvDjTcSlzF3702*C<e*XA
zmL6lNj5mQ;AxssdEQ2`3l=^RA9m<xx6x>%&I9C$1C!2tVvCW7jb|W5Eif?l$?WJsH
zxgfrk)X_jsPcIk_kgkY5FkJC~016$W)LmVkfnzT`6V4s;?H8UKT@km&n)EiEhCt^T
z2uZxP+6y*IFtEDsEOeCREjKH(7_5c7-ta^v9_&u_Efv`<!IT)p7YJ>UgJm(Xn#yBe
zGMH&J)bf456SSv(5N~4b5Fy^7-gAjxQ;163sl`;RbgxF)QMeT9g;qB%zY%7R<bJS9
z0d9H9Qd|0+F!U3{a;CN)1j-my%!4J0*P~EsSJ^%Zul$CqQC?$NOwB_AhR!20<&KO+
z_e)_N{8F&4fKJ@WwFA&?c(`z64UHtR&R}6NvW&od#t<d?gW79=MH`nd61{q;)yZo3
z9iSR+Y-@-dG+I?$TwuSFUOG3%VS>JjRJt7e!w98?^!}N#%>}bEi|V{fNXx{!j~V5}
zVJPBX4|q^(s`zCoEZF2$Djbx@0#NNRz?xcv#C{PkB(n?~5Nk5#ux(T5OPSrjC-{m+
zRr0Q#BOF1_XB`hu6J2=pX_l1DFng~OQ9N)o%2x9e4|)L(9j3Gr#gfr?3`+36lR;>Y
zjWqa)<=uQjwYN|Ob5M?3MD5VaRr4IGjk{R3E~_B;{SQtF+m<`kOqz{v8(OJvUk#@m
z;+%<vjhCSG_lZ80A}Gt$l@lcv%A(;>q?3sIO7oeAn9SPn{Uc@~`Ry!^W*^!Yc9}Mt
z@f9btD4JMCzGssQ$^q~faYMC18cH&Y4GWc7j10;)jr$@N8-)sDD#?Zl=0M!cJjB|&
z{`|%AFYWOz`Yzz(Z1TkCaJXxw^cb^U<{}N^?}QF#v-l;C=%?lp8zc-z#nWXsl>MRP
zC&-U5!MS%XI2&9{>h=>qdU>dsTmxUQmasU+yq*E4(?Su=W<7+pa_!5`No6mimIMQn
zI1B}BwE_^2CT7sEu`#F+SMMqJeO{sH%cfgm_8_&EM&QX?7Hw%E&$ew?k&Em^u_A@t
z9Wyjz@~FDStMw>*_E!-ZxQ-ExlI#xm(=qmf9#~aV#&3}?0YV<V=6MKe9?{y%lyM(2
zlgv;;<@<;)ui9TSiSW1euQ9C*>Iz!JGHkL@$%bD7VUsZda)Ftw2<*~Vy9SzLcLdkW
zd8SY=%zozz?f(E6jdgEt_b@t?kY*g}15g+fo`;TtnD2HCOxG7`_9kbebNP!AFN5&~
z2l=cYF{rYDg#5yc%y#`)9wOc_6wGb8FetExpc$3gR+6}9<wmw(<|xvO`pX$hn4Y-!
zdT`R<5ks$NWCT~VN-<jYoV=+yA)fH)H+u!nT5!!o#T$<rICh7j#-Zh;dc+rKerUTg
zmzIv#fjg7P(giS5$4AprL<72LCH8PlLPC<0u6SeQhT6zy6EhL8n3XmM3m-Q=I(spU
zg{9dm)4+8Ymi3|(?-M>m!>ls+XLl3KW8E$p-%%lh<_a|QyAU5pEy2&X`3n1e^R)1o
z@q5P%P{@S3-A^0oJRhX5hthZtq&q*PX)aK~s++TMqWzEg4c^u!9fFIxSGRcMl}x$u
z9NH*e^8+Plg>@Kvo+Y6jy4i7KT8w$aj7VH+V?0A{*pzVmrh6yEq;Q)a@uUxwxo}0O
zxD;R9an{vdkt#6xLKTJ8lRBk)C4vKpw3jcE9ilExbY-bK0o!pY5MP#~zUAkLdN((4
zcPdb@*AtZL7>yUE?)*TwyPh8rmEbw1AnA9h=~U5&F~IE{zY_hB8!IriouJ8wQ&VE_
zrADo)E5)Xlz^3p1MM-YK)7k<(wbK(It7~7F7Xeh+=H<#Ed>l--hw(17hBBr+8GJ|K
zFOpR(m=x{}VVU;ChMr=)p@s?8n5kWtDpNmXW8o{{9%nMk?htN4M*%-b2ifLvhwgoU
zFfn#AQL$DFdH1Nvq^vKC;xsDRd`2~Q#-eHY`j6cg$zZsz0G=((+W-n*2}boD2Aq))
z{HVX)IRT<xE)ofWcI}9xc3ekk=gSx!7+H?;_{S-kBGwwYM*jZ*9-Xx`x)P@4UeL((
zr?MV4FPUifE`wugJQk{~;vd@`a7zFfTwkCuWfFBOd&I1w<r~M`-w!htDeT<1_nrR$
z4|sIc_q1lFit{RqJl+);=x*n^FigkE3D{XXF;ErHViFn1a2<E1H_Ur5x|?+7UTzgb
zGg+>%_Xq~#4~AjKGs<>3CbOjO(=PDt7{-j-!#2Jn1uU8wESQ?3O`Z9uxq7pyW=obF
zkC^-&wQk9jX2WvJy2!Khm{TdxB~1V|XEZ-}#501&RZx1fY~`uAfZ|;h&{rUf@0+cd
zdrmB&-K#CfTeTxhd$_X9?th5y_<*S^3Y<myhO=jMg8t!>!3HaUgx6L+X93Gy{UnHe
zu^h;!b1=afM!v>?+kApoAA8Y)Du_<)gd@n)kplJm3_TmFGakv4SaBb0Zp|ks-@K%?
zVp%zg4HcYrhem7_J@Eln8AL@?zSpscb_<G~6Uj{Utay|hPR-$D>~2W3y&8i)>7263
zk^Ufj&uE>={t}?9URZ^DA^^*ocFTh>sWaHV=6NRG%&Xqx=L~y&rbD?a!iL!H3*srt
zEGk;oQF9gghkAu`xoxJIc5*Cpxv+1uRXh{2wWl$ym@rxKl=B`T2KCH1P`qH3Y<)xw
z7YJV1e0~J9S`uBp+(&RgTGsrqTB(0QESL>UjkiB&;g6PU+WSm~n!`|R+f$G+5?qOI
z{N^gr4Jq`7-_;VpdrsS!${}?GAt6*Er7Af^dwBHo5I0dLVbL$JJaH~VfT=`BxP%sV
zrQLS{7&DU*7$U^!yN94f*VQ#0EoIm9nih-y08+iEy|wQsI4zlV?-)4qhg=Je?^1xr
zGCwiEII|HHxEzm4pD`hQslbX(osAO<;Yj4^UCGWLxcMiu4h<l&$rqE81;i<y3g%<k
zp7X=^9?`Ajn(iT0J34}e=`$=19pPui%NoX@#hB}h(woMANH3dcGUHO4>N}Q1qFYN&
zNAm*f6}I`5NFG`k@X27#J6J?sIaJBMfgt4bS5GjrD513H$pK=a@hcM&rlY9}2$G#=
zic%;FykNOSl3GjlnU2s7R*u^sHu%AItSa#+?E(EKfcl2vCD>yD2oM0S^1Nm~QGX=f
zmC^EJ4=uvR>|%pd!gBzaM7CvPx#`~2Y^|vad?ze3J2LrU=oU^~MXL@5u33t*6l>l+
ziZT6*g>ETA*m#t^10mVetqaE3)dk6KnP&p=qXfwSaAN4(33@&rgp?h{&6G3Z(tuf%
z9_7T>ZTp1hr^NPU;8ZH)ltA0{CS26J1x*;I&Bj^p_dVvfsjno~O;0hQji8mPFh-oW
zF&s@F-yI7z1b@)~0J8E9yiXT|rYM2%Y7<5+)f<ik%DNwEkao@SD1SDFOo_)vMNE~r
zo59=OT)T}Ls`vMXl!1KM{7R`+D(5`{?O__`9c}hhLmSJ^>_IAOgo!wj1hXQ=I^CiT
zn(K|uVPdfijD<tc3-NL84}VJ-tIPK@wOFt6G2BmgptodJ%RXD5kWl_l52nz&gHqj}
zd&+*!Tt9kbKYZk|B=WcDwa;$VHgp8qAS<;yiltRvUVS`|KawO7#cwNBKNCHyp3QiV
zljND<xCSY4XD~QtNu;XUR$gUK(rZ~KoQ}}d{bE^W5*zrV@fLbnR3QRnk7s#y7$ZOc
zvx!*qQus)w2Aa(<4q&UI<tkfC<XKCnn*~1*7q!vl0EP-TIZwqeFmoz_k!p#}#|`HJ
z-TBnJ36V*4f?Nio_&woR5C+$SEEkW6cqRpz4=v2CWm@Yo&S0%Mu4cNRB|zKnBZxGj
zyXehPeO%b`hP_oDwbsA$=l=i-{{X_XE9Oz{E4g#s@X9%|Q_e|G5b=)_+H;BRhI5QF
zY#i?ly~Q+)utf&$p7Sg+e5CM@`mwOpNG1_G!d+`$c|-e=Ov0U|e}Mk)+pUi7YB9dX
z-Xd~Pj}&nztyGvkC7Iynv-1FpIph14VfmspCC?<-<oSN7zK*Jp=mLP&&T226SAaX}
zHh8oiFNyLLnnPqnqi>UliR$0U7FH**&|ZxK*L(ruBNYeG${jIwsc3K#ryrJOuB$LO
z4Q~v$X=-P5w`q8{Xb_d8Gt}~|Cagu=5ZUoRtxBld+F$(2S3&JRw4N{2@&2ce^*_p=
z;7{-;_>=sZ{#^e6E@f*`VkH9Z6W*Tm_ho=h&1!qI-ktlNS>~>fzMYy&J0Z#?{3i--
zWXQhq$7$~@aF2PwzGcN0FadKK99+?Nvc_QI=K`_<Wh)F3>3mQCq|9?UXKHbYqIj0$
z`peA_F)ds~yTs1QNo;bL!Vbbm{Dfjr<;={%bis86Rlr9z0?craXyIlD#qV!9m`k6R
zJ<Jmgt>5~~D2(6_9%9)QwEM9ySaPcpj#_tb;vb}IATq+|6&uNB?d6$*8MQF7wDK=`
z_=p#HKKq}j!s_D8LtV#EXEi%UImJ}C?(_6xsZ1Ccj+W6^cp;E~(qBEcT2$gdM6YV6
zwq{kPj-%j;isn?78N{Nb4c$v#*HfDcf{@@{y_`X87#ZymuUdg=UN+6E&Vm(pK38((
zfaVvSLhhyt>m)v^Wrr~gMX`J0WDtHzluFinjOJ8Wx9(Sz_dBq^Y?lrDO6xlZ8FLyb
z)*6XE<nM%RWEne?c8Iqvr3{%Ry=S(%aVzFJ@i_4{<c<pBYW6x8(PiP{?K=&AuWQ^O
z1+^4A+^B3(ZKV%!Q<Ke0#z5g`FKN0oysOL1Jc<T7VKcxrB^?nOs>p5%x~8j2mXy@V
z83Xj@S&+W+x~Q0O1=q?{Zc172;q4Pqm0S!PfP2Wa)3|OF2w}3bhj0Q34+|?V+@S)C
zTHk!-G1b?d6P7Ag5rTu}4=9=~wi3z%`MHc#&<;pR>B{BYwOr8~z(sBffw!1M0?|HV
z`mOTJ#;minxKIa%WMxj*iN$Spmf7smV1-)VEd@2)9Yn$hi;D$3+K$KHEb9Bj4L<VC
zuvQ77H`;3}9G#7g03?E-B-?kqih(*}(<V>oW#u4fc|?|ss_{A4acC(tG+q`;8qN#=
z2b@P@qa{kR4bmUl6hQE929R09P$G+Fw=VuFq_ZByRfB?~k#c#S@3gNc-1vRVcz$4|
z_?{_l%=0DqCm>4g^(&KIqg-T*>2+KT4Hk)_pcLq;+^^`iywzCW%v^-tmV2Lznv1Xv
z<%hID-V|2frxP1QtWCT8)Ww$ohwXXh5~h?5jU{_#aj*-0x$u`i#j5&z_k;ssJ=jmO
z3Bob=M|E$rRzn|wJ6-Bk+O_51ywnxiDiu^Kem$lGU?){7$G|)?yLUw#Ov7u4_%Y`e
zA&nZI+|PzRWS3)9OmLt^X5a2%HM;i_riW&suZ-C^QYg7bdro}0nWJHJ5em|IpHiRV
zTjsAF2!rI9hWCsnBwy0Piib@Yv$i8botfnQ#1*lF?U)9%Dk|@D``E1pKJi>^*!bzV
z=VpRf^cY#?M(!GjV{1`DZb!DiF#&uNH&Mj3Z2{MtfGvHe61ph7;#YE0u>-cH5~tZS
zdk?&~y8Y$$54_|QUtCzRhusqHoASB%_Jl7=;~z0xbg^~fCSCIuz;glpaWD|Ddz*?B
zd&h_Nma>f|cf`1DoNM5~cZ9eb7&yjeUz?++z7|Z~(g5FQ0CNk7p^0sdw?uVG!OOXP
z7pH~p4q*-fjnuM~++1-snF{7-4gO|n34~g(Vl>^&Xb?@!9hA40%E<Hyf{eZ4yFcJl
zGM{voOui6S(qh*Dngy&7G$2`QzMqRJ+IS&gIEJAC)X`qaZtNGk2x6M%uZIx|-h*=R
zZX{HHY(IZa!ZcSi@l5^>n%>Y=d3?{`QDUxi2|?XlYBA0!Tbf@o&RNTIV;x0m@?b_R
zcQq6;K39pDUE|S}nNT}F623hZDpdVv>pvDf3l|SFU#RfQT7j%~xldpKNp2W94Z|nG
z?&S@&`?!@5wsV6?D5+gDzFA_q>TmHd!l!EP0A&V^tev#Ne_AV|hM$A3=6T@5!<MnO
zYJH+c*g5RC#IGvNU2DrSkzfPKD^iqu;?UtRCS^C62LjFcsi^+|8&Nd1Ka^$_UxHnP
zx89rCE&=gUyDBa3_?Po$#iwNb2-LX@rb4$K036d1iR~uGpHjUK@FjW~4rF67UeJq<
z{m4Gvlq}{H5&M)L+4?S*K(#w?#Z_1qHSY<g;iY0v2er&WSQYOpF(qddl)M&YaGcYI
zTTcm6rF2fe9*15hzueBbnmyiy7eMXpfgjP#sX*aX`%7JFgP|x`He6<5o-D`aJjtW#
zx?s=js5BxX99K}`oy)*Ru~S!Zn2DnqnLCI+HTxxo<`;7d4Y*}{CTZZ>SB5RLL+;VM
zc$6GD<nnF>7=w7(<~0^l{fTx9R9kVZ2HD(=G7ZX-<N!lg;WJB#OWj$X34h^hU(Bt>
z0R?9$S0yX@LHI>PU`zHxK9m7{v?Fx(J4C~TgQJk2*1s~zTzHtevhA3a+FL+WXb(0$
z)H)O;K51O>D-Dz=t9?w=pR{*My%UIQbubU)>6lY<aHFMGz8I{TB%`81iESDF)c*iZ
z0?Ib}N$u9CXMH)9nXhU3aLu;|&^P!s=a2a;H3^E~%E)aD@&3nxgl1Ics$9*GJYVw4
zL7jQ>{{WCUQ(#vA0LXfhmM@;ryRfUD1dVujjdd28YS~iix5Qg|r_*yJ5d(hT%(}r@
z*P!DP*A-bsnz*WwS62o&3=#qx`0*bn45_=t%7SFUP^+SOfCF8S`UUilb|^zca7;7>
zti-d~rpZHTN5t=BJW>&boU%q3@x_w=0IPu8edD<UNQ?77-kd}8K$-mG%6?@($t;%c
zv%e>HLi(@Vum0l~oG%~Y8UDO>KHiX8pO!jHqj`M`&*{~FH1dAunfEPMZ@KUN&K0^P
zPxQ%Q9|YUi1~5!{>&p1Pv&HqF+)pRcd6+xTsr>}+pUgWrf82t9FGqi|FX4Xj{Uipj
z63@@jH<&(oI=Oj3;JkkF+W`kFsefyh1726Kr?cy52om525#BN0Xx+r_MHypnWIbLa
z%f%Q;O}v-_<Mm=@8ca>WYIyNaiGeDm{<?|hF=mBTe|0$kgKE4O0AKmM{K0^u55xsZ
z_lcS*mJUcW8nhn~Y~5b)APBVz9HajLWo}X9prdR;Tpt9*z=v}UOIGFx)!Qy<A2a=F
z{)E5z=le3oKFOQ+AxsYT0^vo<{V`>*GKj+(-=ZgAmpLcILCAg}GQ%Tqm4C5?_gD6o
z@9CER00b$o!8@z*Kjxt8_T}&Prz%3UyNedz(=0O@{{Vt-MS6Zo{mi)ja?XEn>Dhnw
zO7E38rNl@<i)1WQ^A{MlFSU-%!v!XnqH&CYqkB{Ud?MiY1O+e5Lv_Ak^{Lad{-cBj
zo_NQ>7=@t&n=`-KD^QgeyH4hTej_vTdx*t*8=O_&k1?jw){5;g{?g*(R@eB{q)Wb?
zrR$Ik96Ud)><+ZKX^9+~h@eGP#+6^ZxKeB79@Ic=E6e6_<4djFslYX;#ERi8ID))h
z&wmqw=I&#-3U<t)qT9}+pD9t(2M}2(deo^<I$S*lT)Buzw^uOgbsnBi{$m^I?)ZZr
zK-9jAp3^Y^az&3rfP$mF(X~^Fq-PKJ#32&^-w`Sd`w`&2$ArlxTA)7r#O`kH&ON0b
zIPBB~UQ0mujuEAkUh4+!A#xgyt(TT35@KFuC3kgkAh@^?`lONQ4FKi<INS3I`xb|B
zXs36WP!7S-+7#PN!C<)<%vg&UR%E#S=m=hOQEP%&?)X7eqr75SfQsl>1@u>;>OB$)
z47qXXt@M2t2NLD<9=KsS`dq(P9WFg;SE9_=Hvn>tc#4ZSm0BtZ3sbUWb0#^4^)ode
z2uTJkDT-Z_R|?(7xh+u#KvB_hgI>Xuq03R`D6r!IyDjXA!aN_O<5c#QQ9ri>f0$ie
zqx6klgA-c?NttrzniFThAIxEIU$R?NyAt%lk^UqV(iFm6Nm9Bi)FLabN{>aSqPkQ%
zodz%bfPyu|W7V8XuR|@?TlW6|V1XL^%fIeQB|BmgZ+n&IQ@p2%SG2{%{{W4HpnDSH
z7>k|&%nX7fUI+=J^9+>klY#x)xkYveU;Y#Q(mGgmaSb5x;L3V@C&UXfx#cd|J)sml
z3_v4~@hKKY#<)+&VUA_&fzrKHN|h>9u9ecd)T7gQgXlUwvA%QGy&Ou9s7NIeyhD^(
z-<evs&SB5+sbx^Bs20!I%X}(tCGjxqe|_ih_x*?S6L!8=G5-KB@js;gB|Jsi`XOc>
zMpz3zW2#aTrxuCWlZFN{gbr#me1bc<;Ey2;E^wCM)Xjkq=uXr#?e#pLsLfXPY6~r%
z(8}bFf`W4Nx49qgHC$^CBN<Y!QD(m1_cb3fg&(%YynPJ*y9WM3rHnE|yP)12c#xbq
zW7;#4{uxx&ZVERy`>2lo#H0Dqg8u-Pi1PZLCR$VmBPvun5ivo1I+ZF^sZl2%_;IeE
zJzr>K&;zA3cIS=%02AgWao3KQFRO{lb`$rNEU2XKFE6k&{{S-XaC>?*L4OI77G>QC
z3r`~x<@~WJD87Vc0(mBy{fLa^bd!Up&^=cR5QI9A{{YF9cAf{+{{SLg{2eX+VjuI9
z{FD=aPJfMu{A^dGh3D+g@_JjhFZxzL_1Il~kLFR+?fk%N@_#c~`5+klnQ=$vVG>-p
zZfz?)P8nH*=3ZjYKqn+aY0tBxna@XY>Bm=5KV(YgI~aXQL93je-i;IV=286qgcxIE
zGYS(;K~MkM01N{G00IC50000G3uzPdi#?>8OLz$1>(e$?ZKfWjqv=2mx8N9`z3q!-
zhR>Uxlth^>dsGCzU*JdgqDv+18J<EiGFp3WKMT>j^&yf72ub(C_=x`ir8MDf-{11;
zP`J&6^$Va?$@j~eAJ{;0N?eQ*LI>^sr>tZ^W%m+sHp`khgx_SKI+dc%hU8KW`xe!c
zf^7r_+IxFt2oI^^Mu-A+zLBF&KPmtNJYr!>`vL(H?Je6(nYP<?{MjKc$vv9wy_aim
z_zVgt4U$;5ubZmO(m)9PA@8?<;HjHpWl0%(2iQe`rBst|)r}`bzRRTzeWJ9?zTquo
z1gJvTO*m4^0{;Ne-GA4$^kQ!#JT7HYMAn8af|3s=)}9C{Y_M2ih^zkq2O2j|P&$C_
zBUSSh?*<c#Ls$O*C}pUfTUtvYOk{Rx9kw-l;#>o~w*{X_QC7c%V6n?AMnOc*`=zBb
zE5H2MOb{if-rCaIe!t{pySFduLF?%YsgBQXdIrBtrpGqR?Yc<9QJ?YnfJu_|oKfxt
zlC%E$AxY*$4ZYIV%}iPF%19$DZ<FRr222n8Nl3fdyu#hh{{W?);s7<C<Zd^9<kuY^
zDR3qr4dPFFLJZNq{{TYw<n_~j;mkE)fqF#?>nDRPTi@uh>TDcx`z9M?Jw;D_^GUNw
zvlXx)5a)XlZ~h|T0=NGE0Iaul>x*OsXj^X5CL~-ichM(yi=hXxp`2p<2~AYXHZ&^=
zDqBfTOqp6hSl{%30MBjMCZ+cGBGV{Y1++2AYYRotpku=?d1bs(q6_r@0I>UMaU$=`
zn<u8&{rcHb7-Sba+g|VhSFUJAg-iiJYgI=XeQxaue{9~!Gz5z`Ty~?LE5MF>-GI-p
zsDF0W7<W8}X-4D@ZT{4wOIe_TR=e-y_9B$|QPlXe>vGWs*V-vHS~-GbCr7#>SN8Z~
zd12aMwIq#xOd&ZIGXy2~$1qe!;s+x)7ykh3X+L{lkdLfdf%8Veryq0#lJ&T@3E-1&
zx>+{i00L@aFUhQ1VmkwML^UL(L0gQ0!MUWYs8a$OlO&sOy!(Ff0o&6?%*>w7!B$+_
z$uBTJ;iXx~hl;w>bTG?+W2t6keP84({{Tn;X|PhEV%VgFMgIV5Ih}+4fe^RL4V5Mx
zey6mU0w~@62U#_gw<b#=&?tSD*ir!Crx>?l!E!!Jr1FU!MB)d6W}}I_gI@mt<pD7-
z#AIuXmO}d!nd>^AeJB^5oq!D<JvWWo+xuPli*Li}RFRNLgm5wlI@yG^{9l~uY;@%7
z5po0PPq$3<-iL?JIzP;?$BHkc+s2if7F^6{r8e}Zu_nOX02`Fi?_Y|5T6CCTxbSbH
zxp8PWYJeL^-`AzYo}S+vUu;AGXfb{UjbC*0AB$TqjmmqR%S;8y1a*}uC{q3eC~9!P
z;qeE(g*4AA_kupR(#25yv{gq0uhGhW_r11~M$P#X5Y+Fh{Vg>Gfo~_-JZZnZ#b0(M
zwSgqaV|$r@sXKr2PD;fkLtdjLc?nL29s1%<*&7mA6O!dYglY4yW}@YgKqLSRfL=+Z
zhR<QXXB%Ym-MrMqSf4}#S4+>DwSNIxqty@+Kp+4KAYGDR<W-f<fhB!c!YlrJOXQFk
zfN`(Kgb-j>C<>w&X#kJ`CufZ~86Q+p$YuWkw&~Nt^L_T}Kr9_@gV2N4l^xWC*#QKS
z2(zrqLvBNOQ3rFNQMKj?GF0DW?);MNL_*Nc-sd48BgqufM83#@xipSWE)XLF0Q$2t
zLN7&2?c|q#_xb+-8~W~sd*i-&^pQ>Z;ByxG_$ur>*?w=w|HJ?!5dZ@K0s;d80RaI3
z0{{R30003I5FsEjFhD_3VR2A_k+J{U00;pC0RcY{G^M=g@pw}D@Q7-1gFnG9W%_yH
zn;8f^FHMV!FFHQQ*-OUxZfSj6@Y0k_cwRMmPH<?1@ZV+ndEiKd8WI}9AqUQQehZ6Z
zfAFukqhn<#-cu>xI$s0g(Fw|86WXQq6PrUrLPJ@ECpbbAn&C<S@IVj0Qur^sJu~A*
z$~IBFCR`iv-vaV7c;}hD4~#SDmDpdQVE+INPow1x*JPfm4G@D^NQ57T3&Sw*+4IA3
zeF#B26!tHW%laSKM3c~$Vjp7v0Aama2`UW@wu#_eA1YK*jo}n*?4Ecf#uJnwJn)%}
zV;<FXXm%T?DpHBT0ev(%uF<B@Lr{?I2{@zXAo)TQ2D1oGaGahfxYG0JY(()sN{2C%
zy|MRWOTQ(eZh{(OHfUR|{)<(I_B+w3lrAuwDM~k##>!qar5_kV3q6e!#YX(E3uQLw
zO2FN*_vEF@a7?-#^p>dS7}c=mm_qSF5M{$e@h=@Iad}@w_Ax3Vr_tMY)lH|N?SK?f
zB*ZLieCbLzgi*5kZ;Rs&g~s^ip#d%a2JeRXTZ0;*X~75)^TNr9HNp~cUK@%pB|G$0
zL2W(@w0&NLAqK?wQ9024FkKXU=}KJ}2xS}BG(2d8%pklI7mM?>822~yf9wAM;A(3(
zlhL>E*p&|CCQU4eWroGxE-n{d7(t>FFrGEW;dyVP1TAQ4pHD``bjOCVkL-U@Kk5CC
z>^t1q7}ZoJDuYp**!@tw@Eb)NjA(t8`hR2l;dD<8#U2(I`r?XFbDW<DLUBX$YMT`I
z9`NhvA+H$ThOxgzmR<)qq!!W|8f3wZry2G-C4{1BGxv?s+aL9W^j|^pn3!BCbA9Nb
z*0{!mr%5kix`{gxk&%&+i&%s$jqqmdX*7mGiVQV#F1#<29v>CKPYmMy1W-@m{Dt`v
z`a?%({%7cfl^t+yY-PCY+oHlBz)tu!p#`iqUI$>wNBA;{AB<zX*I9h$Hctfew0!gH
zN-(Tx7#cij(te$I^eDkUM(7fRL_|t3bbMN7;QKls=#{$9;6WHmUMJQbR$rkxHRv}G
z$BVoesv#PJDs|Y+F(MF{J<OXUfe?$K_r{G84#vhU3GjMC+ratX4KKaiZp>Zdu?>?I
zY5EteL3Qv{Q?Y1Ll%=%B`N1Qw<t{E)dl`HmX2v^$mks(MdQ*fa8RPFe#Ib|LLjM5l
zKkxw~_#U{a#?^;{%4czd4WY5Iy$->ogDBYci$d9ECx@|PWLUYR@TKAqgdqvy8!1Xs
zl)g2tI>dz~6e@Z`ID~a*tt8^bBO;7|@cI`=z&h+}@MYOX#q>NhG7W5H(B9dmDT?75
zA7beGKCsdtkrq8Oad}P<gk=a!8uN4(FoFmrVFf~vLUqEv3AxzX&@xR3Y()l%4(ReQ
zli<#cN-@JsXlPyVqzR!KK<1q==%R*Yid<Y)@k&zB(?a3CB`Hgrh~AKs+9cD>E*H(v
zO@xJuD78S6-4jv}<|<UO99T<&?1cgrhJl+Qje=9)`w*B)3}ilVfju7!8;la8i57`^
z7O@G2EE*;g8)toFPMwE9M!zweWrbWd4K7nqAvH(Im7p_iwmq$mxgB0+ENmn}eqfk`
zi<@Ko7M3Hr8Z9Gb2RNMw?k<U=6N`m~@rKKbm$9L=d@j;rB(d0gffft8A`qA?pwaa1
zG!1Uy9kI3GKpbTHP^Ohwv!=yXgY0yaQ{WOhhr~iP37-ll;apA(O@tx}7qr`oLLir8
zWDsON2_}LT#=*%2Oh{c(>MbHp5lT^PoLKrgli-l5l0_S2s#-NpkX;L|6fOz8A7v@n
z+2D6V0hyvmHaghZyM#aKEHMyEU$M@8i!3AH)`W!@j+cb6XvYSF3l(}KZi;9#q8t2U
zB(lGdgSF6(ACU$=8->DUolfIUS}Vqe%VSA&QLBnlk9gBQ#+d_KAqC#Zrtys}jgFIO
zO_mv5iZ@TNcoU_i6jm-hiBFk#gdl;hop?^e-3^e-qd;hn&5|Kll}p<utXdjELqb?A
zCCcKDtj@|F+ro1t!jz>cN_`N2ObJk%T@Qe}Kc<2<TPaJxbUqPw6;g~@dlPT6l%?l$
zOTrGK;Fz8Q7h+hHZO#3Ss=~E~tuZYRIE8@~jCe!hnBFxoLISp&VF*GHgeE5lLJ))}
zC_->3vnDBc(v-X@aZ1FrRe}<W5YXHBR1#7@fxl>53U`8tJEDwTWpv*NLJ|uNXweCd
zI3@GXL1kqOPg20;%xYvZNV^BEM()JdvcjY=>?E4l(<rQa>~*9hTrCf*CLaZgXjpw>
ziLp#m(VkVwVLe2;Wx+uaf$cM2V)SNW7>=VB;$o7fT5M99sOTt}RN#GKp$mKtJ1yu<
ziOr%Agdqq*5QHHJLKB21fpS<+S3o-K-b*Rp1T`N}`U`QR>?LS|2EPiFqeW{5^3dg?
z+F};7JWzxoIkZnA!xPkyW85Fjjekaq=->2zrWRko{zvi>J_$bw{1OIM%`zC0XoL`i
zgbm<CAqJ4K(Fv3&kxEkefB{if6%`bwht8CxD4Bh7N6ayk5QHHS5e9`})Pl6l;Vul=
z(0fAq68a{65|7L*3!;2q{2EdDUy=COgP`p_7Hvi@vd7r^8*;G;vW5#H3ca8I!~h`?
z00RI60RaF50RaI300000009vYAR#e9FhEgZP;vj-00;pB0RcY{ZX6;PLQ7Vtkb+1N
z5J4d#Q&i9-f<=~whk<Oz;FtGUBs5Rizo+21K|NT!5Zfz5;l$XsF<DA32dgcQ0%By*
zvy2nb4dVXH{XUS4AeNAHC4wz*h7#vdvNDDjB22h6n8ac(ty933tuazICI0{|_$Dq1
zi!^LPWn*+(4+<!2_(2A_V{l<H6Gy`C*xz_x;J;3O!f<&BYUo;87)zcTBPj47nS;R?
zwQCNvEW!;CN`K5@;6XiG6B8*&rWzI_NB9K`k*;Y0gv86Ea7GVwr5i5y^sY$$l^64Q
zAuvi8PK4re%OQlhVD&;Xr%Azb#;F+vnU*x@ywj&fA%2Z6V4p%B4S0s%2GNRGm-PCC
zKSl^C?$oD@E(w!H#uF^0Qx4HW6neLy@=<8qDjPzYQL(;_t=l>JjjU%7d)S;zRyXi$
ziTaEBJWK0}`-V~Zy72TubU|qU0D(0VAvP>nYNCY06UEWfr%1$^Vp1`oG-Sm3C#yXY
zpQGsbs}Q(8;NFLTEL_2%A{fbDF@xFA+a1Pon3IgThl2FDCM_ts&{Tus5k**?d3?|$
zXeV@Rr7pZGD%Ct8&(T8B_dFG0Ut(}yf)JO{Ecy)_cZ0gn>yAY}4$3ZXqB03W=?LhW
zj3*e1cr39!AoOf>Xi2>2$Bv0=#!4(q^jKe=xTwjIu`MXSJXnS<ha7125YJHh3Qm^b
z;o(t5qhftM4;=_`iYlIzZzKs48tB?Xg-I6d+7uSiAAdR*!Wh*;(!WN0{SITVMh1%;
zVFYaOK7=1aYS67ImRRaRS!8JSv$*NmoEVgk1<WK&HBkk@6D)(MUW@Y6s5f@Rx)<q3
zs##<?s=>5WCMT<8HZP12>4wHcj}t{8f+0ntWkp9baY~M2=<yK0Ex_-CK>LTZAu%-J
z&|Fcek4c7sCxJRb3rI*WPTVBuj^ZI2I#m$#QM00NOZ+2QS~kYm+x-W%6*s{P{{Tg~
zQVUuqL`+n`pXu0RClL2hh8l&$qoIy4qH9_%u;fZ7UQi_xH>)^`iiIGg(VQX_)|}m{
zCHbKT@{0ceVVs5k0Iomu;B6`)jch`79td6GK?{st^cbdR!0B+|bZABo_8)?DnJm0s
ziqWP}j^{#QT6?|`k&80R!%c~+L*Yd9rBe6vE*>RF#vsO{MMW(nLq|@Ljr2-&jitgt
zVq;UDQh7<HB--HBMk@x{TOea}sNklwNl3yNn!{uXp*41JP~<8TT5#b~7PSz`vq*&P
z7v}p*P>>-gacnF!x%A+5j(ivM7YKUrZGMf;m9@dmchbFLLXW|ELGJo4Gu$V0SoN=s
z8D%KOGox!UShcQ+iMzZs?GUg)ycaUu9blN5nsUSUT1d1sMdd6Nx5A1q9*T|%Y^5rt
zLTF84l?n-~tMG~u92^#m;lWLi`>GuX@b_%3DUZlekBgV!w+aMFOmMv$;m?~V5Sh|A
zToa>J29X32hv<;j@2A`@Wzilz3t}49@Ozybhe16PJr|;V+rjJ67_B-t{5?Ahfrt_f
zi#QqN{{VCJeiiJg+rz-rvV&-Ol=yi>qX~Wy0!<J}t6GXvESZ+DbV?-B9*;+%YvA>V
zhBv{S8{p8SFtARL=*H~V#}54_VHl{9L=iD9L13__{vMR=dN#Z&+G#epznS9Z{tNgY
zgX__sFhKmB;c}G>X_M%OkyG$N1wu#BcZH`<0-?r)p&;mVo+yxjUjeU-sCaBkm&z`V
z95{s0P@4RBG2Y;%QAtQlLJ)*xAqk1A@P6ozL-We_qKk_YFTt^JJy@7JLXc1NK@??l
zN<rX>s*aM+aB{)n?F|^@WXE|L8-w)tDw$bWn<&_-d)Dg+LJ*j239+$a6p9N-O=f&*
zKVZhFO{W;yhJZ>C=7?;de^NFTWHCRiPl6bdVxnUm6%!mCveF53R+}tkqV^Ue!&n%9
zx&4Br!=f@m2$?zBFGSL3Pa<;4D58rjvWqOT(3L>zx*mlD7v+9wL6bw7M$oiFgGf6u
z{l=uCTSde?ml5DjcwdZt5)&BZonk+@Yl8Hm8>3}f5k>-bKcU?S!VE&zq42!1f8dyg
zKL}xch5dw{4+r$lyNoeQ#oRyCO{yUx458nL-wbeF7=ikVxf5(f%!~ZSduauNvMstK
zk<$v<NFofHRD3O*2E^HS(y|c8fm9<%WgP_BPb~_S;JPn^ec^Cb!$9VURBe(}Ng3p@
zF=Ltw!M8(f{{V8oL_HjEI=Cu%!_Yv%p<NKJUOfjleh&&oqFgPsCZEu%`Z`M&nqCPq
z_4%3Yqs545dQz0?gx;0<GgBD&F<_Um^$#$(#-ZcPR54juONLja@cbY6G%t4?6|^xT
z4V_L1D4iWfcpJP?xEF);H{7IHbV3WvQP`xY`7kHgLb##)gn<xZjECqN3}le8Bsan9
z!~D)_N>XK-9eTi>V6k+C*3gmaDIs;e5R|a_hoO*UiJ|T2mC*Rl0?K7Dl$pUD4l5DD
zI^kgRiLkhY$ZUDk7(+dlHbbnUOhLM31UPQKAK)c*X3QC8XJU#O`e6@Z!Ox6qp+$s#
z6#<sV#4+)ZbVL>*yfKA^pD>*gOiU4~DZ_^u5fKpxOpt;?A|fILvNQ@1#1X7I&?Nd;
z>2OHUghUX8AqYYcgdqq)d$00)5b80U$|)Zj7Ql{li=vRIWciPUtSQjhiHV3zAvTKa
zeZ!xb6ehH#(r?0-6Y$cP1n8j=2tr~Igdqq*5QHHtp%e5?d=|PVh-}>x{>IbBG(8dc
zHT)e1-c}P16%<0@1C0=c(qgBqK`I(${{X{6{X~qRF-gkc{{W%>qW;4E#lNMjSAfr0
zM=K7oRL?~eH5=fSVt%syJRgHZT}3XmxVv0Zm-N#Y>7`KQ5Qk2QjSz=K+7PH=v5krK
z3sxIxoow_x{0Y0kkC<aZ(KC%52wf>kQk11BN>Y~>H}gCpKhu;*=$T5f!s`Y+Jb$Aa
zj|S++Oo)h7G&mD$3Mpwr;>Sk|rQVddr73n#^RLudXwF9k7G5Z6{{Z$ZvWCeCd^iaj
zqD&o)F;KH0XhC4OR3ovhAqYYcpOTbJr722Zz=ioH(-3HD2@r%K2tp8pRSu>%(Je=^
z65$0!rwGh&hL^NZ*lSOp^==p7Vi)9GUq(S_h2Z&3m5tFhF?fIf!~iD{0RRF50s;a8
z0|5a60RR910RRypF+ovbae<Mc!LiZs5aID4|Jncu0RaF3KM+_o$Mq=%Q>9%-ZUAY3
z!4$k8LN(%SDz?AlJV(hI2*cpIEx%E_p3Vo=W@g$VAo#4tfCUI$d@4|*XwmAjLpKJD
zIqHaN5+;?*J#!BLsIY%T68JzR2Mjeqo&nWy33!)&BIC?3ER{z800fRps@{vcbGbuS
zH!~?N(_(1R2U5ad8X&+3D^(SUsEc(H)5H-prZzfe9hHp>N@^hpO;FIDu`x-sT{<P%
zxThpToJ1V1@G}C2Z_ShDTb34r<3F6ho<ZH53(XPOkwa)MrYmzbZf2K`rq)c5eUNdL
zYK1WTF+bBrQtbJ-LT#)f#;v<*1K?f<iKj5c$m#$(hUGKVLFI>%P<Vlgm##>zVkUoa
zL%JbA;~9rGY^VUYhy^aca`8(sHC$Y=@<Td(lefonG`f65`tD-v{L8b8=#NHI$tcU>
zHsjm+iVSCnJ87@X6$PV+U28SOca2I`xBHhzR|M@a^2*OYajs^IX8t2<u?)3ibuQ%&
z`wCkKgGE`fYC?<#<LQ>vaj3nQEP=F+2>pc|1zk0Wxv!Ag4r?La)wa|H>xc>JtMwVd
zM}XI_63i8h3*ji9U_n+^U%avb2!blCm7`>Gt3qW?hP%V}6&Em&h-xwdGB#4wH>>A~
zzZExA%3f_eN8sq|P1|1(m82vsufhbgl>>~|%rIzY;v!Yb0N2b{S@XqlLyq7o2spuP
z@di*$7AB9FRO|y-HdDy~igR!`VG{<)>@|N<+1!Ykc@DpE_a*2Z4%&)>#|6on^&0EQ
z_f1^Ojl>sKb3s+Pt)ELr&0;lB>V_=G)ILTX3(@9RhFWT?q4BA0T3z!v0iI!v8D*@N
zsJeravjCcf84JX$7@FQAl*(O7(JXbSmbv-kL0DxJI9B1DGXY-tWAXLOym^Odf%h$D
z3;trOO>enq*ZSp|_bIE)Tt(aFCkF@d6XS_|xaL)Euv8lFFk17tA&m-O>KonogWDXN
zoYY!#2sOX_BIPt_snyDIfSFK2?5#`(xv;{kP^*m0e8AH!ioe9M0E8#%?jhO=NA5Q%
zks$dcP@aZ^elF$9s>6Wym4S~Eg{)9mkW8sSRlW0YF<dI4eZkWSzJ1&<ORgoZW{3%w
zSEBp6X6-T&@n>&|jih?g?+r^?X8Vh+&IGn?kb9hcgCS0#IEN28OYDflUYCb=;%1Tn
zk*+h|C8O94hR3q!8THUKLZZ}dh}&G!TYH8Bj8#nS<&N46F5>epGN(290pXOPefe%G
z;Zs;)rKQZqtu|Bmj2J-O?)a!*$x)E7A9FeiL08~_IV;?75rH8VJM%bumh+83AE;v}
z8;z>56Q@yj+`n?z%wAkdRsR4flU>Y=SKeiEU%2aS7?+Pb%ZZos66c?Axc)87rnsDp
z{lPBr2acfUFm&8$#9ST4w=bx;E@nxb+%t#;({(rVyO(E}*?DGV<cG#;+!0Pcw2xb$
zR~`2V4JGD|Sa_gKD#_n6!HKFiJUaTH%wPG2R#m)2VIW&x%Z>)Jm4`CJv<m2>23cH$
z!B<|Gn|>{W=JyKkQ$ZguI-E-jA$7yuKfx<9^R})5%LDRUEyM0MZ^SC~9Kn*Jtuoc-
zAw1ZmzM6y82B?eGo+HV(%`f>>NR?ovJRaiMeA0_JfXWjHg7TH!Mq(grt!VeST8q<i
zEZvtx+6!d{T#KuUmh4>XPpAn)Y+DZcfR^mrPjwI^iNH&@tV;dmk0)-zKPBe2Ko{G@
z+;F5gex;iNd?}ooBd->!(6!GTUzp>zD}cy67NzVLLL6EEIUpr0FSz2wL`Aq|Z>e(%
zB1)YaiflNGf+``@vfb2IZ*a#q?hY!c_Y&6p+{ZG|!k5gkGk)PtZ`=a5QYAcbJeBGk
zF#e$8yXIZ<FF3h%jmlH_mr6@pO&(z4+&k0|@2|Ka!waR{Tx4bVDlT}bYgbbm6)bTL
z?*9N}Q5i)ms?--^qt7fwYUGulH5xVTR(|EvGaYU}WNTpT91)fl<d4URcM*_1E(oG^
z0=jKw_KuOY_QYyRHQU~aRm|SzDW~jZ-#Dg2yw~bp?N&h@=Bx1lFg@eohPMm6Puy|1
z=9QIA@RHx1VMc~=42?@Wd)-IBP{RB3b0_fJD=!_#0CA$noq3n4YG&7B9adH&P<qyt
zS9$KGVvpiQ1D>H}Sq8$>^%COB9kf-xXJ}Nn7G)Et@f83E4Q%qvC6cTGa=sXjI1*eQ
zCBVU;D#sth%%sw#FLlJhmocr4HxMbd1%;_X=Xqd0sYPFZ4>H%|aHK>$8DpOqMwqMh
z8yKv&M-W+E64Pg>U=Xt9O{+$u7UW{iyby}bl`>guilJ_1A$1o{m|!Ae;J1Ta^8=J`
zm>iz&Tmag@UQc%}VEsh}iGCQ3xLdhpU0qeihMF!-#+EwPV1nrig<uR_q`ieyF;^!r
z<l;0y+bwP}3X9Zffo7VUynbfIGPxi#<HvAtPZbu7b2ZiRC{`=P8rfXb7F|WVD=v4x
z+BMSPUcR7LaH+M#Tum&r<KK>;l4vho!zMy8AW5NATx&0OH2birZp=;>Nx5k)fZF96
zY>C<a<~b5*Q23RYC@?AOoI}|Lnq37hvlok61tz>gn`dyNNl}s_;Z~3H5^63kYC6a9
zE?#45%Yr$M1!^Md*Y^f02@U;S>*fg`Fm7pY_X9~ySQ8ew*ol0ypm}C{s5C%knST=5
znHs5}&nqs$0hXz9*94#iA`q8(f;DI=CzH6{0^`(G*wg?k3j!jT2%^;H>!=(cq@=r1
ziwLLUo8lx_qRC~EMqFvRMJTxW%u==~3t*&L&DXBw4_h$V#l7<}a1w3pz>7c!a2D}#
zz&_xrg+y1Bq+|$12Dz31F)hS3mmCpo!--|!-H~ZFHj=|j9PVbt(U_~fivsHAgM3Xk
z-ex4S$_3R+EmjwqN&=h2424G>#KO{ESYIfCG0BDM8(=iV>{t=4{vznJ(Jig3i-4)m
z=4mNfVy&hY(F6@q>NbM4E&$_xBJFajjEm0^Y2fn@rE<$L<-|Ban7%)mdXFYSJwyy8
z8nc;iIMpje%8;{BdzMs7lpPA}yZeBlBPEWtFyn+>U1|>*%*b`BihY4qI<XDb`Y7A+
zDV=;ofX#}Z<c?kha~|{m0J7UdiYE8oA{8<a%e+8MaVp$LIJn6o##S;#Dyqr`nlVEs
zX>GKXT;gwS>#B!i6+tRK1<aC&3i2>w<G4-0urPcv%{kcFw61Dni`4Fj_8iI+z{Pfy
z7^WnoV)uf-QC?$9d5G2_u#Z!Z7w%M;K6Pj?iCS9tT?fM)%3`gmeX)qC#d-zJkqq{(
zIa<fKhRtr>*TNJV0I;^fSXVc=x(o{-Qg^Ae4{Y&cox=feFy<Q6R~y`2VH##P+|2lc
zlTlbg3>2xNGHq8V8CJ7U1aDkU&%{~`#IY5DtheG|bBym(&Vx(m69x}I%g@UI3-T)a
zwtig?!&CVuZ`1)vk~UbLQGZblU+?&4pk=R2e<1#1VTOj&`9Cp*4afNb02JA*W_f$M
zmL|eo)w!8%#Z&<1{L2!GXBE$gt=~|<yJLF)0J))(i(2bYVTA{sAQW5$s)z}doc)r>
z;^mbxfUL{#Ad~?3fVPxFO5<F`-n4?W<6O-dL{hI`sg_AnrFfz$0z4)1aSDg@ry+q|
zp_i?5`+$59;=;~<b0zE|^~l>DlKsl+H&ODjY_8lIfK3IPN?k(Di#gj$dW8lZ8K*D<
zF>1a1L1n6y;z(__roX7x$RGt9m-?@$r}Jwq=A}i2s}|Nf&ZF-5>2=}|0bXNyW+TP<
ziB+tIn^4i_3C!_c<64Q(u+6c5qXj5hm9yxGHdDD<S6xb0Z>`LMRYS_jl=4wJx=O<y
zUIh$(4yFC9fv7F0t6Aa|?8Gv#vd}|vfUs^2l+(-(Vqq1%;<|viK4G(#4RE+FZ#_!5
zHE|9)gycRVoPKT+!vS0P<&<d;h8YYxVjl=-2hoG^8-N1p$$vy|zz%KwN1w5=a>v<E
z)Jrgb)iAOT_9Qg1YN#Z%U5H~+@AFJgG3(pmh>0lceg6PtRDX}~eUm5?j~}!5QzWqS
zaN(#w67OGgvrm`<U&M9SxH);`n4i&83Ct4EFJrIf2zf!X;oB_=6k&DLFPXk((RmtR
zqaE8HOB{q@LaP>iBT(s(s>Pp3oRn8SjK9n+9e{%P3t*<TQ=b`ofr76Q?n5e}GI2eV
z8imjx7%yDnUP=)=ah%E?;hKuo);vXw-zGMQ*??ivGr)$E5gFVd--{~o=2Kmz1)LRH
zs6K$KL2wnVeMfd-XfEMs`IOU>>R#-w4ET(Evy5)Q>u_+RGj(it9}<wMtkp%E&MwdW
zrh-%8nFgS+&Pj|9CRkP;C3|!TFPblNNhHflS@^9$j})tA?0HN^uA;qYy<ah|a;~CF
zYPn%<E?j9S*_*9+gv{`vLl)bZZm8uh#r14Ew%$&t>4DAv01*n4sfJRLjG=@Bf>ML(
z6uirHULqw~5GufoDw~HG8>*-c4mpp_;sv%=qGE9?fM>YLRi0uFmH`*9?E!)hk#DXa
zh?>FL@9M)7sJ{h35DtS73!yRGEj*n{PfnmMq;ar=B%;kjb#_#&LSW>4nT*&1^N0C=
zF(d5*+U581gs6=}_<rGQHm^Ua*;Ao<=kqI(JEi>p0A$ZRJa_nI^D8ud9Q(BXAik@V
z`tRaw-l0HkZa58*DYT*cBQ(a8R=SvnX!&8hVyNTm_XOBM?hZ2+u`Xr6Tywj$z?WmY
z7c?ASn2I=5%~dzga5TlvYX<#lBRg7Y@?~S@7q+~MSU7~+2~Em51;(D1BzJ?GPJKfX
zhU<83xchqDsW(tW%nagQ9X?q90Meb;#2?hyRW(50)WeKi4eVC_CP?%<(z}c-DxCUP
zm&A7=kwqTP7%gx95O^X{P+h&~Asm@VK97<YOC=(fCVc8%fkIS264k1e!~j9*whT46
zuEhmu;ww0~qN-^4fv}aZ(^~fi&7q1Z^)41wD#gSI#uqx6RhI32cN+F=E8VqAmU4((
z>dnA7a}_B`08(R21i0B15W0n)CogeQOh1TFBuW=Bi_bE`#->6?O@!HBM5_#LXP2Pv
zFhMPUjs96wmOw4~d3{3KTekeasK>YhRklW`y1u0{<??{8zYtfC$uw$dNC&6|6|GbX
z&&H;I8i>2ESeGb)WGvdRp8jP!<P0C_`XxGFl|tT6{6an)Y959D7{4HnSB(DvX+c-B
z_rviz-YSnNwf?z$t0kkx!{+0)_0ivv(t4F+o2UTAOD;C=<}t`>)M3tG)yY1yVGD^P
z#$IUZbQjF4>N<$B?R>x%yTA{@0|AP%I&(0o=rx2aZn}vR%SILM9GM)y0_x>kqJ#0a
zX;mn8kfUeen!ZRqvJq$82@IOz_bQD80^ATi5YThzg1kejnko?yDHasmq~y@)*2#0h
zsg=Pif-lIdJ|O_(s;1Yd7%*4B;tJS)BGdIN{HH2&Qw#w_v6KxW>RedDsd)7Tlzjzt
zR`YCT3+d(-=46G@@f+bQx`O_#MUNF5+dA#NJxiDpqj`sC)J@<fk$n6>BW=dGfbEYc
z4`lnAl1t7wI5YZU7xbtAu10cS#I(n7OMKNXJT)M1g8e|QEOFdw28_$4FvK^JE-D!0
z7!VTL(*q%XqM6J}<T{G1fdnXp<my#OW?PJ$MV$)Q_3j9VC~?u~{I?PyY+nz|P@!w%
zqx+4FY3dT)74Z|J)yq<Vo_U3fdg?e>eqwS_M{Ptj=z5L~VVXa3gi@sgl;Ty@w6Hhx
zDN9op&sh11V)EDzb*3tVV*&fo@ldoYo$EUfn3Ht=W)Ijw?T%0U5r$K>+1T*LPuiw^
zwX@7v=kXODnNncH1b=XZQNJQ6j3pL_<_Zf|FEZnuMLCE#H#6oowmdTt>{HgxVa#!>
zw9%Z**q|}+z@b$O<H)h_GPmT!${XB+8YzNkCsux?^=LAI@%oDsI24c3h&ZMKtbvU(
zsK4eU^sDf}+zESM6*UH(d{=R|YIFEXqK&9|FHy6BAy{G&0{q4AP>Y)$j@gcp6*YZQ
z6LC@44sI_cmRNyBW|>v4BYj0>*uzH3n8(K8WB?>xsm`UOnt=m&eQHxdrm7ee)aLR)
z6`ZNy;y;Z+I4&{e3*Vv?h~&ZX)H5{r+@YgfEkxHv@!T=YcEyE@xnM$Zh$#_pxw8=K
z<HW-p#Y@G)u<(F<RJ^1I$YnHH@HC_Czln(eLgNMe{^ADg{z&<G{{YCvss`mU+@N81
z6#_lfvWhg+LkwaCz&|Y$b#CIsuBVw4ZIFrlPFKkr3#7BDe~iT5@sBXbr#Alp5nJNQ
z@lXY<$1-0Om`_nSVdp*QpAxWMfq%ymvela5j&ZS+0?UaYE6-0_n<s1*TyD)Y%mB1q
z*?En?SsJfDQuG@Z+_~Z#wt0<BhkVPCd%&m8<CnB(Ubh)qFezXWYBLw3WDZ-8X2TT@
zwGj_hY1W`(FL-N(OA&=8GGVFV_f(i4{>5|LDSX<U#Q2zp;#8GV08?`Sywd^Bd6cA-
zLDDCIm`sWx@kjZUZ-Eher|MOTq}Q)xG{ks0V=E16i+PrO+OEeZHF=rz$h63{)U5og
zI|yA~$N{94LvJXYN;cv(-9&1{aYwP^*D;1MCw$Mr3nZHY6<X$)gB;Py6q(_;rHi2Q
zMTi7}LN<yS$=tMrJ+04M*XBEHYL&Hit~^0trUAPv+PLu$-a6IFj>hNcGT1M2<&UeE
z?75$yOG-Xv+_Kj@jtBDtxfN_VfTHJ7r&^e|n2xUHEubz^?LW9GIY4+Q^=uSV_ZhD-
zUA9ZYoW!<GYT>~*^ZJ2hlD)CQadL{id0=tOOmUfQDm*>7W?X=t;$7xe9AC%#5wkz-
zM8)4VTZmT7W+iKqWTU8=GJ2k?C%<Pu%n5OKzDOahh2v9Wm`Kn7ZGV`@GVRqwf?I4;
z?j#o$j<}Y4oNEyX;sV5tD~f~+)^_A-KL@2w7rKdILLWUeN=gIhur@d3tB<HRIrdLN
zUli#Wxa)`?X!t3WK1)X~poyt?g+%5b(=I|{@QS}bxM%qsS3l+{ShYd-GlpV_h$xn&
zc_8?G!v6sHm@|3m0z*N07u2bYWXr02%pgd#Sp(sX?J#I`&FM<Jl)z9j*tG3imkvh>
z-7r{s)Gf6*yy_lrD&(V>(5fK0$n!4-@?bPJR%3;8JS@u$7jxi-0;O)wo9<C4bNXo1
zGcI*70++Ox6g@mZokrl;mUObrV4O`4XklPMy9T0MG>jW2Y1gP4>FAk%bWVv*YoLr^
zY1!4?pNI7yW|FQPe=y|Yh>D@25Otnn)~sSy{{W&mdQtNj41wK8jbJ=MZ#PWDGF#&$
z8#e-#uB8ozMy1()-{uZLYns$SRaa#{Gg^k#eOea>yybkdFl@M5rGO*%8m(0L9E9aT
zc_nWi=&#HaWuU6TOEP0w#K5ahQ%W?$LyoQdJ-{Nh?L0nX6lT<6+=^H_(=+SkX*ocI
z1S7kap@q)QwJ~zzeBv}`s6x?DRQXtjEMP8@vF=$7Bo`%9%zFwoKMK?wuqxX8L36|k
z?VNwX;iFs!xrw6{Q0=|TM$&-t%qBw0t#8~k1DT0|jtoi#$!((z%hl-BpwpREczb|{
ziEAhek%1&>W$lPp5tLAO<{39BWMX1mJN?`NzZOFBK0AWUthE5N^#1@59TdutU!ndY
zeb{!Uwec$XjXAlIPtc8JYQ`Ln^8%=C@^bx4yv<Zy+u%19*cQ%SIPNF9IStDTRTmcb
zELn#U)&vmp&uvFqL#w^ZhSvnaVz^bU7xpX{7Sh6vIAFZs2Z9&vIv{M&=39vB<q%SJ
z3K2=B4YLDG+%Kf5Wzr-4Gk#k%K&)IMS`~n`UjiFyzVUz$yXpaud>{+_2j&hdtl>8p
zuE}kUwTKF?JxhqTK+XRE5WsYril+_kXjrqv&Ddu5C|z$i#LQ0{&?jmiP)ZXV(8Ghz
zb5kP`1AwzbcdLt7HD!Yam7bu{Ddli7rtYRS5i(K8TypGx$gx8dA^!k9%I7RvuIL-S
z5gxM(YSRV<pAC!-1)`+_uDrwmCpp%>XEw>ETu48>6$HYs=>jd#Dj!B5cM9O;{{Vf)
zp-O_?uAombdwPOKiKC7Bf)_+{Z(EgzP#`Sz6v%blVBDl*ozW~zvTMYa%a0lz2gv}~
zU}0TMjA4f}+;N!mYP29VZNNuyat%trd4i+Np2f@n*eB%NwXA}}!v=90xZ0aQe?AB^
z%}i0jas6rNy}e7FH$f$#F!vsXd6eZsUbX1S#nr`ZlmI61GQp`fq;re?Lh8yV$eAJ!
z0~H7g?kt~*ZtnvI?$!47s)S6p1RZxRP`7fA4bq;}iWL07RU7FZ1KgqD_JF!G+&qpD
zz_tx11O8)f!%WCBcPuX;uh%f)x}oPs@*jwXuaj~gWuC$W;sB39Pda?m+tthqtZEN?
zWt6oy-FFJyehO_r=JOsm{{UUWxU0wa!nA`>K&=j3mBwc79Ci0*4=^_Z=i!O|4Y<gg
zYz2X=_=|Tl3yPy_AkIAYE{7OKCIeWkHJZl|g&Z>0Qh?#cZcx}Fj327F??|uNJA6v5
zTF|rGd$@^n8BT?cPOmtIN6HC<OE|R<9%=3lW_&?)L;Dfk9wC0{q$@A;7LgTNu?R8%
zS1zIFfAJ%@p;%C~O5|c!bBev@9tg<Is@3jLni9eJ%Ni9B#vV(RD#S+os`Ub+#LJk*
zyoZs2Nb|HJQEL+6=>ZlilG(1RRk1wA7DbDej5i-HY_K+)?JQO!iAn~6pl19oA2XKc
zFFv9ci!K+UU~e&e&powQ-NR1k>rK48#;BRF;8^kwVQUlULAMs&!dM4tR(br-Zs*02
zCm2a>D03QferCe00^|)~{$tUJi!7^J)usEutA`iymUoQt8oQ)5$1zqClbA)V1_I7{
zV{qP7I_*@_;0WFn4P*iv#FeEW0D>Ebm>t2>Evc1?0XvlDBhn+q$tkH}kWiK@!-0sl
zHy59A3h5-v9FeGuO^^bZWNB;Fq%T6cn@{k;o@Eb#;8y%xQ5R)=50jsUU=uO6zu1uH
z6;_Wh58y?)x<d7}eqb!WG?%#CK_EDpD)&;^knU5Ai$_#c6_}ce;lG%JOo5HL&i5}1
z3M>QGCHdd~04mfk{$Oe;DQVuIK9qciMACj?VH4y41)w;~z9taz@F_uF$ToIWW23$I
z@f@(qtO;%UxN$51^)HHF5LJS{t`_Onk{VBO{G{g9DOyc<MB9?|De*5^Elkb4&Ske2
zKA_WVtxHRSfk|?ih!CYc<_4Q!ZQVycAx1r_^D39L@IjaMG+cKZ*TVA(^wBVU$4P2t
z2c`j+1}`wiI_;u92)Co$RR;LXa}8?^3!AXOTQ5_mlrOd%tR0Usolc7RisU9EiIr?%
z>R9q&uwEAPDq@Ocz4t71bQ=sD>?20fbg?CHE%rutdVyQImPud;JcT%!`v%`Q;Dsh7
zrpr7_7vW>p!?GaepA4-ikT8u~GWUW;1BRw<lN=cHQ4SvQn+LL1*jTRuFNN%bt87l+
zBdDRG2OY|n9|k7n<bZFuAYjT66e=+SQ+lZf@iRqH<nlf*%srx;8tL&^fG2Sep=LU<
zn8k&u8|G@lP!SLQOqD|``yf)_r`)+m)F{+Vn1N(c9GSef`Y98I;kkP4lL84q31A%M
z%&~A1mu+Cp<97nE#)U;5&f<<-97?57H0~7@5^UUWAHyJT$51yYs5C9D=Q@bPVA|((
zrKS%wUMwPE&&**_vY;>>?arzL01btBZYuIR2lq9h>M-U|RSLjIh#z|P=ufgbcnjL9
z0@6A?rDPd&srAo(Az^XcYn{h;3?%RRAGl$*p{Ag|^jlcWD1MgY;>HMkl+U`{4%kfj
zUj!+Y;g0;umQWl6mJf{Q$0Q&ttgWI8%&CGnAp)hKYZsUlnyLyjX?@FbpNGr}(S_9N
z_54MU%}7Tj`NT|V1Gf0A@Q#Wi3n4=?`C(X)wiku1)GzTwl?KBFO4UJD1S`;)j4VJ1
z?y~?Xwi>S2bHI9yV(%Dni*LEJ-i5puSpCgavxM?E4&LLHEB7Zo4MYTV<Z!^wX!OQ6
z`yqq!BAD0!Hr=&Yj|#*M)|!5Z1R#>$7B!MDn*eMZFSk&0@GxxP{K_g+KQInF23RTH
zgmCD1_{&nRk5R3g*ZYowDa}8)UtfoB<`$--oBsfEi+J+!1XX9Ti=ndlKT(x~t$kj1
zb<AQFVvPz7z0$|etx6dmZg0=xWSUwc)z_2!dznS+V{L!c7cB!oORMm6#BFK?gR+`n
z0gyjN&%+jy-wVXfgm@Q^#7*|=FG27@awr6$p^*2ur*pLI1*`^>)j7;$G0x0rcKzZJ
zWrlGuK!&YmLih+3gbz84Obv#;A26X7`Dnr7ytOSi5x0ub_ZjQ3M3hr>^!S3fnEYH`
z{{W&Ok-%AY9!t~_{2o6M0bR+cmPhdgn|UAsX1i)wnbJH;4-j&EDGE#7tNB8zI-dUk
z;w>Roh!kX4Vw9s|>fmJBwdUQ%nO?=G&oE8u3awvv5iQ*42xDo;sq*ZZN_HLZdm`l1
zV0z3`b6#b_xpxQLv|GTgyUJ8ykr1cLF&dTw?q#U4l|^UB=_>>7nyC4U#K$v=B~2Z^
z-PG1%n+KKVCm5lj;f3=<a{R*Ac;e*@>UCdf*nGnjSS3$zk!RvMnVuuD$Seb6Oj2Nd
zPF9ammbb|P&U2<~vtr(cJU&7X-i*U3NxZ3z!p}_JFnDGb%1uH{7I4H*E-RY1j-ZnY
zq2S!g-XW(fD!LVI{6fA550a>FsdhfnVMJGxoy9isF*6JXjDQV4l072fl7eZGFhp}D
zqTmke5l$2yIP>Dz)F40z2L;8fjO`M}?B26QMO-@^(hR}b(VbBY8f~#{ba|9O^NuEc
ztIV;kqt*+%pdkx2f0*EEo?A@N=yG+qaX}^Usw;;HtMdbB?TFYAGV>MOs2pD7a;unH
z<zKjx;Hu3XD{6=f1^K;}8+A>R(J7G#(|1VfY=LqDw0h6QN^Qy&R9uzr6;*T1ZXVD9
z@Q%jr=?#|&OcC!CA924WtTda=%Idkp4OqKC>E;g=sO~Ix4AR#R@mC8J+|AuLHG#2w
z7thSkH3r_W+q7AtsuET;t>$`_6|8RIU_K%_^ZSBO<7wMt)XV~reIL1`xpDU|V$Sr{
z=2*!Ka%{M5ZMCi#<t=E**!G4{Htn3-w-MeBd)`bBm>4^pWQceP%0U(Y8Oy>A!9ZqD
zAbL6ZfmXi2zkw}&ZDP13S*-U0jOHa8m-6agirzCa>h$fZj~}^G3$ENNTq9NPRQA`a
z)xJwAkwZ|f6!#Xe*zB1H=wUg>hxhB5SN-8F@AOn*tDvEP7z&J)3V#CHhK>&i)dIt&
zR)+W_Z*p{<oXFs|>trAWG;t16!tMg647V4cHXZ9RBTT<_YYq4YIDz2R?hZq^<iTp{
z#^F3%6HT-$i{?|QOmev<%Fnq-RGu0X7j(VAtAdcjO&v?NgsTJ9Ca&)=vl_$3zZDAL
zWNv0_ii)iN01(h`;%**w+&T72SmhK~3;7{jAW%&?!~Xyon4jsn7_Ti#ipZ}~q9Tim
znMqiv%wr^cRba+T+{SH$*#m&UIQWkmnvB^^6@rR{MTp#(zH!7VLCnzFKYNZK;Elz&
z*D_H1juRC#@epA}A{SYjjTM9(EzWF7O8k-Di?*z*2+O$YE2@c1s(Dsy8`@&DKxtK|
zps!gk7v=>-^r`um%mvDLMv@yd(y05GsR2V)eB3m`>jC!8Ar^Be$7x9UB0@ufB7SA?
zyi=s=?&Iq}GxC@p2J7>?kM=o9`C;NDuQ0$zCvT~>iop9ZgAKL6CFU`5wJlTTb&Hq^
zwi)D`VeqGxps++k!?Nng*5U#f9tNz1cBs1}!MG)4PbrXIR2y(QT+0LqtZA&uFh&Vo
zjp3J#mX_M9eN0SbiU%+7nHKxbrD02UpvfO90zC|pxg3ZK3D5oxFy<P^{z?L<8xro4
z;SLcfFcw3AYJfA&qKgtEDeK-y<O;6yAh<rGmVB{*FsAsH5ombWSC<=wQcECZU4e1j
zf78K6s_RnM5;m(x`6m@`5#@VU#*VcD#%91%aYwr-1%VxIwJAo%-EtXBt;}+a77(L(
zZ{5Tl5i1cuZ`7}7^Hu!C2<K)pqvo)($yOUVBd~H-9a(A3u&(2PNV;oeJ<;cAEZ)vM
zOT!o<vNw!3+#8VEDmngcS*06x9U&S^7&@oo7LC<QE3GK2v>%c<zT=4Glmx4o6$cWE
zi78U5CUG#-u4R3~!d_*|mvL*%+-mLtkvZaQ;t$l`IG0>XvW!H&<tnC&@3sNrS5`v?
z?TN1As23fb%RP4PHe%ehAXkzZ4V=NSqV{SaWOtlE3wCla9!^xknmm!bfqb39mG~XX
z8w&VkS6|sPx_f&gne+OHu{QqzfV|>;+54L8l%CeVnCgwo-}?|1pW+t(0EP`Aguj3A
z#2V-r#zNm7pde->Tl<xw;J{nSRbM-cAyCrRAgIN-{{Rx*!hfoMU~I~HPyt3BVz%8>
zphyaMFxvw4%QYK1F08BK1XS=fi%&m!(-ZBI#e<P0T~*ei`KGfs=d|p~$LLp5uCncN
z;+a4eN*6hJf38=yMx~rx=Jin|0Lm>aWZ}PY<}lnLNZ4K7!tekEUP!*!Ev`V(T(7E+
z(&BL^1{bU{vBQvm#mzST`4(`?D>gd3Vq~CKsaCe}1czgTj71iU2I0->>R}roCkS&N
z98@s0ywtlAEE*zvVyapBgJ628av41n8iMk)_Hi^ix{bMA7;`dJ?(0&G!vVB<Vj#X+
z3fxkAR<)UIo5swsQ0xqE%j*Cf8A{{+9u9Uh)L(!1+TWP+f5Y0%{{ShI_Mn>k5oh-&
z{{VRrN>v<YJuuyXg<4OU;|mZ6<%kuShhaLlXW!BacI2tZ1K^i;z7hq=^-}w`&caY)
zgM?shis_q_QAGjn7@0e{VgNMUUV$36CbjMYM|wC6WwHUGD68E+iKT*@ir}iq6#zPE
zvC;nknzdk{110$8488>tc`GEffJ_k@x~yU!Gz!X=#kc<e%9KJB0?|l2%bvYNicuLC
zbeEhvK(fK%i}Ba$JcT8du-@e$WlW<LM1Cc`!2s1>Z#KQl6v-@zr0C*Q25Y%U4kb}?
z0czsk1ZB;VA|U45#a(!Zp^5`dth(*$p-9AW8=@2i=Q6A$vm4Fg>8wiXu|Fp4Rdn?Y
zyMh}u@;bPvkdAD*qh!o%WSRo+h7&QI#{!QT#J8kq(NEMrY%GnEh^wHRR$%9+Itrkr
zlrB6=7DMhU2g3xx1ENq+zIb2*(F+mhn0)BfjQW)o@J*F3>4-H^tjAy(?hpp-f}gBk
zD5YXcywq1CS`N?RQuuG0<U@N=&||JXWuAyO{L2xV2Jx**1EFZPG@fC?U?^T7RfeW)
zM4_$JV-YH$2x6EZT(nx8*PdYzrBiT}U{nyP<5k)(Fy+$rg25D7rQA&PKpXgpwLoA1
zdV-)8fX}I!fHZ>I{7Sf>!lp3CQqqNL^)gc`0)tlrML-q{2BMdO#4m%yQ4nke29og^
z1d1q}93^(3SdGK3RcAfSUL&hvuPjJ-JHoTjj*D`}wu$v0rGa+a@|9Dqn6@m|2=EZW
z<yQEq#y}<MtwFpP{v{W9ZUa|W6F9%9fl5?C@_35ZD5k7_8AUZ+#Ro}4Jo-&xnQ*5s
zJw=$?(*<DNx8g1$*lAexS#7|@P~L64d4j-TH=AKxt3$__V&Gw9tC|Y0xQ0rC(N#B8
z*;N~UUfQ%!TOXXjq#y#t!d<p$_=vja5suSLxE$O_(Qh$Zmw9|*D^^ppkxgXJ++Si=
ztpFF|_}mHrwc;Cj$blKWyfN9chv57ep?)|HPcWgk?om)_J7yOcn0`nIAx2Ev^1Q;Z
zfGq%EwkWvlN^~G4U?oiT=3C`xniYx4ic^?fREMPu54Ul>w4Yf|G-?1BAk-gK$QxTM
z@d@+=IT7wFXw*ujpr3HDvz#$^AI*fj007f&;=H564LIb8<-izjS^xkp&ipZ+7~d6H
zh%PD!dgO)j^_MfuiU_Zcq#xp9)m1}r=TY#2D1Ai=L#zJ)2()6G%O6lMH#Ge&qUO!Y
zYmyQl5pe8=4@j|E)ai*l<ipT}gG2n{4Pt~#;)UHohJ{@eUUkU?NUjL&tageS_7%9j
zv9uh1X<N*0<FI*#gBO%w*~02mdCjE)RD}irh{np=QlbHhFbIyA=mIP@Aw_*mMZh#h
zjhn7T-w@-#DvRz5nS~GQjqwpe0*&3e&f`BFW=`qj;&R!k6Uy!PFl|~jV15{d5|u%=
zQn6Q6{@^VSl*}=A@L$|$Gw~yuv(=N1m(;N{9|cVACyY;F>4@yUN{F@tktOzV%9b!M
zE|3h1czA%Pf58_W#iAy+35Zfs=Z`V5!<lL_MQB<>7Zf{h7l3LE3m0V*6*FNJV6#{^
zw(b$AsIC$P<RQ+kn8y6M)S}x4%O8gV0IhIygeWf;O-*K?0j5z)VaUN*jr4%95QQ=o
z)@(N!a>TGw$}%TECV7>2Aw>a4D6(MIF;P(=1u1;gx?NE5NAgR+>vIT7(Afk7B|R*)
ze9?kcYt-g|k(;W)=2IXn1k;8CTIMw`K(K6v4P17)%(mIVE7YZ5gf&*(*6VX5)pK3A
z+`yT!ltchmXl;(#uoC6*;xQUHsSo$m>>W(}3jv*&sKMMSI}3RiFt&wo$0ADGy;^_q
z0@A`6((3QrJgcQ$Gn{HzR$*fsY=zL)#-mUMDhm|gN~xI{4B6A>D6o{QP+dfw3}C~G
zTvK(*ZcuBy+OYs5jND}!zqL)w7g)vYO+7$U)aQnd2}Fms9kDz{(Qy7$cCAIc9Lmr<
zt*4VK6SSGdfK}K~y2M7wR349W7%#8<!iB_b{{Z3y&5MLOiK1?0PLkum9!kK(;8ThZ
zq#Q;eFtG)eR_ZEY+%<ja<vU|<^nY_@7s36(Rd!9kGkHuf(ti--W5Y2hD3e>}q6Wjj
zVg*<qNl{@du}@6R+NXBL*l>-DmbM={S($9)OJ-8n_FT+4Yni{ez03?|w*{XJJ{pKq
zR)Y!wUKOvYc)VF#2-2KVVXLknhy-#CvVp?tp%R5P7hnzyWzI2}p<_y=il%~>O#6y7
zz+MWV$d5XLQFEw9i$tc`N-Ail!jx@Gl*LrKt*2IXm^Ll$yUb`MO9yTN4HIam5gYa`
z^Pk*wS7GdKQTaZjllhxPj7t$a+zU|U4;h&CIa}XhXjgl?%B}*3s}(6{kcVJHV$i|R
z7T157hwxbiaqze!NugMR5vsemNIqjj?S;i>Ok=J7W*2z5jM1gl5l|k?M_zLpQBDII
zyxr60HindzUzu{XO0fNkgfKX@V@mrbMJrv)v{=FOQ3FV~L;SNPyuj~v5yQaHLMUi6
z4yYC;;f-PpT8iAo<sOte5Sz=vfe2e%M{P%(C;tFKf9n{c0dCossu`?N%CB>Mz*?SO
zVeV{4)UbSK_+3mM_+~~6pqXq<hY)wla-WHRFfj!?A|+_uhc83`v@19Au@>k*a-t$j
z0^*jEwZi#`teQG*TZL3B4|0^XZ}S~eh6un#trE-gX){c8Ef>51?8xpH!<aMEFSo>Y
z(sb@r=pgBrPz~wx0`<59iv2*CqEe~Mcs~gnYVb7oGin%YjakM+_dCUmf6FUH2w;H7
zMK{^INAW8|T?!1HRWZ|$g}N!OsF(9QZUMHbYXw|40YqE;Tv~FNd<1vA6A(iLU23eM
zx9$;Y>C-`Ls!D_!6fTmwXs4Ks*A#gxmw2x-f%?o58<&p3dT`1S#VgvW{8UQYzM`lq
z3hhpm-DW;|c4@OPDB-*TxLq<L)f5;vi`%+zLf$1-(9J9|pseqBh#IPT6c*B%TJS^*
zW7SXsM}uw)+#@g`0BuXg-^4Qz3bz*z$zC9jq6i|g_S?^>HK?d5lm7tJR^U~(91j%_
zR%JUED4(|kBWVPKWl&02%LxsW69sSXUTN}ws3|D`uTv@UE9Hf-z@4!zy;BY#y+rUC
zeGsaHvH%wAnQg&dxy&H2^M(c>q<g5Cm7n;rJ<6@sN1?)bBj+e{W5J~G{{Rx{19o|j
z&4knmQ7WiyIsX9dD#EU5O<fw582*L>Soje?0k#cE@>Cxt;l|qPT>|WEiej%GV?olz
zI33=j$gSI$Ew$frzf!2+Lm}M&4k{BzeLJG%zjQFueNJD*1?KO}to7;+6#42^ImyEx
zLQ3NzR&1<I#I}ukio0_0EHRnHHt=cgS+{h4fA{_{N?ERq@e$6@b~v>&f;r<*W;mZY
z!}()*;ZcRfndeVXcPRpy7(lFz+X%K5h($4%IVHJWhV;U-g5j#GS2U7hu?BJkCGk~w
zre?(PTrvR5bq%?-C1UF96)`X`gOGY|6vh_ssYExQi~EQMHZt=G5O5PVG)m+E88l)l
zbP53lDZ<i#bhiz`=_(nuDMM?Z;xIJYkf0_LdAW_FtSrMEHP29VgKrs2aLHvs1&Xx%
zvD0^H$5SU*qJsO6egNb<mg7@rce;gG8O5_KZ2lA}5{bH9Mqs4|GkR(eW@F9yks%@9
zcK)VL^Cfo|Q(!JvUZpr2ycH23^l^Um3AN}nIv^;rYwHVev->Aw`DO!6E>Np+t9PZ(
zDD=-&-e7zCn6?X0Kqw=8u%#3!JBR*}DBtBjxQLTr?T!;=R*FwUPClTj1K;|FIvOQs
z0wbW5a?H)Ux4DYf0yX*n0P-DLHkHa9MxQVN*nJRav6>t(LEz`PMQUnB3fu(JLf0SD
z^*t8l+_`jNd_s;hFz+8wa?;B^*K&)DJdms5;sL^*7~xC!jrLv+rL<@6CMP~6EY^7=
zK}_;Glprdt97NNTXV^?!#fCgZg&d@K6`@!vR-SVCVxkUZCmmaPed-NB7NuJw#nv7o
z)Ix10jW+McmL~{y%sNmTtl!kV4JSCDODj1LQv_UwR#0b8i1ikY9nf;&PGO!y1xvfn
zcnk9@AY+EjjuU(4qDoG-Vxz|qN;wePynHr7Rck}f>RjN4Aw?ie;_X#}?1xms1zgK=
zw7w5A_|1*ASDQHdmLUUF*UHr*p-|dh0|^RCa>s}~9QQygMsh@jRU7XzgIX_8*bKC-
zCHYB2rKdwN!ii%bd4uX`ws$Z60-#m@05IB2S4Hf{5J|fpKXS?@d6xeGY%+#rEo>L}
zxb!v>K1t4#cS#0NJ-CSgr<=|pTriByTZ+*i2s?&-VA*lengii6jPxis`Gt!r@QEy3
z#)#&BokVKex-$ME21k$l#7?~@{$-&PLM!EhX;<6&fjR#GZ&O*#{{WQ?ihoc3MSH8Z
z@8BXCFCu=R#y>ap19OMZ{y{JCKlw4xufP0-on1fqHZ^~oL(cyIokJ}>ZlK8-*e0uA
zSi)VvwS&y@(EigQ${$kZi>b-SGL4tV_cT*|XY(xI#vk0V$$US!e{_Gi*`wjSE1*7b
zvg=-LG+(>@LEP8Pzp2CQSM-WMJ7?-47vCb)!Q(D_yd}VV;f7beNGjEggDC7=bqRwa
z7WC8n&7b&jUJ)pbc*ceNDf2VLN|fk2A2IQOEz?F00e#FJ;J6|VKZsZpDUil%mHLC5
zUn67ribpAx1y;)4V&k%jRfcyjaV530{rHRXz?OCnp<DyANNn&MuJ;@RO63NR1!L|Y
z1p{6RqIM-`lMF_T)C8i(J^GyLA{wf!<?XtUj2xUo9o4bHN)Zh)E78Ed1(>bjDk~(W
z=@D3f2F0m}LJVAbKsASDQG2l}UDX&S68CxjV>YgCViI->c4L4L7)REke<XE&QwW*1
zL(34wm{=}=yG&?eD=k(*@^=YA;<qmXBeXd27ugclzi>GZaCg-yp7I`042S8(7PIBR
zb!ppxy|<VK?XiseARMQ`5~5!Ox-mQ@$gio47-Jud<CWG@=ns<<Ca;Lh2=4I;7z05u
zoN3Mf0O~1V9>4bjf-dHeC*1NQ5)|}Eg$dMKq`IXQn{(|rN9qMOugB_AjQJ(QMwm4g
zav$@EWEL@thC=wLQ7qju%qOyp-DAf|xcWtt2NeNr_*k{>dAVh^dW*I%zx<eQ<bI|G
zmia&O9U6NP{Lc*vnu$d#IdkzeJI#mlE5W{jiG=+KPz}=%Z@5zo7tD4&!=&_uVV{OB
zx}>phPZG)$U#3py#YA#o<lyre$e@`OT_#cXtpcS05`?ysI^tdh5iJ3*(Tsw!=b1>f
zC~x?VL{(Qy@L~y>6=zO+BfM3O7S3(m25XkMmIxm0mnr&>eh12;#7Gq8lx)dw=HUcQ
zkE{<Oo0uk<sB=NmljLqEKopF<KZ#x-MMGA`uWu6Y(v4JITv#+1L$@`=wpiBL^HwSQ
z5Wa8}R!?6a5~XKOig6W|NGn?)<Xzv0Q^3j>j^;BVOnuCkRyf_uDJm?dO4#2K!o{S1
zAGoS3HJH(*U?YN%rb~;I9mSn-FmmIWh>IGi3oWR+mSrCj?YL5D7MqKl!*ZtJXv7CH
zr6wSyEUIM}hAI+(-r>7-3QSBH<_xKo;egY)dI(FGCk&xwGL(sCW|-xd3?SlBWfnM$
zHInx+^#<L>$GLYbLk-m7Qr*iHG6CDV{6oMSv^k2HDT3XSFj-H<N1-*S`vtNs4OTJX
z3v9VmTqD1mis=R&m;C0n4r=gV`c)2gW>_E&04nHZT(=QW9@3$=0<5as2*RyYJrODX
zWi0`9cgWVRu2nMQ0Jr8&%9cjv5lR=yw>~9NI|k%8QEhDEAQj-;q}rE5nCSeM;D9wH
zOctl=VdqII%{Ft3gslQjg)#Ipz*lz>NCd|=@3<S&jx5|uREiqtW#AVm-XPN2<U0s~
zAhAaofUJ%MxWk8^@sFw_*;0hPAZCpndp%|i61xR;k8q*@#IRKuC>42$sBpaE6(w{+
z`ovu3E@zlHloN695E+a?W?eH7#9cQpLvz{*7c%UEBQOv697?9V!DGWw3nrnDaZIpF
zQYFq~Ogff;i~j%$c_V3K94@9#vnsKvk~udm1sFbKF?z=Bucpta#8FbMJ3>k)#Io6t
zy7q2ar5anhqxp-l3n3Dtkl|m%7k15k$IHX0-q6cYuBf}3e?x1C5rGD(`|1FQV0sFf
z;WS7JRhjTC!{C^5)VhV$K!|!-b;!pw4b1JocNbRz)l+$S!Sg8ypu^H~USglna8i{{
zV+<9(pl?%Aq$7$E7V?_<fB>2gxnz8kM>J<*37RF!IkhetK3bdNfm`gBl7c9de^SRc
zaA_5X5p;aaC8dt;GnuL6Lb$EO-lb-iOUsA+5HhUPuA!7I1;Z2ZIK;)qt57O2xk9lp
zmve}?gkdFhFWWDxQP6vVD>457iJ3`>K`#8poG9w<Z4#Ap&BCsDK}Zr3IaS*Ws3l~A
z^>N%lLA<&62i!d9y9~bsPKWm`%AX7$4$G>8z^3Nfa=VoWPO;q0rA&L%Uigc$?blIM
zB};YO%C)dESlyOtf?G`7v`k3a_|wFx3vqPQPvMGICXiholS9;2P~8`QhzSsgs^c6*
z70X)IFtJAxDS`8FYu%i`+GFVd02__QACs>n4{y_83Qn~!)xJzZR2#jrHn!VU<^e9&
z%o+gA)xW!Cw`DaLFKRpv8Jw`x^%M4Aac;`WHY`ynfM1A;xR{!lsBTcO$$OYezxG9U
zEB*uiDj=AR_bD`7Y-xwM)kdYTVs!#lki<}GR_-=45Mg189FZ!MnBB`tVm8N8pte{t
z{{YIXm6=NF4PY)<B$OsGDYKL#GRhbUNYuGf6so_54)0!YKbwr<SkEP1(hZ#Qzfpy4
zPiVTPFDKj=FQUTn^RN6u3ZvNt6uf^zS&<(wRJl(sKXjkeQ2R}PP;5uwB7E-zSPE#N
zmLA37!dYML<q#CBdzPJ2--sj`Rcfn~#2T=!a)=9CqR~O5HqMP^7npt^4I=COMg?pG
z+^S&;(-zumo?#F*IrtcSLXj7$fZOG#^(v!RG=>|v$o83<Dsuut3^dD^da_$xpJ8lf
zS_~}H;E#Zqu(!p^ifefP0CN!hL|#>ae2%3+x#U6|w>=-yJg2Nv{UcI;8UCOR9;ri_
z@MFOKj4iLkf@bG|8vLNlo-YUYEe1DY-)QSEoK1uCh&hCf3;lH(ihtv&w3o~Jo7KMm
z0MyzK%3{yZ#5jJX(<Bo#HIFdzKI7zl!OfKqXWUfb{LCn|k%w$<8*OG>;g)!1x?;MQ
zc$bI-RH+fT@f^c)gec+w)e?#*VA#>P*d-f|4CN5FGJ(fXlqH1JdB6zZ+I}iIH!W0V
z##`C1L4j6j2<VusIfr+dV{6P?XuFEZmNeet^D6m?LU9h6&8Ie3@RUS-IRlEXH!OjP
z2sxzb!}kgxQ1l>Q6J$3Q4=~(LQ4?qbb?laFAOUa&tMNzbAy$09s0yp&{nQ=ehCi4S
z-NF7DwkxDg7UBgI3M{%$B;)=QaW@b&#6__&wH&;|{{YAb{V%pUfrtg_0`yM<>K1;K
zU;2=CSK}_<){E`E{{U1~-}}5p_x`UCme2U=FaA%&y>mB>@q&LwImeuLBmV%?{@{5Z
zf80j@0LkVUv;BTx)(=q`P)uV%Fj<fH+_E8G>u}2@c|Zc7IX<H7*VMQ8gmi?*ihvFx
z6!$P%?rj-INHg&`OBg-IEWOor4ErL$(6|!7`)0?O2MC4bTR#Nk&b>+-!wl|hn?ter
zjdfrm32d!1l9iTSp~s^U{0KRm&SqW)JmeP3p?+#5MYhp^cM7V*xKS_^F%^<O1o1<F
KA|5^hfB)I_w8jDe

literal 0
HcmV?d00001


From d38f5c3ee79a51d1199c60c004238c4a1496b379 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 28 Jul 2023 12:33:44 +0200
Subject: [PATCH 02/21] Adapt coding style

---
 Sming/Libraries/MPU6050/README.md              |  8 +++-----
 samples/Accel_Gyro_MPU6050/app/application.cpp | 12 ++++++------
 2 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/Sming/Libraries/MPU6050/README.md b/Sming/Libraries/MPU6050/README.md
index 19ff23e2ab..45243771e6 100644
--- a/Sming/Libraries/MPU6050/README.md
+++ b/Sming/Libraries/MPU6050/README.md
@@ -2,8 +2,6 @@
 
 Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
 
-- Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
-- Removed map function in favor of the Sming built-in one.
-- Adapted include path and applied clangformat
-
-
+-   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
+-   Removed map function in favor of the Sming built-in one.
+-   Adapted include path and applied clangformat
diff --git a/samples/Accel_Gyro_MPU6050/app/application.cpp b/samples/Accel_Gyro_MPU6050/app/application.cpp
index d49c231127..1506994989 100644
--- a/samples/Accel_Gyro_MPU6050/app/application.cpp
+++ b/samples/Accel_Gyro_MPU6050/app/application.cpp
@@ -1,11 +1,11 @@
 #include <Libraries/MPU6050/MPU6050.h>
 #include <SmingCore.h>
 
-constexpr float main_loop_interval = 0.02; // sec
-SimpleTimer main_loop_timer;
+constexpr float mainLoopInterval = 0.02; // sec
+SimpleTimer mainLoopTimer;
 MPU6050 mpu;
 
-void print_accel_gyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
+void printAccelGyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
 {
 	Serial.print("a/g:\t");
 	Serial.print(ax);
@@ -21,12 +21,12 @@ void print_accel_gyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy
 	Serial.println(gz);
 }
 
-void main_loop()
+void mainLoop()
 {
 	int16_t ax, ay, az;
 	int16_t gx, gy, gz;
 	mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-	print_accel_gyro(ax, ay, az, gx, gy, gz);
+	printAccelGyro(ax, ay, az, gx, gy, gz);
 }
 
 void init()
@@ -38,5 +38,5 @@ void init()
 	mpu.initialize();
 	Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
 
-	main_loop_timer.initializeMs(static_cast<int>(main_loop_interval * 1000), main_loop).start();
+	mainLoopTimer.initializeMs(static_cast<int>(mainLoopInterval * 1000), mainLoop).start();
 }

From 1a1c3ced0880ab346272e06f417146c428b7432f Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 28 Jul 2023 15:01:19 +0200
Subject: [PATCH 03/21] Refactor application.cpp for MPU6050 sample

---
 samples/Accel_Gyro_MPU6050/app/application.cpp | 17 +++--------------
 1 file changed, 3 insertions(+), 14 deletions(-)

diff --git a/samples/Accel_Gyro_MPU6050/app/application.cpp b/samples/Accel_Gyro_MPU6050/app/application.cpp
index 1506994989..4bd118f8a4 100644
--- a/samples/Accel_Gyro_MPU6050/app/application.cpp
+++ b/samples/Accel_Gyro_MPU6050/app/application.cpp
@@ -1,24 +1,13 @@
 #include <Libraries/MPU6050/MPU6050.h>
 #include <SmingCore.h>
 
-constexpr float mainLoopInterval = 0.02; // sec
+constexpr uint16_t mainLoopInterval = 20; // ms
 SimpleTimer mainLoopTimer;
 MPU6050 mpu;
 
 void printAccelGyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
 {
-	Serial.print("a/g:\t");
-	Serial.print(ax);
-	Serial.print("\t");
-	Serial.print(ay);
-	Serial.print("\t");
-	Serial.print(az);
-	Serial.print("\t");
-	Serial.print(gx);
-	Serial.print("\t");
-	Serial.print(gy);
-	Serial.print("\t");
-	Serial.println(gz);
+	Serial << "a/g:\t" << ax << "\t" << ay << "\t" << az << "\t" << gx << "\t" << gy << "\t" << gz << endl;
 }
 
 void mainLoop()
@@ -38,5 +27,5 @@ void init()
 	mpu.initialize();
 	Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
 
-	mainLoopTimer.initializeMs(static_cast<int>(mainLoopInterval * 1000), mainLoop).start();
+	mainLoopTimer.initializeMs<mainLoopInterval>(mainLoop).start();
 }

From 271296abd1b2298eaabb57a4667336ab1c13b541 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 28 Jul 2023 15:09:38 +0200
Subject: [PATCH 04/21] Delete quaternion related code

The code is not used, since MPU6050_6Axis_MotionApps20.h and
MPU6050_9Axis_MotionApps41.h due to deps to freeRTOS are not included
---
 Sming/Libraries/MPU6050/MPU6050.cpp     |   1 -
 Sming/Libraries/MPU6050/MPU6050.h       | 205 --------------------
 Sming/Libraries/MPU6050/helper_3dmath.h | 243 ------------------------
 3 files changed, 449 deletions(-)
 delete mode 100644 Sming/Libraries/MPU6050/helper_3dmath.h

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 5d174ab7a1..d6cf680d01 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -3221,7 +3221,6 @@ void MPU6050::setIntDMPEnabled(bool enabled)
 }
 
 // DMP_INT_STATUS
-
 bool MPU6050::getDMPInt5Status()
 {
 	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 0e0bda8f62..bba931d1d3 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -38,7 +38,6 @@ THE SOFTWARE.
 #ifndef _MPU6050_H_
 #define _MPU6050_H_
 
-#include "helper_3dmath.h"
 #include <Libraries/I2Cdev/I2Cdev.h>
 
 // supporting link:
@@ -54,7 +53,6 @@ THE SOFTWARE.
 //#define pgm_read_float(x) (*(x))
 //#define PSTR(STR) STR
 
-#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
 #define MPU6050_ADDRESS_AD0_LOW 0x68  // address pin low (GND), default for InvenSense evaluation board
 #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
 #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
@@ -827,209 +825,6 @@ class MPU6050
 	uint8_t getDMPConfig2();
 	void setDMPConfig2(uint8_t config);
 
-// special methods for MotionApps 2.0 implementation
-#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
-	uint8_t* dmpPacketBuffer;
-	uint16_t dmpPacketSize;
-
-	uint8_t dmpInitialize();
-	bool dmpPacketAvailable();
-
-	uint8_t dmpSetFIFORate(uint8_t fifoRate);
-	uint8_t dmpGetFIFORate();
-	uint8_t dmpGetSampleStepSizeMS();
-	uint8_t dmpGetSampleFrequency();
-	int32_t dmpDecodeTemperature(int8_t tempReg);
-
-	// Register callbacks after a packet of FIFO data is processed
-	// uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
-	// uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
-	uint8_t dmpRunFIFORateProcesses();
-
-	// Setup FIFO for various output
-	uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
-	uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
-	uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
-
-	// Get Fixed Point data from FIFO
-	uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
-	uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity);
-	uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q);
-	uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);
-	uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0);
-	uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0);
-
-	uint8_t dmpGetEuler(float* data, Quaternion* q);
-	uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
-
-	// Get Floating Point data from FIFO
-	uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0);
-
-	uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData);
-	uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL);
-
-	uint8_t dmpSetFIFOProcessedCallback(void (*func)(void));
-
-	uint8_t dmpInitFIFOParam();
-	uint8_t dmpCloseFIFO();
-	uint8_t dmpSetGyroDataSource(uint8_t source);
-	uint8_t dmpDecodeQuantizedAccel();
-	uint32_t dmpGetGyroSumOfSquare();
-	uint32_t dmpGetAccelSumOfSquare();
-	void dmpOverrideQuaternion(long* q);
-	uint16_t dmpGetFIFOPacketSize();
-#endif
-
-// special methods for MotionApps 4.1 implementation
-#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
-	uint8_t* dmpPacketBuffer;
-	uint16_t dmpPacketSize;
-
-	uint8_t dmpInitialize();
-	bool dmpPacketAvailable();
-
-	uint8_t dmpSetFIFORate(uint8_t fifoRate);
-	uint8_t dmpGetFIFORate();
-	uint8_t dmpGetSampleStepSizeMS();
-	uint8_t dmpGetSampleFrequency();
-	int32_t dmpDecodeTemperature(int8_t tempReg);
-
-	// Register callbacks after a packet of FIFO data is processed
-	// uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
-	// uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
-	uint8_t dmpRunFIFORateProcesses();
-
-	// Setup FIFO for various output
-	uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
-	uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
-	uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
-	uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
-
-	// Get Fixed Point data from FIFO
-	uint8_t dmpGetAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternion(Quaternion* q, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGet6AxisQuaternion(Quaternion* q, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetRelativeQuaternion(Quaternion* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyro(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetMag(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
-	uint8_t dmpGetLinearAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccel(VectorInt16* v, VectorInt16* vRaw, VectorFloat* gravity);
-	uint8_t dmpGetLinearAccelInWorld(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetLinearAccelInWorld(VectorInt16* v, VectorInt16* vReal, Quaternion* q);
-	uint8_t dmpGetGyroAndAccelSensor(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroAndAccelSensor(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroAndAccelSensor(VectorInt16* g, VectorInt16* a, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGyroSensor(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetControlData(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetTemperature(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetGravity(VectorFloat* v, Quaternion* q);
-	uint8_t dmpGetUnquantizedAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetUnquantizedAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetUnquantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(int32_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(int16_t* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuantizedAccel(VectorInt16* v, const uint8_t* packet = 0);
-	uint8_t dmpGetExternalSensorData(int32_t* data, uint16_t size, const uint8_t* packet = 0);
-	uint8_t dmpGetEIS(int32_t* data, const uint8_t* packet = 0);
-
-	uint8_t dmpGetEuler(float* data, Quaternion* q);
-	uint8_t dmpGetYawPitchRoll(float* data, Quaternion* q, VectorFloat* gravity);
-
-	// Get Floating Point data from FIFO
-	uint8_t dmpGetAccelFloat(float* data, const uint8_t* packet = 0);
-	uint8_t dmpGetQuaternionFloat(float* data, const uint8_t* packet = 0);
-
-	uint8_t dmpProcessFIFOPacket(const unsigned char* dmpData);
-	uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t* processed = NULL);
-
-	uint8_t dmpSetFIFOProcessedCallback(void (*func)(void));
-
-	uint8_t dmpInitFIFOParam();
-	uint8_t dmpCloseFIFO();
-	uint8_t dmpSetGyroDataSource(uint8_t source);
-	uint8_t dmpDecodeQuantizedAccel();
-	uint32_t dmpGetGyroSumOfSquare();
-	uint32_t dmpGetAccelSumOfSquare();
-	void dmpOverrideQuaternion(long* q);
-	uint16_t dmpGetFIFOPacketSize();
-#endif
-
 	void CalibrateGyro(uint8_t Loops = 15);  // Fine tune after setting offsets with less Loops.
 	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
 	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
diff --git a/Sming/Libraries/MPU6050/helper_3dmath.h b/Sming/Libraries/MPU6050/helper_3dmath.h
deleted file mode 100644
index 4e8ac2d51b..0000000000
--- a/Sming/Libraries/MPU6050/helper_3dmath.h
+++ /dev/null
@@ -1,243 +0,0 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D
-// math helper 6/5/2012 by Jeff Rowberg <jeff@rowberg.net> Updates should
-// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-//     2012-06-05 - add 3D math helper file to DMP6 example sketch
-
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
-
-#ifndef _HELPER_3DMATH_H_
-#define _HELPER_3DMATH_H_
-#include <math.h>
-#include <stdint.h>
-
-class Quaternion
-{
-public:
-	float w;
-	float x;
-	float y;
-	float z;
-
-	Quaternion()
-	{
-		w = 1.0f;
-		x = 0.0f;
-		y = 0.0f;
-		z = 0.0f;
-	}
-
-	Quaternion(float nw, float nx, float ny, float nz)
-	{
-		w = nw;
-		x = nx;
-		y = ny;
-		z = nz;
-	}
-
-	Quaternion getProduct(Quaternion q)
-	{
-		// Quaternion multiplication is defined by:
-		//     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
-		//     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
-		//     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
-		//     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
-		return Quaternion(w * q.w - x * q.x - y * q.y - z * q.z,  // new w
-						  w * q.x + x * q.w + y * q.z - z * q.y,  // new x
-						  w * q.y - x * q.z + y * q.w + z * q.x,  // new y
-						  w * q.z + x * q.y - y * q.x + z * q.w); // new z
-	}
-
-	Quaternion getConjugate()
-	{
-		return Quaternion(w, -x, -y, -z);
-	}
-
-	float getMagnitude()
-	{
-		return sqrt(w * w + x * x + y * y + z * z);
-	}
-
-	void normalize()
-	{
-		float m = getMagnitude();
-		w /= m;
-		x /= m;
-		y /= m;
-		z /= m;
-	}
-
-	Quaternion getNormalized()
-	{
-		Quaternion r(w, x, y, z);
-		r.normalize();
-		return r;
-	}
-};
-
-class VectorInt16
-{
-public:
-	int16_t x;
-	int16_t y;
-	int16_t z;
-
-	VectorInt16()
-	{
-		x = 0;
-		y = 0;
-		z = 0;
-	}
-
-	VectorInt16(int16_t nx, int16_t ny, int16_t nz)
-	{
-		x = nx;
-		y = ny;
-		z = nz;
-	}
-
-	float getMagnitude()
-	{
-		return sqrt(x * x + y * y + z * z);
-	}
-
-	void normalize()
-	{
-		float m = getMagnitude();
-		x /= m;
-		y /= m;
-		z /= m;
-	}
-
-	VectorInt16 getNormalized()
-	{
-		VectorInt16 r(x, y, z);
-		r.normalize();
-		return r;
-	}
-
-	void rotate(Quaternion* q)
-	{
-		// http://www.cprogramming.com/tutorial/3d/quaternions.html
-		// http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
-		// http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
-		// ^ or:
-		// http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
-
-		// P_out = q * P_in * conj(q)
-		// - P_out is the output vector
-		// - q is the orientation quaternion
-		// - P_in is the input vector (a*aReal)
-		// - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z],
-		// q*=[w,-x,-y,-z])
-		Quaternion p(0, x, y, z);
-
-		// quaternion multiplication: q * p, stored back in p
-		p = q->getProduct(p);
-
-		// quaternion multiplication: p * conj(q), stored back in p
-		p = p.getProduct(q->getConjugate());
-
-		// p quaternion is now [0, x', y', z']
-		x = p.x;
-		y = p.y;
-		z = p.z;
-	}
-
-	VectorInt16 getRotated(Quaternion* q)
-	{
-		VectorInt16 r(x, y, z);
-		r.rotate(q);
-		return r;
-	}
-};
-
-class VectorFloat
-{
-public:
-	float x;
-	float y;
-	float z;
-
-	VectorFloat()
-	{
-		x = 0;
-		y = 0;
-		z = 0;
-	}
-
-	VectorFloat(float nx, float ny, float nz)
-	{
-		x = nx;
-		y = ny;
-		z = nz;
-	}
-
-	float getMagnitude()
-	{
-		return sqrt(x * x + y * y + z * z);
-	}
-
-	void normalize()
-	{
-		float m = getMagnitude();
-		x /= m;
-		y /= m;
-		z /= m;
-	}
-
-	VectorFloat getNormalized()
-	{
-		VectorFloat r(x, y, z);
-		r.normalize();
-		return r;
-	}
-
-	void rotate(Quaternion* q)
-	{
-		Quaternion p(0, x, y, z);
-
-		// quaternion multiplication: q * p, stored back in p
-		p = q->getProduct(p);
-
-		// quaternion multiplication: p * conj(q), stored back in p
-		p = p.getProduct(q->getConjugate());
-
-		// p quaternion is now [0, x', y', z']
-		x = p.x;
-		y = p.y;
-		z = p.z;
-	}
-
-	VectorFloat getRotated(Quaternion* q)
-	{
-		VectorFloat r(x, y, z);
-		r.rotate(q);
-		return r;
-	}
-};
-
-#endif /* _HELPER_3DMATH_H_ */

From 7e86618065a0b5d6fc3e3832b112f1661ddf8384 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 28 Jul 2023 15:50:12 +0200
Subject: [PATCH 05/21] Refactor coding style in MPU6050 library

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 27 +++++++++++----------------
 Sming/Libraries/MPU6050/MPU6050.h   |  4 ++--
 Sming/Libraries/MPU6050/README.md   |  3 ++-
 3 files changed, 15 insertions(+), 19 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index d6cf680d01..f09aa7653e 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -43,9 +43,8 @@ THE SOFTWARE.
 /** Default constructor, uses default I2C address.
  * @see MPU6050_DEFAULT_ADDRESS
  */
-MPU6050::MPU6050()
+MPU6050::MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
 {
-	devAddr = MPU6050_DEFAULT_ADDRESS;
 }
 
 /** Specific address constructor.
@@ -54,9 +53,8 @@ MPU6050::MPU6050()
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address)
+MPU6050::MPU6050(uint8_t address) : devAddr{address}
 {
-	devAddr = address;
 }
 
 /** Power on and prepare for general usage.
@@ -3315,10 +3313,9 @@ void MPU6050::readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank, ui
 {
 	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_DMP_MEMORY_CHUNK_SIZE;
+		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
 		if(i + chunkSize > dataSize)
@@ -3351,18 +3348,17 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 {
 	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_DMP_MEMORY_CHUNK_SIZE);
+		verifyBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
 	if(useProgMem)
-		progBuffer = (uint8_t*)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+		progBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
 	for(i = 0; i < dataSize;) {
 		// determine correct chunk size according to bank position and data size
-		chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
+		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
 		if(i + chunkSize > dataSize)
@@ -3378,7 +3374,7 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 				progBuffer[j] = pgm_read_byte(data + i + j);
 		} else {
 			// write the chunk of data as specified
-			progBuffer = (uint8_t*)data + i;
+			progBuffer = const_cast<uint8_t*>(data) + i;
 		}
 
 		I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
@@ -3440,7 +3436,7 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 	uint8_t success, special;
 	uint16_t i, j;
 	if(useProgMem) {
-		progBuffer = (uint8_t*)malloc(8); // assume 8-byte blocks, realloc later if necessary
+		progBuffer = static_cast<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:
@@ -3468,11 +3464,11 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
       Serial.println(length);*/
 			if(useProgMem) {
 				if(sizeof(progBuffer) < length)
-					progBuffer = (uint8_t*)realloc(progBuffer, length);
+					progBuffer = static_cast<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;
+				progBuffer = const_cast<uint8_t*>(data) + i;
 			}
 			success = writeMemoryBlock(progBuffer, length, bank, offset, true);
 			i += length;
@@ -3596,7 +3592,6 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 	int16_t BitZero[3];
 	uint8_t shift = (SaveAddress == 0x77) ? 3 : 2;
 	float Error, PTerm, ITerm[3];
-	int16_t eSample;
 	uint32_t eSum;
 	for(int i = 0; i < 3; i++) {
 		I2Cdev::readWord(devAddr, SaveAddress + (i * shift),
@@ -3610,7 +3605,7 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 		}
 	}
 	for(int L = 0; L < Loops; L++) {
-		eSample = 0;
+		int16_t eSample{0};
 		for(int c = 0; c < 100; c++) { // 100 PI Calculations
 			eSum = 0;
 			for(int i = 0; i < 3; i++) {
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index bba931d1d3..cc2fe0e1e7 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -45,7 +45,7 @@ THE SOFTWARE.
 // http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
 
 #undef pgm_read_byte
-#define pgm_read_byte(addr) (*(const unsigned char*)(addr))
+#define pgm_read_byte(addr) (*static_cast<const unsigned char*>(addr))
 
 //#define PROGMEM /* empty */
 //#define pgm_read_byte(x) (*(x))
@@ -831,7 +831,7 @@ class MPU6050
 
 private:
 	uint8_t devAddr;
-	uint8_t buffer[14];
+	uint8_t buffer[14] = {0};
 };
 
 #endif /* _MPU6050_H_ */
diff --git a/Sming/Libraries/MPU6050/README.md b/Sming/Libraries/MPU6050/README.md
index 45243771e6..68c01c3f30 100644
--- a/Sming/Libraries/MPU6050/README.md
+++ b/Sming/Libraries/MPU6050/README.md
@@ -3,5 +3,6 @@
 Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
 
 -   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
+-   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.
 -   Removed map function in favor of the Sming built-in one.
--   Adapted include path and applied clangformat
+-   Adapted include path, coding style and applied clangformat

From 9ec0afb7ffd8564638b6e0f60fc7cc3e4d7b0607 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 8 Sep 2023 11:40:10 +0200
Subject: [PATCH 06/21] Introduce readByte and readBits helpers

Move trival functions to header
---
 Sming/Libraries/MPU6050/MPU6050.cpp  | 210 ++++++++++-----------------
 Sming/Libraries/MPU6050/MPU6050.h    |  39 +++--
 Sming/Libraries/MPU6050/component.mk |   1 +
 3 files changed, 110 insertions(+), 140 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index f09aa7653e..5b9fda4a3a 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -40,23 +40,6 @@ THE SOFTWARE.
 
 #define I2C_NUM I2C_NUM_0
 
-/** Default constructor, uses default I2C address.
- * @see MPU6050_DEFAULT_ADDRESS
- */
-MPU6050::MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
-{
-}
-
-/** Specific address constructor.
- * @param address I2C address
- * @see MPU6050_DEFAULT_ADDRESS
- * @see MPU6050_ADDRESS_AD0_LOW
- * @see MPU6050_ADDRESS_AD0_HIGH
- */
-MPU6050::MPU6050(uint8_t address) : devAddr{address}
-{
-}
-
 /** Power on and prepare for general usage.
  * This will activate the device and take it out of sleep mode (which must be
  * done after start-up). This function also sets both the accelerometer and the
@@ -72,15 +55,6 @@ void MPU6050::initialize()
 	setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
 }
 
-/** Verify the I2C connection.
- * Make sure the device is connected and responds as expected.
- * @return True if connection is valid, false otherwise
- */
-bool MPU6050::testConnection()
-{
-	return getDeviceID() == 0x34;
-}
-
 // AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
 
 /** Get the auxiliary I2C supply voltage level.
@@ -130,8 +104,7 @@ void MPU6050::setAuxVDDIOLevel(uint8_t level)
  */
 uint8_t MPU6050::getRate()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_SMPLRT_DIV);
 }
 /** Set gyroscope sample rate divider.
  * @param rate New sample rate divider
@@ -174,8 +147,7 @@ void MPU6050::setRate(uint8_t rate)
  */
 uint8_t MPU6050::getExternalFrameSync()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH);
 }
 /** Set external FSYNC configuration.
  * @see getExternalFrameSync()
@@ -216,7 +188,7 @@ void MPU6050::setExternalFrameSync(uint8_t sync)
  */
 uint8_t MPU6050::getDLPFMode()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer);
+	return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH);
 	return buffer[0];
 }
 /** Set digital low-pass filter configuration.
@@ -253,9 +225,7 @@ void MPU6050::setDLPFMode(uint8_t mode)
  */
 uint8_t MPU6050::getFullScaleGyroRange()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH);
 }
 /** Set full-scale gyroscope range.
  * @param range New full-scale gyroscope range value
@@ -279,9 +249,9 @@ void MPU6050::setFullScaleGyroRange(uint8_t range)
  */
 uint8_t MPU6050::getAccelXSelfTestFactoryTrim()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0]);
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);
-	return (buffer[0] >> 3) | ((buffer[1] >> 4) & 0x03);
+	const uint8_t x = readByte(MPU6050_RA_SELF_TEST_X);
+	const uint8_t a = readByte(MPU6050_RA_SELF_TEST_A);
+	return (x >> 3) | ((a >> 4) & 0x03);
 }
 
 /** Get self-test factory trim value for accelerometer Y axis.
@@ -290,9 +260,9 @@ uint8_t MPU6050::getAccelXSelfTestFactoryTrim()
  */
 uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0]);
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1]);
-	return (buffer[0] >> 3) | ((buffer[1] >> 2) & 0x03);
+	const uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
+	const uint8_t a = readByte(MPU6050_RA_SELF_TEST_A);
+	return (y >> 3) | ((a >> 2) & 0x03);
 }
 
 /** Get self-test factory trim value for accelerometer Z axis.
@@ -311,8 +281,8 @@ uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
  */
 uint8_t MPU6050::getGyroXSelfTestFactoryTrim()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer);
-	return (buffer[0] & 0x1F);
+	const uint8_t x = readByte(MPU6050_RA_SELF_TEST_X);
+	return (x & 0x1F);
 }
 
 /** Get self-test factory trim value for gyro Y axis.
@@ -321,8 +291,8 @@ uint8_t MPU6050::getGyroXSelfTestFactoryTrim()
  */
 uint8_t MPU6050::getGyroYSelfTestFactoryTrim()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer);
-	return (buffer[0] & 0x1F);
+	const uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
+	return (y & 0x1F);
 }
 
 /** Get self-test factory trim value for gyro Z axis.
@@ -331,8 +301,8 @@ uint8_t MPU6050::getGyroYSelfTestFactoryTrim()
  */
 uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer);
-	return (buffer[0] & 0x1F);
+	const uint8_t z = readByte(MPU6050_RA_SELF_TEST_Z);
+	return (z & 0x1F);
 }
 
 // ACCEL_CONFIG register
@@ -407,9 +377,7 @@ void MPU6050::setAccelZSelfTest(bool enabled)
  */
 uint8_t MPU6050::getFullScaleAccelRange()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH);
 }
 /** Set full-scale accelerometer range.
  * @param range New full-scale accelerometer range setting
@@ -457,9 +425,7 @@ void MPU6050::setFullScaleAccelRange(uint8_t range)
  */
 uint8_t MPU6050::getDHPFMode()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH);
 }
 /** Set the high-pass filter configuration.
  * @param bandwidth New high-pass filter configuration
@@ -492,8 +458,7 @@ void MPU6050::setDHPFMode(uint8_t bandwidth)
  */
 uint8_t MPU6050::getFreefallDetectionThreshold()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_FF_THR);
 }
 /** Get free-fall event acceleration threshold.
  * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
@@ -526,8 +491,7 @@ void MPU6050::setFreefallDetectionThreshold(uint8_t threshold)
  */
 uint8_t MPU6050::getFreefallDetectionDuration()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_FF_DUR);
 }
 /** Get free-fall event duration threshold.
  * @param duration New free-fall duration threshold value (LSB = 1ms)
@@ -562,8 +526,7 @@ void MPU6050::setFreefallDetectionDuration(uint8_t duration)
  */
 uint8_t MPU6050::getMotionDetectionThreshold()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_MOT_THR);
 }
 /** Set motion detection event acceleration threshold.
  * @param threshold New motion detection acceleration threshold value (LSB =
@@ -595,8 +558,7 @@ void MPU6050::setMotionDetectionThreshold(uint8_t threshold)
  */
 uint8_t MPU6050::getMotionDetectionDuration()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_MOT_DUR);
 }
 /** Set motion detection event duration threshold.
  * @param duration New motion detection duration threshold value (LSB = 1ms)
@@ -638,8 +600,7 @@ void MPU6050::setMotionDetectionDuration(uint8_t duration)
  */
 uint8_t MPU6050::getZeroMotionDetectionThreshold()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_ZRMOT_THR);
 }
 /** Set zero motion detection event acceleration threshold.
  * @param threshold New zero motion detection acceleration threshold value (LSB
@@ -672,8 +633,7 @@ void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold)
  */
 uint8_t MPU6050::getZeroMotionDetectionDuration()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_ZRMOT_DUR);
 }
 /** Set zero motion detection event duration threshold.
  * @param duration New zero motion detection duration threshold value (LSB =
@@ -981,8 +941,7 @@ void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled)
  */
 uint8_t MPU6050::getMasterClockSpeed()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH);
 }
 /** Set I2C master clock speed.
  * @reparam speed Current I2C master clock speed
@@ -1038,10 +997,10 @@ void MPU6050::setMasterClockSpeed(uint8_t speed)
  */
 uint8_t MPU6050::getSlaveAddress(uint8_t num)
 {
-	if(num > 3)
+	if(num > 3) {
 		return 0;
-	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, buffer);
-	return buffer[0];
+	}
+	return readByte(MPU6050_RA_I2C_SLV0_ADDR + num * 3);
 }
 /** Set the I2C address of the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1051,8 +1010,9 @@ uint8_t MPU6050::getSlaveAddress(uint8_t num)
  */
 void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address);
 }
 /** Get the active internal register for the specified slave (0-3).
@@ -1068,10 +1028,10 @@ void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
  */
 uint8_t MPU6050::getSlaveRegister(uint8_t num)
 {
-	if(num > 3)
+	if(num > 3) {
 		return 0;
-	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, buffer);
-	return buffer[0];
+	}
+	return readByte(MPU6050_RA_I2C_SLV0_REG + num * 3);
 }
 /** Set the active internal register for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1081,8 +1041,9 @@ uint8_t MPU6050::getSlaveRegister(uint8_t num)
  */
 void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg);
 }
 /** Get the enabled value for the specified slave (0-3).
@@ -1213,9 +1174,7 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num)
 {
 	if(num > 3)
 		return 0;
-	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
 }
 /** Set number of bytes to read for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1244,8 +1203,7 @@ void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
  */
 uint8_t MPU6050::getSlave4Address()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_I2C_SLV4_ADDR);
 }
 /** Set the I2C address of Slave 4.
  * @param address New address for Slave 4
@@ -1265,8 +1223,7 @@ void MPU6050::setSlave4Address(uint8_t address)
  */
 uint8_t MPU6050::getSlave4Register()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_I2C_SLV4_REG);
 }
 /** Set the active internal register for Slave 4.
  * @param reg New active register for Slave 4
@@ -1372,9 +1329,7 @@ void MPU6050::setSlave4WriteMode(bool mode)
  */
 uint8_t MPU6050::getSlave4MasterDelay()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH);
 }
 /** Set Slave 4 master delay value.
  * @param delay New Slave 4 master delay value
@@ -1394,8 +1349,7 @@ void MPU6050::setSlave4MasterDelay(uint8_t delay)
  */
 uint8_t MPU6050::getSlate4InputByte()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_I2C_SLV4_DI);
 }
 
 // I2C_MST_STATUS register
@@ -1698,8 +1652,7 @@ void MPU6050::setClockOutputEnabled(bool enabled)
  **/
 uint8_t MPU6050::getIntEnabled()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_INT_ENABLE);
 }
 /** Set full interrupt enabled status.
  * Full register byte for all interrupts, for quick reading. Each bit should be
@@ -1853,8 +1806,7 @@ void MPU6050::setIntDataReadyEnabled(bool enabled)
  */
 uint8_t MPU6050::getIntStatus()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_INT_STATUS);
 }
 /** Get Free Fall interrupt status.
  * This bit automatically sets to 1 when a Free Fall interrupt has been
@@ -2212,8 +2164,7 @@ int16_t MPU6050::getRotationZ()
  */
 uint8_t MPU6050::getExternalSensorByte(int position)
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_EXT_SENS_DATA_00 + position);
 }
 /** Read word (2 bytes) from external sensor data registers.
  * @param position Starting position (0-21)
@@ -2244,8 +2195,7 @@ uint32_t MPU6050::getExternalSensorDWord(int position)
  */
 uint8_t MPU6050::getMotionStatus()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_MOT_DETECT_STATUS);
 }
 /** Get X-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2449,9 +2399,8 @@ void MPU6050::resetTemperaturePath()
  */
 uint8_t MPU6050::getAccelerometerPowerOnDelay()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
-					 MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+					MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH);
 }
 /** Set accelerometer power-on delay.
  * @param delay New accelerometer power-on delay (0-3)
@@ -2492,9 +2441,7 @@ void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay)
  */
 uint8_t MPU6050::getFreefallDetectionCounterDecrement()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH);
 }
 /** Set Free Fall detection counter decrement configuration.
  * @param decrement New decrement configuration value
@@ -2532,9 +2479,7 @@ void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement)
  */
 uint8_t MPU6050::getMotionDetectionCounterDecrement()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH);
 }
 /** Set Motion detection counter decrement configuration.
  * @param decrement New decrement configuration value
@@ -2743,8 +2688,7 @@ void MPU6050::setTempSensorEnabled(bool enabled)
  */
 uint8_t MPU6050::getClockSource()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH);
 }
 /** Set clock source setting.
  * An internal 8MHz oscillator, gyroscope based clock, or external sources can
@@ -2809,9 +2753,7 @@ void MPU6050::setClockSource(uint8_t source)
  */
 uint8_t MPU6050::getWakeFrequency()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
-					 buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH);
 }
 /** Set wake frequency in Accel-Only Low Power Mode.
  * @param frequency New wake frequency
@@ -2994,8 +2936,7 @@ uint16_t MPU6050::getFIFOCount()
  */
 uint8_t MPU6050::getFIFOByte()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_FIFO_R_W);
 }
 void MPU6050::getFIFOBytes(uint8_t* data, uint8_t length)
 {
@@ -3025,8 +2966,7 @@ void MPU6050::setFIFOByte(uint8_t data)
  */
 uint8_t MPU6050::getDeviceID()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH);
 }
 /** Set Device ID.
  * Write a new ID into the WHO_AM_I register (no idea why this should ever be
@@ -3057,8 +2997,7 @@ void MPU6050::setOTPBankValid(bool enabled)
 }
 int8_t MPU6050::getXGyroOffsetTC()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
 }
 void MPU6050::setXGyroOffsetTC(int8_t offset)
 {
@@ -3069,8 +3008,7 @@ void MPU6050::setXGyroOffsetTC(int8_t offset)
 
 int8_t MPU6050::getYGyroOffsetTC()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
 }
 void MPU6050::setYGyroOffsetTC(int8_t offset)
 {
@@ -3081,8 +3019,7 @@ void MPU6050::setYGyroOffsetTC(int8_t offset)
 
 int8_t MPU6050::getZGyroOffsetTC()
 {
-	I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer);
-	return buffer[0];
+	return readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
 }
 void MPU6050::setZGyroOffsetTC(int8_t offset)
 {
@@ -3093,8 +3030,7 @@ void MPU6050::setZGyroOffsetTC(int8_t offset)
 
 int8_t MPU6050::getXFineGain()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_X_FINE_GAIN);
 }
 void MPU6050::setXFineGain(int8_t gain)
 {
@@ -3105,8 +3041,7 @@ void MPU6050::setXFineGain(int8_t gain)
 
 int8_t MPU6050::getYFineGain()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_Y_FINE_GAIN);
 }
 void MPU6050::setYFineGain(int8_t gain)
 {
@@ -3117,8 +3052,7 @@ void MPU6050::setYFineGain(int8_t gain)
 
 int8_t MPU6050::getZFineGain()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_Z_FINE_GAIN);
 }
 void MPU6050::setZFineGain(int8_t gain)
 {
@@ -3302,13 +3236,14 @@ void MPU6050::setMemoryStartAddress(uint8_t address)
 
 uint8_t MPU6050::readMemoryByte()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_MEM_R_W);
 }
+
 void MPU6050::writeMemoryByte(uint8_t data)
 {
 	I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
 }
+
 void MPU6050::readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank, uint8_t address)
 {
 	setMemoryBank(bank);
@@ -3521,21 +3456,20 @@ bool MPU6050::writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSiz
 
 uint8_t MPU6050::getDMPConfig1()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_DMP_CFG_1);
 }
+
 void MPU6050::setDMPConfig1(uint8_t config)
 {
 	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
 }
 
 // DMP_CFG_2 register
-
 uint8_t MPU6050::getDMPConfig2()
 {
-	I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer);
-	return buffer[0];
+	return readByte(MPU6050_RA_DMP_CFG_2);
 }
+
 void MPU6050::setDMPConfig2(uint8_t config)
 {
 	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
@@ -3648,3 +3582,17 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 	resetFIFO();
 	resetDMP();
 }
+
+uint8_t MPU6050::readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length)
+{
+	uint8_t bits;
+	I2Cdev::readBits(devAddr, regAddr, bitStart, length, &bits);
+	return bits;
+}
+
+uint8_t MPU6050::readByte(uint8_t regAddr)
+{
+	uint8_t byte;
+	I2Cdev::readByte(devAddr, regAddr, &byte);
+	return byte;
+}
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index cc2fe0e1e7..bb7f625d4e 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -35,8 +35,7 @@ THE SOFTWARE.
 ===============================================
 */
 
-#ifndef _MPU6050_H_
-#define _MPU6050_H_
+#pragma once
 
 #include <Libraries/I2Cdev/I2Cdev.h>
 
@@ -431,16 +430,36 @@ THE SOFTWARE.
 #define MPU6050_DMP_MEMORY_BANK_SIZE 256
 #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16
 
-// note: DMP code memory blocks defined at end of header file
-
 class MPU6050
 {
 public:
-	MPU6050();
-	MPU6050(uint8_t address);
+	/** Default constructor, uses default I2C address.
+     * @see MPU6050_DEFAULT_ADDRESS
+     */
+	MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
+	{
+	}
+
+	/** Specific address constructor.
+     * @param address I2C address
+     * @see MPU6050_DEFAULT_ADDRESS
+     * @see MPU6050_ADDRESS_AD0_LOW
+     * @see MPU6050_ADDRESS_AD0_HIGH
+     */
+	MPU6050(uint8_t address) : devAddr{address}
+	{
+	}
 
 	void initialize();
-	bool testConnection();
+
+	/** Verify the I2C connection.
+     * Make sure the device is connected and responds as expected.
+     * @return True if connection is valid, false otherwise
+     */
+	bool testConnection()
+	{
+		return getDeviceID() == 0x34;
+	}
 
 	// AUX_VDDIO register
 	uint8_t getAuxVDDIOLevel();
@@ -829,9 +848,11 @@ class MPU6050
 	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
 	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
 
+	// I2C helpers
+	uint8_t readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length);
+	uint8_t readByte(uint8_t regAddr);
+
 private:
 	uint8_t devAddr;
 	uint8_t buffer[14] = {0};
 };
-
-#endif /* _MPU6050_H_ */
diff --git a/Sming/Libraries/MPU6050/component.mk b/Sming/Libraries/MPU6050/component.mk
index 725f8abd75..2416b727f3 100644
--- a/Sming/Libraries/MPU6050/component.mk
+++ b/Sming/Libraries/MPU6050/component.mk
@@ -1 +1,2 @@
+COMPONENT_DOXYGEN_INPUT := .
 COMPONENT_DEPENDS := I2Cdev

From 0adb5b2c1eb56ad96c1c069deee9adb43a2d2509 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 8 Sep 2023 12:11:59 +0200
Subject: [PATCH 07/21] Refactor braces and make helpers private

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 54 ++++++++++++++++++-----------
 Sming/Libraries/MPU6050/MPU6050.h   |  2 +-
 2 files changed, 35 insertions(+), 21 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 5b9fda4a3a..8be8a446d0 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -1055,8 +1055,9 @@ void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
  */
 bool MPU6050::getSlaveEnabled(uint8_t num)
 {
-	if(num > 3)
-		return 0;
+	if(num > 3) {
+		return false;
+	}
 	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
 	return buffer[0];
 }
@@ -1068,8 +1069,9 @@ bool MPU6050::getSlaveEnabled(uint8_t num)
  */
 void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
 }
 /** Get word pair byte-swapping enabled for the specified slave (0-3).
@@ -1085,8 +1087,9 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
  */
 bool MPU6050::getSlaveWordByteSwap(uint8_t num)
 {
-	if(num > 3)
-		return 0;
+	if(num > 3) {
+		return false;
+	}
 	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
 	return buffer[0];
 }
@@ -1098,8 +1101,9 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num)
  */
 void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
 }
 /** Get write mode for the specified slave (0-3).
@@ -1115,8 +1119,9 @@ void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
  */
 bool MPU6050::getSlaveWriteMode(uint8_t num)
 {
-	if(num > 3)
-		return 0;
+	if(num > 3) {
+		return false;
+	}
 	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
 	return buffer[0];
 }
@@ -1129,8 +1134,9 @@ bool MPU6050::getSlaveWriteMode(uint8_t num)
  */
 void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
 }
 /** Get word pair grouping order offset for the specified slave (0-3).
@@ -1146,8 +1152,9 @@ void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
  */
 bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
 {
-	if(num > 3)
-		return 0;
+	if(num > 3) {
+		return false;
+	}
 	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
 	return buffer[0];
 }
@@ -1159,8 +1166,9 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
  */
 void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
 }
 /** Get number of bytes to read for the specified slave (0-3).
@@ -1172,8 +1180,9 @@ void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
  */
 uint8_t MPU6050::getSlaveDataLength(uint8_t num)
 {
-	if(num > 3)
-		return 0;
+	if(num > 3) {
+		return false;
+	}
 	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
 }
 /** Set number of bytes to read for the specified slave (0-3).
@@ -1184,8 +1193,9 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num)
  */
 void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
 					  length);
 }
@@ -2280,8 +2290,9 @@ bool MPU6050::getZeroMotionDetected()
  */
 void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
 {
-	if(num > 3)
+	if(num > 3) {
 		return;
+	}
 	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
 }
 
@@ -2332,8 +2343,9 @@ void MPU6050::setExternalShadowDelayEnabled(bool enabled)
 bool MPU6050::getSlaveDelayEnabled(uint8_t num)
 {
 	// MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
-	if(num > 4)
-		return 0;
+	if(num > 4) {
+		return false;
+	}
 	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
 	return buffer[0];
 }
@@ -3586,13 +3598,15 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 uint8_t MPU6050::readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length)
 {
 	uint8_t bits;
-	I2Cdev::readBits(devAddr, regAddr, bitStart, length, &bits);
+	const auto count = I2Cdev::readBits(devAddr, regAddr, bitStart, length, &bits);
+	(void)count;
 	return bits;
 }
 
 uint8_t MPU6050::readByte(uint8_t regAddr)
 {
 	uint8_t byte;
-	I2Cdev::readByte(devAddr, regAddr, &byte);
+	const auto count = I2Cdev::readByte(devAddr, regAddr, &byte);
+	(void)count;
 	return byte;
 }
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index bb7f625d4e..13cb6eba01 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -848,11 +848,11 @@ class MPU6050
 	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
 	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
 
+private:
 	// I2C helpers
 	uint8_t readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length);
 	uint8_t readByte(uint8_t regAddr);
 
-private:
 	uint8_t devAddr;
 	uint8_t buffer[14] = {0};
 };

From 3ee776563540e7d7476ae8b19d217bbd5feba1d7 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Fri, 8 Sep 2023 12:12:18 +0200
Subject: [PATCH 08/21] Add readBit helper

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 244 ++++++++++------------------
 Sming/Libraries/MPU6050/MPU6050.h   |   1 +
 2 files changed, 87 insertions(+), 158 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 8be8a446d0..cf76233c3a 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -65,8 +65,7 @@ void MPU6050::initialize()
  */
 uint8_t MPU6050::getAuxVDDIOLevel()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT);
 }
 /** Set the auxiliary I2C supply voltage level.
  * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
@@ -313,8 +312,7 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
  */
 bool MPU6050::getAccelXSelfTest()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT);
 }
 /** Get self-test enabled setting for accelerometer X axis.
  * @param enabled Self-test enabled value
@@ -330,8 +328,7 @@ void MPU6050::setAccelXSelfTest(bool enabled)
  */
 bool MPU6050::getAccelYSelfTest()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT);
 }
 /** Get self-test enabled value for accelerometer Y axis.
  * @param enabled Self-test enabled value
@@ -347,8 +344,7 @@ void MPU6050::setAccelYSelfTest(bool enabled)
  */
 bool MPU6050::getAccelZSelfTest()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT);
 }
 /** Set self-test enabled value for accelerometer Z axis.
  * @param enabled Self-test enabled value
@@ -656,8 +652,7 @@ void MPU6050::setZeroMotionDetectionDuration(uint8_t duration)
  */
 bool MPU6050::getTempFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT);
 }
 /** Set temperature FIFO enabled value.
  * @param enabled New temperature FIFO enabled value
@@ -676,8 +671,7 @@ void MPU6050::setTempFIFOEnabled(bool enabled)
  */
 bool MPU6050::getXGyroFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT);
 }
 /** Set gyroscope X-axis FIFO enabled value.
  * @param enabled New gyroscope X-axis FIFO enabled value
@@ -696,8 +690,7 @@ void MPU6050::setXGyroFIFOEnabled(bool enabled)
  */
 bool MPU6050::getYGyroFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT);
 }
 /** Set gyroscope Y-axis FIFO enabled value.
  * @param enabled New gyroscope Y-axis FIFO enabled value
@@ -716,8 +709,7 @@ void MPU6050::setYGyroFIFOEnabled(bool enabled)
  */
 bool MPU6050::getZGyroFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT);
 }
 /** Set gyroscope Z-axis FIFO enabled value.
  * @param enabled New gyroscope Z-axis FIFO enabled value
@@ -737,8 +729,7 @@ void MPU6050::setZGyroFIFOEnabled(bool enabled)
  */
 bool MPU6050::getAccelFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT);
 }
 /** Set accelerometer FIFO enabled value.
  * @param enabled New accelerometer FIFO enabled value
@@ -757,8 +748,7 @@ void MPU6050::setAccelFIFOEnabled(bool enabled)
  */
 bool MPU6050::getSlave2FIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT);
 }
 /** Set Slave 2 FIFO enabled value.
  * @param enabled New Slave 2 FIFO enabled value
@@ -777,8 +767,7 @@ void MPU6050::setSlave2FIFOEnabled(bool enabled)
  */
 bool MPU6050::getSlave1FIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT);
 }
 /** Set Slave 1 FIFO enabled value.
  * @param enabled New Slave 1 FIFO enabled value
@@ -797,8 +786,7 @@ void MPU6050::setSlave1FIFOEnabled(bool enabled)
  */
 bool MPU6050::getSlave0FIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT);
 }
 /** Set Slave 0 FIFO enabled value.
  * @param enabled New Slave 0 FIFO enabled value
@@ -829,8 +817,7 @@ void MPU6050::setSlave0FIFOEnabled(bool enabled)
  */
 bool MPU6050::getMultiMasterEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT);
 }
 /** Set multi-master enabled value.
  * @param enabled New multi-master enabled value
@@ -854,8 +841,7 @@ void MPU6050::setMultiMasterEnabled(bool enabled)
  */
 bool MPU6050::getWaitForExternalSensorEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT);
 }
 /** Set wait-for-external-sensor-data enabled value.
  * @param enabled New wait-for-external-sensor-data enabled value
@@ -874,8 +860,7 @@ void MPU6050::setWaitForExternalSensorEnabled(bool enabled)
  */
 bool MPU6050::getSlave3FIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT);
 }
 /** Set Slave 3 FIFO enabled value.
  * @param enabled New Slave 3 FIFO enabled value
@@ -898,8 +883,7 @@ void MPU6050::setSlave3FIFOEnabled(bool enabled)
  */
 bool MPU6050::getSlaveReadWriteTransitionEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT);
 }
 /** Set slave read/write transition enabled value.
  * @param enabled New slave read/write transition enabled value
@@ -1262,8 +1246,7 @@ void MPU6050::setSlave4OutputByte(uint8_t data)
  */
 bool MPU6050::getSlave4Enabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT);
 }
 /** Set the enabled value for Slave 4.
  * @param enabled New enabled value for Slave 4
@@ -1285,8 +1268,7 @@ void MPU6050::setSlave4Enabled(bool enabled)
  */
 bool MPU6050::getSlave4InterruptEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT);
 }
 /** Set the enabled value for Slave 4 transaction interrupts.
  * @param enabled New enabled value for Slave 4 transaction interrupts.
@@ -1309,8 +1291,7 @@ void MPU6050::setSlave4InterruptEnabled(bool enabled)
  */
 bool MPU6050::getSlave4WriteMode()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT);
 }
 /** Set write mode for the Slave 4.
  * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data
@@ -1375,8 +1356,7 @@ uint8_t MPU6050::getSlate4InputByte()
  */
 bool MPU6050::getPassthroughStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
 }
 /** Get Slave 4 transaction done status.
  * Automatically sets to 1 when a Slave 4 transaction has completed. This
@@ -1388,8 +1368,7 @@ bool MPU6050::getPassthroughStatus()
  */
 bool MPU6050::getSlave4IsDone()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
 }
 /** Get master arbitration lost status.
  * This bit automatically sets to 1 when the I2C Master has lost arbitration of
@@ -1400,8 +1379,7 @@ bool MPU6050::getSlave4IsDone()
  */
 bool MPU6050::getLostArbitration()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
 }
 /** Get Slave 4 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1412,8 +1390,7 @@ bool MPU6050::getLostArbitration()
  */
 bool MPU6050::getSlave4Nack()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT);
 }
 /** Get Slave 3 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1424,8 +1401,7 @@ bool MPU6050::getSlave4Nack()
  */
 bool MPU6050::getSlave3Nack()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
 }
 /** Get Slave 2 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1436,8 +1412,7 @@ bool MPU6050::getSlave3Nack()
  */
 bool MPU6050::getSlave2Nack()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
 }
 /** Get Slave 1 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1448,8 +1423,7 @@ bool MPU6050::getSlave2Nack()
  */
 bool MPU6050::getSlave1Nack()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
 }
 /** Get Slave 0 NACK status.
  * This bit automatically sets to 1 when the I2C Master receives a NACK in a
@@ -1460,8 +1434,7 @@ bool MPU6050::getSlave1Nack()
  */
 bool MPU6050::getSlave0Nack()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
 }
 
 // INT_PIN_CFG register
@@ -1474,8 +1447,7 @@ bool MPU6050::getSlave0Nack()
  */
 bool MPU6050::getInterruptMode()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT);
 }
 /** Set interrupt logic level mode.
  * @param mode New interrupt mode (0=active-high, 1=active-low)
@@ -1495,8 +1467,7 @@ void MPU6050::setInterruptMode(bool mode)
  */
 bool MPU6050::getInterruptDrive()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT);
 }
 /** Set interrupt drive mode.
  * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
@@ -1516,8 +1487,7 @@ void MPU6050::setInterruptDrive(bool drive)
  */
 bool MPU6050::getInterruptLatch()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT);
 }
 /** Set interrupt latch mode.
  * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
@@ -1537,8 +1507,7 @@ void MPU6050::setInterruptLatch(bool latch)
  */
 bool MPU6050::getInterruptLatchClear()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT);
 }
 /** Set interrupt latch clear mode.
  * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
@@ -1558,8 +1527,7 @@ void MPU6050::setInterruptLatchClear(bool clear)
  */
 bool MPU6050::getFSyncInterruptLevel()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT);
 }
 /** Set FSYNC interrupt logic level mode.
  * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
@@ -1579,8 +1547,7 @@ void MPU6050::setFSyncInterruptLevel(bool level)
  */
 bool MPU6050::getFSyncInterruptEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT);
 }
 /** Set FSYNC pin interrupt enabled setting.
  * @param enabled New FSYNC pin interrupt enabled setting
@@ -1605,8 +1572,7 @@ void MPU6050::setFSyncInterruptEnabled(bool enabled)
  */
 bool MPU6050::getI2CBypassEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT);
 }
 /** Set I2C bypass enabled status.
  * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
@@ -1634,8 +1600,7 @@ void MPU6050::setI2CBypassEnabled(bool enabled)
  */
 bool MPU6050::getClockOutputEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT);
 }
 /** Set reference clock output enabled status.
  * When this bit is equal to 1, a reference clock output is provided at the
@@ -1684,8 +1649,7 @@ void MPU6050::setIntEnabled(uint8_t enabled)
  **/
 bool MPU6050::getIntFreefallEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT);
 }
 /** Set Free Fall interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1705,8 +1669,7 @@ void MPU6050::setIntFreefallEnabled(bool enabled)
  **/
 bool MPU6050::getIntMotionEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT);
 }
 /** Set Motion Detection interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1726,8 +1689,7 @@ void MPU6050::setIntMotionEnabled(bool enabled)
  **/
 bool MPU6050::getIntZeroMotionEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT);
 }
 /** Set Zero Motion Detection interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1747,8 +1709,7 @@ void MPU6050::setIntZeroMotionEnabled(bool enabled)
  **/
 bool MPU6050::getIntFIFOBufferOverflowEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
 }
 /** Set FIFO Buffer Overflow interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1769,8 +1730,7 @@ void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled)
  **/
 bool MPU6050::getIntI2CMasterEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
 }
 /** Set I2C Master interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1791,8 +1751,7 @@ void MPU6050::setIntI2CMasterEnabled(bool enabled)
  */
 bool MPU6050::getIntDataReadyEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT);
 }
 /** Set Data Ready interrupt enabled status.
  * @param enabled New interrupt enabled status
@@ -1827,8 +1786,7 @@ uint8_t MPU6050::getIntStatus()
  */
 bool MPU6050::getIntFreefallStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT);
 }
 /** Get Motion Detection interrupt status.
  * This bit automatically sets to 1 when a Motion Detection interrupt has been
@@ -1839,8 +1797,7 @@ bool MPU6050::getIntFreefallStatus()
  */
 bool MPU6050::getIntMotionStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT);
 }
 /** Get Zero Motion Detection interrupt status.
  * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
@@ -1851,8 +1808,7 @@ bool MPU6050::getIntMotionStatus()
  */
 bool MPU6050::getIntZeroMotionStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT);
 }
 /** Get FIFO Buffer Overflow interrupt status.
  * This bit automatically sets to 1 when a Free Fall interrupt has been
@@ -1863,8 +1819,7 @@ bool MPU6050::getIntZeroMotionStatus()
  */
 bool MPU6050::getIntFIFOBufferOverflowStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
 }
 /** Get I2C Master interrupt status.
  * This bit automatically sets to 1 when an I2C Master interrupt has been
@@ -1876,8 +1831,7 @@ bool MPU6050::getIntFIFOBufferOverflowStatus()
  */
 bool MPU6050::getIntI2CMasterStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
 }
 /** Get Data Ready interrupt status.
  * This bit automatically sets to 1 when a Data Ready interrupt has been
@@ -1888,8 +1842,7 @@ bool MPU6050::getIntI2CMasterStatus()
  */
 bool MPU6050::getIntDataReadyStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
 }
 
 // ACCEL_*OUT_* registers
@@ -2214,8 +2167,7 @@ uint8_t MPU6050::getMotionStatus()
  */
 bool MPU6050::getXNegMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT);
 }
 /** Get X-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2224,8 +2176,7 @@ bool MPU6050::getXNegMotionDetected()
  */
 bool MPU6050::getXPosMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT);
 }
 /** Get Y-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2234,8 +2185,7 @@ bool MPU6050::getXPosMotionDetected()
  */
 bool MPU6050::getYNegMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT);
 }
 /** Get Y-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2244,8 +2194,7 @@ bool MPU6050::getYNegMotionDetected()
  */
 bool MPU6050::getYPosMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT);
 }
 /** Get Z-axis negative motion detection interrupt status.
  * @return Motion detection status
@@ -2254,8 +2203,7 @@ bool MPU6050::getYPosMotionDetected()
  */
 bool MPU6050::getZNegMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT);
 }
 /** Get Z-axis positive motion detection interrupt status.
  * @return Motion detection status
@@ -2264,8 +2212,7 @@ bool MPU6050::getZNegMotionDetected()
  */
 bool MPU6050::getZPosMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT);
 }
 /** Get zero motion detection interrupt status.
  * @return Motion detection status
@@ -2274,8 +2221,7 @@ bool MPU6050::getZPosMotionDetected()
  */
 bool MPU6050::getZeroMotionDetected()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT);
 }
 
 // I2C_SLV*_DO register
@@ -2308,8 +2254,7 @@ void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
  */
 bool MPU6050::getExternalShadowDelayEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT);
 }
 /** Set external data shadow delay enabled status.
  * @param enabled New external data shadow delay enabled status.
@@ -2346,8 +2291,7 @@ bool MPU6050::getSlaveDelayEnabled(uint8_t num)
 	if(num > 4) {
 		return false;
 	}
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, num);
 }
 /** Set slave delay enabled status.
  * @param num Slave number (0-4)
@@ -2517,8 +2461,7 @@ void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement)
  */
 bool MPU6050::getFIFOEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT);
 }
 /** Set FIFO enabled status.
  * @param enabled New FIFO enabled status
@@ -2543,8 +2486,7 @@ void MPU6050::setFIFOEnabled(bool enabled)
  */
 bool MPU6050::getI2CMasterModeEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT);
 }
 /** Set I2C Master Mode enabled status.
  * @param enabled New I2C Master Mode enabled status
@@ -2625,8 +2567,7 @@ void MPU6050::reset()
  */
 bool MPU6050::getSleepEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT);
 }
 /** Set sleep mode status.
  * @param enabled New sleep mode enabled status
@@ -2648,8 +2589,7 @@ void MPU6050::setSleepEnabled(bool enabled)
  */
 bool MPU6050::getWakeCycleEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT);
 }
 /** Set wake cycle enabled status.
  * @param enabled New sleep mode enabled status
@@ -2674,8 +2614,7 @@ void MPU6050::setWakeCycleEnabled(bool enabled)
  */
 bool MPU6050::getTempSensorEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer);
-	return buffer[0] == 0; // 1 is actually disabled here
+	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT); // 1 is actually disabled here
 }
 /** Set temperature sensor enabled status.
  * Note: this register stores the *disabled* value, but for consistency with the
@@ -2785,8 +2724,7 @@ void MPU6050::setWakeFrequency(uint8_t frequency)
  */
 bool MPU6050::getStandbyXAccelEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT);
 }
 /** Set X-axis accelerometer standby enabled status.
  * @param New X-axis standby enabled status
@@ -2806,8 +2744,7 @@ void MPU6050::setStandbyXAccelEnabled(bool enabled)
  */
 bool MPU6050::getStandbyYAccelEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT);
 }
 /** Set Y-axis accelerometer standby enabled status.
  * @param New Y-axis standby enabled status
@@ -2827,8 +2764,7 @@ void MPU6050::setStandbyYAccelEnabled(bool enabled)
  */
 bool MPU6050::getStandbyZAccelEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT);
 }
 /** Set Z-axis accelerometer standby enabled status.
  * @param New Z-axis standby enabled status
@@ -2848,8 +2784,7 @@ void MPU6050::setStandbyZAccelEnabled(bool enabled)
  */
 bool MPU6050::getStandbyXGyroEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT);
 }
 /** Set X-axis gyroscope standby enabled status.
  * @param New X-axis standby enabled status
@@ -2869,8 +2804,7 @@ void MPU6050::setStandbyXGyroEnabled(bool enabled)
  */
 bool MPU6050::getStandbyYGyroEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT);
 }
 /** Set Y-axis gyroscope standby enabled status.
  * @param New Y-axis standby enabled status
@@ -2890,8 +2824,7 @@ void MPU6050::setStandbyYGyroEnabled(bool enabled)
  */
 bool MPU6050::getStandbyZGyroEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT);
 }
 /** Set Z-axis gyroscope standby enabled status.
  * @param New Z-axis standby enabled status
@@ -3000,8 +2933,7 @@ void MPU6050::setDeviceID(uint8_t id)
 
 uint8_t MPU6050::getOTPBankValid()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
 }
 void MPU6050::setOTPBankValid(bool enabled)
 {
@@ -3147,8 +3079,7 @@ void MPU6050::setZGyroOffset(int16_t offset)
 
 bool MPU6050::getIntPLLReadyEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
 }
 void MPU6050::setIntPLLReadyEnabled(bool enabled)
 {
@@ -3156,8 +3087,7 @@ void MPU6050::setIntPLLReadyEnabled(bool enabled)
 }
 bool MPU6050::getIntDMPEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT);
 }
 void MPU6050::setIntDMPEnabled(bool enabled)
 {
@@ -3167,54 +3097,45 @@ void MPU6050::setIntDMPEnabled(bool enabled)
 // DMP_INT_STATUS
 bool MPU6050::getDMPInt5Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT);
 }
 bool MPU6050::getDMPInt4Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT);
 }
 bool MPU6050::getDMPInt3Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT);
 }
 bool MPU6050::getDMPInt2Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT);
 }
 bool MPU6050::getDMPInt1Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT);
 }
 bool MPU6050::getDMPInt0Status()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT);
 }
 
 // INT_STATUS register (DMP functions)
 
 bool MPU6050::getIntPLLReadyStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
 }
 bool MPU6050::getIntDMPStatus()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
 }
 
 // USER_CTRL register (DMP functions)
 
 bool MPU6050::getDMPEnabled()
 {
-	I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
 }
 void MPU6050::setDMPEnabled(bool enabled)
 {
@@ -3594,6 +3515,13 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 	resetFIFO();
 	resetDMP();
 }
+uint8_t MPU6050::readBit(uint8_t regAddr, uint8_t bitNum)
+{
+	uint8_t bit;
+	const auto count = I2Cdev::readBit(devAddr, regAddr, bitNum, &bit);
+	(void)count;
+	return bit;
+}
 
 uint8_t MPU6050::readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length)
 {
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 13cb6eba01..7961677470 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -850,6 +850,7 @@ class MPU6050
 
 private:
 	// I2C helpers
+	uint8_t readBit(uint8_t regAddr, uint8_t bitNum);
 	uint8_t readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length);
 	uint8_t readByte(uint8_t regAddr);
 

From 84d86c0a39a7dd35901b2805e5a1b9c105751744 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sat, 9 Sep 2023 10:54:32 +0200
Subject: [PATCH 09/21] Introduce Motion3/6 and overload printTo func

---
 Sming/Libraries/MPU6050/MPU6050.cpp           | 85 ++++++++++---------
 Sming/Libraries/MPU6050/MPU6050.h             | 18 +++-
 .../Accel_Gyro_MPU6050/app/application.cpp    | 11 +--
 3 files changed, 63 insertions(+), 51 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index cf76233c3a..a8599d8adb 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -40,6 +40,32 @@ THE SOFTWARE.
 
 #define I2C_NUM I2C_NUM_0
 
+inline int16_t concat(uint8_t bits_15_8, uint8_t bits_7_0)
+{
+	return (((int16_t)bits_15_8) << 8) | bits_7_0;
+}
+
+size_t MPU6050::Motion3::printTo(Print& p) const
+{
+	size_t n{0};
+	n += p.print(x);
+	n += p.print('\t');
+	n += p.print(y);
+	n += p.print('\t');
+	n += p.print(z);
+	return n;
+}
+
+size_t MPU6050::Motion6::printTo(Print& p) const
+{
+	size_t n{0};
+	n += p.print(_F("accel/gyro:\t"));
+	n += p.print(accel);
+	n += p.print('\t');
+	n += p.print(gyro);
+	return n;
+}
+
 /** Power on and prepare for general usage.
  * This will activate the device and take it out of sleep mode (which must be
  * done after start-up). This function also sets both the accelerometer and the
@@ -1847,49 +1873,25 @@ bool MPU6050::getIntDataReadyStatus()
 
 // ACCEL_*OUT_* registers
 
-/** Get raw 9-axis motion sensor readings (accel/gyro/compass).
- * FUNCTION NOT FULLY IMPLEMENTED YET.
- * @param ax 16-bit signed integer container for accelerometer X-axis value
- * @param ay 16-bit signed integer container for accelerometer Y-axis value
- * @param az 16-bit signed integer container for accelerometer Z-axis value
- * @param gx 16-bit signed integer container for gyroscope X-axis value
- * @param gy 16-bit signed integer container for gyroscope Y-axis value
- * @param gz 16-bit signed integer container for gyroscope Z-axis value
- * @param mx 16-bit signed integer container for magnetometer X-axis value
- * @param my 16-bit signed integer container for magnetometer Y-axis value
- * @param mz 16-bit signed integer container for magnetometer Z-axis value
- * @see getMotion6()
- * @see getAcceleration()
- * @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
-}
 /** Get raw 6-axis motion sensor readings (accel/gyro).
  * Retrieves all currently available motion sensor values.
- * @param ax 16-bit signed integer container for accelerometer X-axis value
- * @param ay 16-bit signed integer container for accelerometer Y-axis value
- * @param az 16-bit signed integer container for accelerometer Z-axis value
- * @param gx 16-bit signed integer container for gyroscope X-axis value
- * @param gy 16-bit signed integer container for gyroscope Y-axis value
- * @param gz 16-bit signed integer container for gyroscope Z-axis value
+ * @return container for 3-axis accelerometer and 3-axis gyroscope values
  * @see getAcceleration()
  * @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)
+MPU6050::Motion6 MPU6050::getMotion6()
 {
+	Motion6 motion6;
+	uint8_t buffer[14] = {0};
 	I2Cdev::readBytes(devAddr, 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];
+	motion6.accel.x = concat(buffer[0], buffer[1]);
+	motion6.accel.y = concat(buffer[2], buffer[3]);
+	motion6.accel.z = concat(buffer[4], buffer[5]);
+	motion6.gyro.x = concat(buffer[8], buffer[9]);
+	motion6.gyro.y = concat(buffer[10], buffer[11]);
+	motion6.gyro.z = concat(buffer[12], buffer[13]);
+	return motion6;
 }
 /** Get 3-axis accelerometer readings.
  * These registers store the most recent accelerometer measurements.
@@ -1927,13 +1929,18 @@ 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)
+
+MPU6050::Motion3 MPU6050::getAcceleration()
 {
+	Motion3 accel;
+	uint8_t buffer[6] = {0};
 	I2Cdev::readBytes(devAddr, 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];
+	accel.x = concat(buffer[0], buffer[1]);
+	accel.y = concat(buffer[2], buffer[3]);
+	accel.z = concat(buffer[4], buffer[5]);
+	return accel;
 }
+
 /** Get X-axis accelerometer reading.
  * @return X-axis acceleration measurement in 16-bit 2's complement format
  * @see getMotion6()
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 7961677470..f6576c16d1 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -433,6 +433,18 @@ THE SOFTWARE.
 class MPU6050
 {
 public:
+	struct Motion3 {
+		int16_t x{};
+		int16_t y{};
+		int16_t z{};
+		size_t printTo(Print& p) const;
+	};
+
+	struct Motion6 {
+		Motion3 accel{};
+		Motion3 gyro{};
+		size_t printTo(Print& p) const;
+	};
 	/** Default constructor, uses default I2C address.
      * @see MPU6050_DEFAULT_ADDRESS
      */
@@ -640,9 +652,9 @@ class MPU6050
 	bool getIntDataReadyStatus();
 
 	// ACCEL_*OUT_* registers
-	void getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx,
-					int16_t* my, int16_t* mz);
-	void getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+	Motion6 getMotion6();
+
+	Motion3 getAcceleration();
 	void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
 	int16_t getAccelerationX();
 	int16_t getAccelerationY();
diff --git a/samples/Accel_Gyro_MPU6050/app/application.cpp b/samples/Accel_Gyro_MPU6050/app/application.cpp
index 4bd118f8a4..0190f99c03 100644
--- a/samples/Accel_Gyro_MPU6050/app/application.cpp
+++ b/samples/Accel_Gyro_MPU6050/app/application.cpp
@@ -5,17 +5,10 @@ constexpr uint16_t mainLoopInterval = 20; // ms
 SimpleTimer mainLoopTimer;
 MPU6050 mpu;
 
-void printAccelGyro(int16_t ax, int16_t ay, int16_t az, int16_t gx, int16_t gy, int16_t gz)
-{
-	Serial << "a/g:\t" << ax << "\t" << ay << "\t" << az << "\t" << gx << "\t" << gy << "\t" << gz << endl;
-}
-
 void mainLoop()
 {
-	int16_t ax, ay, az;
-	int16_t gx, gy, gz;
-	mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
-	printAccelGyro(ax, ay, az, gx, gy, gz);
+	const MPU6050::Motion6 accelGyro = mpu.getMotion6();
+	Serial << accelGyro << endl;
 }
 
 void init()

From 7094edebab900ec00fac9ab191b86ceeaf618f05 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sat, 9 Sep 2023 19:20:24 +0200
Subject: [PATCH 10/21] Remove redefinition of pgm_read_byte and some deadcode

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 14 +-------------
 Sming/Libraries/MPU6050/MPU6050.h   | 13 -------------
 2 files changed, 1 insertion(+), 26 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index a8599d8adb..ab7f31c38f 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -3275,9 +3275,6 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 					printf("%#04x", verifyBuffer[i + j]);
 				}
 				printf("\n");
-				// free(verifyBuffer);
-				// if (useProgMem) free(progBuffer);
-				// return false; // uh oh.
 			}
 		}
 
@@ -3331,12 +3328,6 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 		// 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 = static_cast<uint8_t*>(realloc(progBuffer, length));
@@ -3358,9 +3349,6 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 			} else {
 				special = data[i++];
 			}
-			/*Serial.print("Special command code ");
-      Serial.print(special, HEX);
-      Serial.println(" found...");*/
 			if(special == 0x01) {
 				// enable DMP-related interrupts
 
@@ -3506,7 +3494,6 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 				eSample++; // Successfully found offsets prepare to  advance
 			if((eSum < 100) && (c > 10) && (eSample >= 10))
 				break; // Advance to next Loop
-					   //            delay(1);
 		}
 		kP *= .75;
 		kI *= .75;
@@ -3522,6 +3509,7 @@ void MPU6050::PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops)
 	resetFIFO();
 	resetDMP();
 }
+
 uint8_t MPU6050::readBit(uint8_t regAddr, uint8_t bitNum)
 {
 	uint8_t bit;
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index f6576c16d1..1a6465d4a6 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -39,19 +39,6 @@ THE SOFTWARE.
 
 #include <Libraries/I2Cdev/I2Cdev.h>
 
-// supporting link:
-// http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 also:
-// http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s
-
-#undef pgm_read_byte
-#define pgm_read_byte(addr) (*static_cast<const unsigned char*>(addr))
-
-//#define PROGMEM /* empty */
-//#define pgm_read_byte(x) (*(x))
-//#define pgm_read_word(x) (*(x))
-//#define pgm_read_float(x) (*(x))
-//#define PSTR(STR) STR
-
 #define MPU6050_ADDRESS_AD0_LOW 0x68  // address pin low (GND), default for InvenSense evaluation board
 #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
 #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW

From fa1612d656a6b829de8b6f2f82b15b6480912278 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sun, 10 Sep 2023 16:44:21 +0200
Subject: [PATCH 11/21] Add doxygen enabler in README.rst

---
 Sming/Libraries/MPU6050/{README.md => README.rst} | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)
 rename Sming/Libraries/MPU6050/{README.md => README.rst} (74%)

diff --git a/Sming/Libraries/MPU6050/README.md b/Sming/Libraries/MPU6050/README.rst
similarity index 74%
rename from Sming/Libraries/MPU6050/README.md
rename to Sming/Libraries/MPU6050/README.rst
index 68c01c3f30..cd6dd4bb30 100644
--- a/Sming/Libraries/MPU6050/README.md
+++ b/Sming/Libraries/MPU6050/README.rst
@@ -1,6 +1,11 @@
-# MPU6050 Six-Axis (Gyro + Accelerometer)
+API Documentation
+-----------------
+
+.. doxygenclass:: MPU6050
+   :members:
 
-Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050). Most of the code is the same, except:
+# MPU6050 Six-Axis (Gyro + Accelerometer)
+Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050) @ 605a740. Most of the code is the same, except:
 
 -   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
 -   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.

From 60a96d479bec98deee3c66fac4acf9a7fb34207f Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sun, 10 Sep 2023 21:41:47 +0200
Subject: [PATCH 12/21] Introduce readReg function and remove buffer member
 from class

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 91 +++++++++++++----------------
 Sming/Libraries/MPU6050/MPU6050.h   | 37 ++++++++++--
 2 files changed, 73 insertions(+), 55 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index ab7f31c38f..19f2ac2134 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -39,11 +39,7 @@ THE SOFTWARE.
 #include <string.h>
 
 #define I2C_NUM I2C_NUM_0
-
-inline int16_t concat(uint8_t bits_15_8, uint8_t bits_7_0)
-{
-	return (((int16_t)bits_15_8) << 8) | bits_7_0;
-}
+using detail::concat;
 
 size_t MPU6050::Motion3::printTo(Print& p) const
 {
@@ -214,7 +210,6 @@ void MPU6050::setExternalFrameSync(uint8_t sync)
 uint8_t MPU6050::getDLPFMode()
 {
 	return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH);
-	return buffer[0];
 }
 /** Set digital low-pass filter configuration.
  * @param mode New DLFP configuration setting
@@ -296,6 +291,7 @@ uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
  */
 uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
 {
+	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer);
 	return (buffer[0] >> 3) | (buffer[1] & 0x03);
 }
@@ -1068,8 +1064,7 @@ bool MPU6050::getSlaveEnabled(uint8_t num)
 	if(num > 3) {
 		return false;
 	}
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT);
 }
 /** Set the enabled value for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1100,8 +1095,7 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num)
 	if(num > 3) {
 		return false;
 	}
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
 }
 /** Set word pair byte-swapping enabled for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1132,8 +1126,7 @@ bool MPU6050::getSlaveWriteMode(uint8_t num)
 	if(num > 3) {
 		return false;
 	}
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
 }
 /** Set write mode for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1165,8 +1158,7 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
 	if(num > 3) {
 		return false;
 	}
-	I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, buffer);
-	return buffer[0];
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
 }
 /** Set word pair grouping order offset for the specified slave (0-3).
  * @param num Slave number (0-3)
@@ -1877,7 +1869,7 @@ bool MPU6050::getIntDataReadyStatus()
  * Retrieves all currently available motion sensor values.
  * @return container for 3-axis accelerometer and 3-axis gyroscope values
  * @see getAcceleration()
- * @see getRotation()
+ * @see getAngularRate()
  * @see MPU6050_RA_ACCEL_XOUT_H
  */
 MPU6050::Motion6 MPU6050::getMotion6()
@@ -1948,8 +1940,7 @@ MPU6050::Motion3 MPU6050::getAcceleration()
  */
 int16_t MPU6050::getAccelerationX()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_ACCEL_XOUT_H);
 }
 /** Get Y-axis accelerometer reading.
  * @return Y-axis acceleration measurement in 16-bit 2's complement format
@@ -1958,8 +1949,7 @@ int16_t MPU6050::getAccelerationX()
  */
 int16_t MPU6050::getAccelerationY()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_ACCEL_YOUT_H);
 }
 /** Get Z-axis accelerometer reading.
  * @return Z-axis acceleration measurement in 16-bit 2's complement format
@@ -1968,8 +1958,7 @@ int16_t MPU6050::getAccelerationY()
  */
 int16_t MPU6050::getAccelerationZ()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_ACCEL_ZOUT_H);
 }
 
 // TEMP_OUT_* registers
@@ -1980,8 +1969,7 @@ int16_t MPU6050::getAccelerationZ()
  */
 int16_t MPU6050::getTemperature()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_TEMP_OUT_H);
 }
 
 // GYRO_*OUT_* registers
@@ -2012,48 +2000,52 @@ int16_t MPU6050::getTemperature()
  * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
  * </pre>
  *
- * @param x 16-bit signed integer container for X-axis rotation
- * @param y 16-bit signed integer container for Y-axis rotation
- * @param z 16-bit signed integer container for Z-axis rotation
+ * @return container for 3-axis gyro values
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_XOUT_H
  */
-void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z)
+MPU6050::Motion3 MPU6050::getAngularRate()
 {
+	Motion3 angularRate;
+	uint8_t buffer[6] = {0};
 	I2Cdev::readBytes(devAddr, 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];
+	angularRate.x = concat(buffer[0], buffer[1]);
+	angularRate.y = concat(buffer[2], buffer[3]);
+	angularRate.z = concat(buffer[4], buffer[5]);
+	return angularRate;
 }
+
 /** Get X-axis gyroscope reading.
  * @return X-axis rotation measurement in 16-bit 2's complement format
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_XOUT_H
  */
-int16_t MPU6050::getRotationX()
+int16_t MPU6050::getAngularRateX()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_GYRO_XOUT_H);
 }
 /** Get Y-axis gyroscope reading.
  * @return Y-axis rotation measurement in 16-bit 2's complement format
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_YOUT_H
  */
-int16_t MPU6050::getRotationY()
+int16_t MPU6050::getAngularRateY()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_GYRO_YOUT_H);
 }
 /** Get Z-axis gyroscope reading.
  * @return Z-axis rotation measurement in 16-bit 2's complement format
  * @see getMotion6()
  * @see MPU6050_RA_GYRO_ZOUT_H
  */
-int16_t MPU6050::getRotationZ()
+int16_t MPU6050::getAngularRateZ()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
+}
+
+int16_t MPU6050::getAngularRateZ2()
+{
+	return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
 }
 
 // EXT_SENS_DATA_* registers
@@ -2143,8 +2135,7 @@ uint8_t MPU6050::getExternalSensorByte(int position)
  */
 uint16_t MPU6050::getExternalSensorWord(int position)
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer);
-	return (((uint16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<uint16_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
 }
 /** Read double word (4 bytes) from external sensor data registers.
  * @param position Starting position (0-20)
@@ -2153,8 +2144,7 @@ uint16_t MPU6050::getExternalSensorWord(int position)
  */
 uint32_t MPU6050::getExternalSensorDWord(int position)
 {
-	I2Cdev::readBytes(devAddr, 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];
+	return readReg<uint32_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
 }
 
 // MOT_DETECT_STATUS register
@@ -2855,6 +2845,7 @@ void MPU6050::setStandbyZGyroEnabled(bool enabled)
  */
 uint16_t MPU6050::getFIFOCount()
 {
+	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
 	return (((uint16_t)buffer[0]) << 8) | buffer[1];
 }
@@ -3014,6 +3005,7 @@ void MPU6050::setZFineGain(int8_t gain)
 
 int16_t MPU6050::getXAccelOffset()
 {
+	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
@@ -3026,6 +3018,7 @@ void MPU6050::setXAccelOffset(int16_t offset)
 
 int16_t MPU6050::getYAccelOffset()
 {
+	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
@@ -3038,6 +3031,7 @@ void MPU6050::setYAccelOffset(int16_t offset)
 
 int16_t MPU6050::getZAccelOffset()
 {
+	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
@@ -3050,8 +3044,7 @@ void MPU6050::setZAccelOffset(int16_t offset)
 
 int16_t MPU6050::getXGyroOffset()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_XG_OFFS_USRH);
 }
 void MPU6050::setXGyroOffset(int16_t offset)
 {
@@ -3062,9 +3055,9 @@ void MPU6050::setXGyroOffset(int16_t offset)
 
 int16_t MPU6050::getYGyroOffset()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_YG_OFFS_USRH);
 }
+
 void MPU6050::setYGyroOffset(int16_t offset)
 {
 	I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
@@ -3074,9 +3067,9 @@ void MPU6050::setYGyroOffset(int16_t offset)
 
 int16_t MPU6050::getZGyroOffset()
 {
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer);
-	return (((int16_t)buffer[0]) << 8) | buffer[1];
+	return readReg<int16_t>(MPU6050_RA_ZG_OFFS_USRH);
 }
+
 void MPU6050::setZGyroOffset(int16_t offset)
 {
 	I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 1a6465d4a6..9b012ba41e 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -642,7 +642,6 @@ class MPU6050
 	Motion6 getMotion6();
 
 	Motion3 getAcceleration();
-	void getAcceleration(int16_t* x, int16_t* y, int16_t* z);
 	int16_t getAccelerationX();
 	int16_t getAccelerationY();
 	int16_t getAccelerationZ();
@@ -651,10 +650,11 @@ class MPU6050
 	int16_t getTemperature();
 
 	// GYRO_*OUT_* registers
-	void getRotation(int16_t* x, int16_t* y, int16_t* z);
-	int16_t getRotationX();
-	int16_t getRotationY();
-	int16_t getRotationZ();
+	Motion3 getAngularRate();
+	int16_t getAngularRateX();
+	int16_t getAngularRateY();
+	int16_t getAngularRateZ();
+	int16_t getAngularRateZ2();
 
 	// EXT_SENS_DATA_* registers
 	uint8_t getExternalSensorByte(int position);
@@ -853,6 +853,31 @@ class MPU6050
 	uint8_t readBits(uint8_t regAddr, uint8_t bitStart, uint8_t length);
 	uint8_t readByte(uint8_t regAddr);
 
+	template <typename T> T readReg(uint8_t regAddr);
+
 	uint8_t devAddr;
-	uint8_t buffer[14] = {0};
 };
+
+template <typename T> T MPU6050::readReg(uint8_t regAddr)
+{
+	static_assert(std::is_fundamental<T>::value, "T must be an fundamental type.");
+
+	const auto sz = sizeof(T);
+	uint8_t buffer[sz] = {0};
+	//data follow big endien convention
+	I2Cdev::readBytes(devAddr, regAddr, sz, buffer);
+
+	T result{};
+	for(size_t i{0}; i < sz; ++i) {
+		result |= static_cast<T>(buffer[i]) << (8 * (sz - i - 1));
+	}
+	return result;
+}
+
+namespace detail
+{
+template <typename T = int16_t> inline T concat(uint8_t bits_15_8, uint8_t bits_7_0)
+{
+	return (static_cast<T>(bits_15_8) << 8) | bits_7_0;
+}
+} // namespace detail

From b42780291ed88d7f0ecde0884e7d261f82d0740b Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sun, 10 Sep 2023 22:01:32 +0200
Subject: [PATCH 13/21] Add braces for one-liner under if

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 42 +++++++++++++++++++----------
 1 file changed, 28 insertions(+), 14 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 19f2ac2134..4462e895c3 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -3151,10 +3151,12 @@ void MPU6050::resetDMP()
 void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
 {
 	bank &= 0x1F;
-	if(userBank)
+	if(userBank) {
 		bank |= 0x20;
-	if(prefetchEnabled)
+	}
+	if(prefetchEnabled) {
 		bank |= 0x40;
+	}
 	I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
 }
 
@@ -3186,12 +3188,14 @@ void MPU6050::readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank, ui
 		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
-		if(i + chunkSize > dataSize)
+		if(i + chunkSize > dataSize) {
 			chunkSize = dataSize - i;
+		}
 
 		// make sure this chunk doesn't go past the bank boundary (256 bytes)
-		if(chunkSize > 256 - address)
+		if(chunkSize > 256 - address) {
 			chunkSize = 256 - address;
+		}
 
 		// read the chunk of data as specified
 		I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
@@ -3204,8 +3208,9 @@ void MPU6050::readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank, ui
 
 		// if we aren't done, update bank (if necessary) and address
 		if(i < dataSize) {
-			if(address == 0)
+			if(address == 0) {
 				bank++;
+			}
 			setMemoryBank(bank);
 			setMemoryStartAddress(address);
 		}
@@ -3220,21 +3225,25 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 	uint8_t* progBuffer = 0;
 	uint16_t i;
 	uint8_t j;
-	if(verify)
+	if(verify) {
 		verifyBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
-	if(useProgMem)
+	}
+	if(useProgMem) {
 		progBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
+	}
 	for(i = 0; i < dataSize;) {
 		// determine correct chunk size according to bank position and data size
 		uint8_t chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
 
 		// make sure we don't go past the data size
-		if(i + chunkSize > dataSize)
+		if(i + chunkSize > dataSize) {
 			chunkSize = dataSize - i;
+		}
 
 		// make sure this chunk doesn't go past the bank boundary (256 bytes)
-		if(chunkSize > 256 - address)
+		if(chunkSize > 256 - address) {
 			chunkSize = 256 - address;
+		}
 
 		if(useProgMem) {
 			// write the chunk of data as specified
@@ -3279,16 +3288,19 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 
 		// if we aren't done, update bank (if necessary) and address
 		if(i < dataSize) {
-			if(address == 0)
+			if(address == 0) {
 				bank++;
+			}
 			setMemoryBank(bank);
 			setMemoryStartAddress(address);
 		}
 	}
-	if(verify)
+	if(verify) {
 		free(verifyBuffer);
-	if(useProgMem)
+	}
+	if(useProgMem) {
 		free(progBuffer);
+	}
 	return true;
 }
 bool MPU6050::writeProgMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify)
@@ -3359,13 +3371,15 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 		}
 
 		if(!success) {
-			if(useProgMem)
+			if(useProgMem) {
 				free(progBuffer);
+			}
 			return false; // uh oh
 		}
 	}
-	if(useProgMem)
+	if(useProgMem) {
 		free(progBuffer);
+	}
 	return true;
 }
 bool MPU6050::writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize)

From d4a55f88ae4ac6b663c6934d9a1e25abdf11bc85 Mon Sep 17 00:00:00 2001
From: xeonqq <xeonqq@gmail.com>
Date: Sun, 10 Sep 2023 22:02:50 +0200
Subject: [PATCH 14/21] Add braces for one-liner under for

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 4462e895c3..e4113b7c8b 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -3247,8 +3247,9 @@ bool MPU6050::writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t b
 
 		if(useProgMem) {
 			// write the chunk of data as specified
-			for(j = 0; j < chunkSize; j++)
+			for(j = 0; j < chunkSize; j++) {
 				progBuffer[j] = pgm_read_byte(data + i + j);
+			}
 		} else {
 			// write the chunk of data as specified
 			progBuffer = const_cast<uint8_t*>(data) + i;
@@ -3336,8 +3337,9 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 			if(useProgMem) {
 				if(sizeof(progBuffer) < length)
 					progBuffer = static_cast<uint8_t*>(realloc(progBuffer, length));
-				for(j = 0; j < length; j++)
+				for(j = 0; j < length; j++) {
 					progBuffer[j] = pgm_read_byte(data + i + j);
+				}
 			} else {
 				progBuffer = const_cast<uint8_t*>(data) + i;
 			}

From d970ee2a52cbead091a58d8cefbc7a61db2e4536 Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Fri, 3 Nov 2023 21:15:40 +0100
Subject: [PATCH 15/21] Move one liner functions to header

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 3005 ++-------------------------
 Sming/Libraries/MPU6050/MPU6050.h   | 2961 +++++++++++++++++++++++---
 2 files changed, 2824 insertions(+), 3142 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index e4113b7c8b..dff39db572 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -62,13 +62,6 @@ size_t MPU6050::Motion6::printTo(Print& p) const
 	return n;
 }
 
-/** Power on and prepare for general usage.
- * This will activate the device and take it out of sleep mode (which must be
- * done after start-up). This function also sets both the accelerometer and the
- * gyroscope to their most sensitive settings, namely +/- 2g and +/- 250
- * degrees/sec, and sets the clock source to use the X Gyro for reference, which
- * is slightly better than the default internal clock source.
- */
 void MPU6050::initialize()
 {
 	setClockSource(MPU6050_CLOCK_PLL_XGYRO);
@@ -77,196 +70,6 @@ void MPU6050::initialize()
 	setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
 }
 
-// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
-
-/** Get the auxiliary I2C supply voltage level.
- * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
- * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
- * the MPU-6000, which does not have a VLOGIC pin.
- * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
- */
-uint8_t MPU6050::getAuxVDDIOLevel()
-{
-	return readBit(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT);
-}
-/** Set the auxiliary I2C supply voltage level.
- * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
- * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
- * the MPU-6000, which does not have a VLOGIC pin.
- * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
- */
-void MPU6050::setAuxVDDIOLevel(uint8_t level)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
-}
-
-// SMPLRT_DIV register
-
-/** Get gyroscope output rate divider.
- * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero
- * Motion detection, and Free Fall detection are all based on the Sample Rate.
- * The Sample Rate is generated by dividing the gyroscope output rate by
- * SMPLRT_DIV:
- *
- * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
- *
- * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or
- * 7), and 1kHz when the DLPF is enabled (see Register 26).
- *
- * Note: The accelerometer output rate is 1kHz. This means that for a Sample
- * Rate greater than 1kHz, the same accelerometer sample may be output to the
- * FIFO, DMP, and sensor registers more than once.
- *
- * For a diagram of the gyroscope and accelerometer signal paths, see Section 8
- * of the MPU-6000/MPU-6050 Product Specification document.
- *
- * @return Current sample rate
- * @see MPU6050_RA_SMPLRT_DIV
- */
-uint8_t MPU6050::getRate()
-{
-	return readByte(MPU6050_RA_SMPLRT_DIV);
-}
-/** Set gyroscope sample rate divider.
- * @param rate New sample rate divider
- * @see getRate()
- * @see MPU6050_RA_SMPLRT_DIV
- */
-void MPU6050::setRate(uint8_t rate)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
-}
-
-// CONFIG register
-
-/** Get external FSYNC configuration.
- * Configures the external Frame Synchronization (FSYNC) pin sampling. An
- * external signal connected to the FSYNC pin can be sampled by configuring
- * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
- * strobes may be captured. The latched FSYNC signal will be sampled at the
- * Sampling Rate, as defined in register 25. After sampling, the latch will
- * reset to the current FSYNC signal state.
- *
- * The sampled value will be reported in place of the least significant bit in
- * a sensor data register determined by the value of EXT_SYNC_SET according to
- * the following table.
- *
- * <pre>
- * EXT_SYNC_SET | FSYNC Bit Location
- * -------------+-------------------
- * 0            | Input disabled
- * 1            | TEMP_OUT_L[0]
- * 2            | GYRO_XOUT_L[0]
- * 3            | GYRO_YOUT_L[0]
- * 4            | GYRO_ZOUT_L[0]
- * 5            | ACCEL_XOUT_L[0]
- * 6            | ACCEL_YOUT_L[0]
- * 7            | ACCEL_ZOUT_L[0]
- * </pre>
- *
- * @return FSYNC configuration value
- */
-uint8_t MPU6050::getExternalFrameSync()
-{
-	return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH);
-}
-/** Set external FSYNC configuration.
- * @see getExternalFrameSync()
- * @see MPU6050_RA_CONFIG
- * @param sync New FSYNC configuration value
- */
-void MPU6050::setExternalFrameSync(uint8_t sync)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
-}
-/** Get digital low-pass filter configuration.
- * The DLPF_CFG parameter sets the digital low pass filter configuration. It
- * also determines the internal sampling rate used by the device as shown in
- * the table below.
- *
- * Note: The accelerometer output rate is 1kHz. This means that for a Sample
- * Rate greater than 1kHz, the same accelerometer sample may be output to the
- * FIFO, DMP, and sensor registers more than once.
- *
- * <pre>
- *          |   ACCELEROMETER    |           GYROSCOPE
- * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
- * ---------+-----------+--------+-----------+--------+-------------
- * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
- * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
- * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
- * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
- * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
- * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
- * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
- * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
- * </pre>
- *
- * @return DLFP configuration
- * @see MPU6050_RA_CONFIG
- * @see MPU6050_CFG_DLPF_CFG_BIT
- * @see MPU6050_CFG_DLPF_CFG_LENGTH
- */
-uint8_t MPU6050::getDLPFMode()
-{
-	return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH);
-}
-/** Set digital low-pass filter configuration.
- * @param mode New DLFP configuration setting
- * @see getDLPFBandwidth()
- * @see MPU6050_DLPF_BW_256
- * @see MPU6050_RA_CONFIG
- * @see MPU6050_CFG_DLPF_CFG_BIT
- * @see MPU6050_CFG_DLPF_CFG_LENGTH
- */
-void MPU6050::setDLPFMode(uint8_t mode)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
-}
-
-// GYRO_CONFIG register
-
-/** Get full-scale gyroscope range.
- * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
- * as described in the table below.
- *
- * <pre>
- * 0 = +/- 250 degrees/sec
- * 1 = +/- 500 degrees/sec
- * 2 = +/- 1000 degrees/sec
- * 3 = +/- 2000 degrees/sec
- * </pre>
- *
- * @return Current full-scale gyroscope range setting
- * @see MPU6050_GYRO_FS_250
- * @see MPU6050_RA_GYRO_CONFIG
- * @see MPU6050_GCONFIG_FS_SEL_BIT
- * @see MPU6050_GCONFIG_FS_SEL_LENGTH
- */
-uint8_t MPU6050::getFullScaleGyroRange()
-{
-	return readBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH);
-}
-/** Set full-scale gyroscope range.
- * @param range New full-scale gyroscope range value
- * @see getFullScaleRange()
- * @see MPU6050_GYRO_FS_250
- * @see MPU6050_RA_GYRO_CONFIG
- * @see MPU6050_GCONFIG_FS_SEL_BIT
- * @see MPU6050_GCONFIG_FS_SEL_LENGTH
- */
-void MPU6050::setFullScaleGyroRange(uint8_t range)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
-					  range);
-}
-
-// SELF TEST FACTORY TRIM VALUES
-
-/** Get self-test factory trim value for accelerometer X axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_X
- */
 uint8_t MPU6050::getAccelXSelfTestFactoryTrim()
 {
 	const uint8_t x = readByte(MPU6050_RA_SELF_TEST_X);
@@ -274,10 +77,6 @@ uint8_t MPU6050::getAccelXSelfTestFactoryTrim()
 	return (x >> 3) | ((a >> 4) & 0x03);
 }
 
-/** Get self-test factory trim value for accelerometer Y axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_Y
- */
 uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
 {
 	const uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
@@ -285,10 +84,6 @@ uint8_t MPU6050::getAccelYSelfTestFactoryTrim()
 	return (y >> 3) | ((a >> 2) & 0x03);
 }
 
-/** Get self-test factory trim value for accelerometer Z axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_Z
- */
 uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
 {
 	uint8_t buffer[2] = {0};
@@ -296,668 +91,24 @@ uint8_t MPU6050::getAccelZSelfTestFactoryTrim()
 	return (buffer[0] >> 3) | (buffer[1] & 0x03);
 }
 
-/** Get self-test factory trim value for gyro X axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_X
- */
 uint8_t MPU6050::getGyroXSelfTestFactoryTrim()
 {
 	const uint8_t x = readByte(MPU6050_RA_SELF_TEST_X);
 	return (x & 0x1F);
 }
 
-/** Get self-test factory trim value for gyro Y axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_Y
- */
 uint8_t MPU6050::getGyroYSelfTestFactoryTrim()
 {
 	const uint8_t y = readByte(MPU6050_RA_SELF_TEST_Y);
 	return (y & 0x1F);
 }
 
-/** Get self-test factory trim value for gyro Z axis.
- * @return factory trim value
- * @see MPU6050_RA_SELF_TEST_Z
- */
 uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
 {
 	const uint8_t z = readByte(MPU6050_RA_SELF_TEST_Z);
 	return (z & 0x1F);
 }
 
-// ACCEL_CONFIG register
-
-/** Get self-test enabled setting for accelerometer X axis.
- * @return Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-bool MPU6050::getAccelXSelfTest()
-{
-	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT);
-}
-/** Get self-test enabled setting for accelerometer X axis.
- * @param enabled Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-void MPU6050::setAccelXSelfTest(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled);
-}
-/** Get self-test enabled value for accelerometer Y axis.
- * @return Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-bool MPU6050::getAccelYSelfTest()
-{
-	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT);
-}
-/** Get self-test enabled value for accelerometer Y axis.
- * @param enabled Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-void MPU6050::setAccelYSelfTest(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled);
-}
-/** Get self-test enabled value for accelerometer Z axis.
- * @return Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-bool MPU6050::getAccelZSelfTest()
-{
-	return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT);
-}
-/** Set self-test enabled value for accelerometer Z axis.
- * @param enabled Self-test enabled value
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-void MPU6050::setAccelZSelfTest(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
-}
-/** Get full-scale accelerometer range.
- * The FS_SEL parameter allows setting the full-scale range of the accelerometer
- * sensors, as described in the table below.
- *
- * <pre>
- * 0 = +/- 2g
- * 1 = +/- 4g
- * 2 = +/- 8g
- * 3 = +/- 16g
- * </pre>
- *
- * @return Current full-scale accelerometer range setting
- * @see MPU6050_ACCEL_FS_2
- * @see MPU6050_RA_ACCEL_CONFIG
- * @see MPU6050_ACONFIG_AFS_SEL_BIT
- * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
- */
-uint8_t MPU6050::getFullScaleAccelRange()
-{
-	return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH);
-}
-/** Set full-scale accelerometer range.
- * @param range New full-scale accelerometer range setting
- * @see getFullScaleAccelRange()
- */
-void MPU6050::setFullScaleAccelRange(uint8_t range)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
-					  range);
-}
-/** Get the high-pass filter configuration.
- * The DHPF is a filter module in the path leading to motion detectors (Free
- * Fall, Motion threshold, and Zero Motion). The high pass filter output is not
- * available to the data registers (see Figure in Section 8 of the MPU-6000/
- * MPU-6050 Product Specification document).
- *
- * The high pass filter has three modes:
- *
- * <pre>
- *    Reset: The filter output settles to zero within one sample. This
- *           effectively disables the high pass filter. This mode may be toggled
- *           to quickly settle the filter.
- *
- *    On:    The high pass filter will pass signals above the cut off frequency.
- *
- *    Hold:  When triggered, the filter holds the present sample. The filter
- *           output will be the difference between the input sample and the held
- *           sample.
- * </pre>
- *
- * <pre>
- * ACCEL_HPF | Filter Mode | Cut-off Frequency
- * ----------+-------------+------------------
- * 0         | Reset       | None
- * 1         | On          | 5Hz
- * 2         | On          | 2.5Hz
- * 3         | On          | 1.25Hz
- * 4         | On          | 0.63Hz
- * 7         | Hold        | None
- * </pre>
- *
- * @return Current high-pass filter configuration
- * @see MPU6050_DHPF_RESET
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-uint8_t MPU6050::getDHPFMode()
-{
-	return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH);
-}
-/** Set the high-pass filter configuration.
- * @param bandwidth New high-pass filter configuration
- * @see setDHPFMode()
- * @see MPU6050_DHPF_RESET
- * @see MPU6050_RA_ACCEL_CONFIG
- */
-void MPU6050::setDHPFMode(uint8_t bandwidth)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
-					  bandwidth);
-}
-
-// FF_THR register
-
-/** Get free-fall event acceleration threshold.
- * This register configures the detection threshold for Free Fall event
- * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the
- * absolute value of the accelerometer measurements for the three axes are each
- * less than the detection threshold. This condition increments the Free Fall
- * duration counter (Register 30). The Free Fall interrupt is triggered when the
- * Free Fall duration counter reaches the time specified in FF_DUR.
- *
- * For more details on the Free Fall detection interrupt, see Section 8.2 of the
- * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
- * 58 of this document.
- *
- * @return Current free-fall acceleration threshold value (LSB = 2mg)
- * @see MPU6050_RA_FF_THR
- */
-uint8_t MPU6050::getFreefallDetectionThreshold()
-{
-	return readByte(MPU6050_RA_FF_THR);
-}
-/** Get free-fall event acceleration threshold.
- * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
- * @see getFreefallDetectionThreshold()
- * @see MPU6050_RA_FF_THR
- */
-void MPU6050::setFreefallDetectionThreshold(uint8_t threshold)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
-}
-
-// FF_DUR register
-
-/** Get free-fall event duration threshold.
- * This register configures the duration counter threshold for Free Fall event
- * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
- * of 1 LSB = 1 ms.
- *
- * The Free Fall duration counter increments while the absolute value of the
- * accelerometer measurements are each less than the detection threshold
- * (Register 29). The Free Fall interrupt is triggered when the Free Fall
- * duration counter reaches the time specified in this register.
- *
- * For more details on the Free Fall detection interrupt, see Section 8.2 of
- * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
- * and 58 of this document.
- *
- * @return Current free-fall duration threshold value (LSB = 1ms)
- * @see MPU6050_RA_FF_DUR
- */
-uint8_t MPU6050::getFreefallDetectionDuration()
-{
-	return readByte(MPU6050_RA_FF_DUR);
-}
-/** Get free-fall event duration threshold.
- * @param duration New free-fall duration threshold value (LSB = 1ms)
- * @see getFreefallDetectionDuration()
- * @see MPU6050_RA_FF_DUR
- */
-void MPU6050::setFreefallDetectionDuration(uint8_t duration)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
-}
-
-// MOT_THR register
-
-/** Get motion detection event acceleration threshold.
- * This register configures the detection threshold for Motion interrupt
- * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the
- * absolute value of any of the accelerometer measurements exceeds this Motion
- * detection threshold. This condition increments the Motion detection duration
- * counter (Register 32). The Motion detection interrupt is triggered when the
- * Motion Detection counter reaches the time count specified in MOT_DUR
- * (Register 32).
- *
- * The Motion interrupt will indicate the axis and polarity of detected motion
- * in MOT_DETECT_STATUS (Register 97).
- *
- * For more details on the Motion detection interrupt, see Section 8.3 of the
- * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
- * 58 of this document.
- *
- * @return Current motion detection acceleration threshold value (LSB = 2mg)
- * @see MPU6050_RA_MOT_THR
- */
-uint8_t MPU6050::getMotionDetectionThreshold()
-{
-	return readByte(MPU6050_RA_MOT_THR);
-}
-/** Set motion detection event acceleration threshold.
- * @param threshold New motion detection acceleration threshold value (LSB =
- * 2mg)
- * @see getMotionDetectionThreshold()
- * @see MPU6050_RA_MOT_THR
- */
-void MPU6050::setMotionDetectionThreshold(uint8_t threshold)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
-}
-
-// MOT_DUR register
-
-/** Get motion detection event duration threshold.
- * This register configures the duration counter threshold for Motion interrupt
- * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit
- * of 1LSB = 1ms. The Motion detection duration counter increments when the
- * absolute value of any of the accelerometer measurements exceeds the Motion
- * detection threshold (Register 31). The Motion detection interrupt is
- * triggered when the Motion detection counter reaches the time count specified
- * in this register.
- *
- * For more details on the Motion detection interrupt, see Section 8.3 of the
- * MPU-6000/MPU-6050 Product Specification document.
- *
- * @return Current motion detection duration threshold value (LSB = 1ms)
- * @see MPU6050_RA_MOT_DUR
- */
-uint8_t MPU6050::getMotionDetectionDuration()
-{
-	return readByte(MPU6050_RA_MOT_DUR);
-}
-/** Set motion detection event duration threshold.
- * @param duration New motion detection duration threshold value (LSB = 1ms)
- * @see getMotionDetectionDuration()
- * @see MPU6050_RA_MOT_DUR
- */
-void MPU6050::setMotionDetectionDuration(uint8_t duration)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
-}
-
-// ZRMOT_THR register
-
-/** Get zero motion detection event acceleration threshold.
- * This register configures the detection threshold for Zero Motion interrupt
- * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when
- * the absolute value of the accelerometer measurements for the 3 axes are each
- * less than the detection threshold. This condition increments the Zero Motion
- * duration counter (Register 34). The Zero Motion interrupt is triggered when
- * the Zero Motion duration counter reaches the time count specified in
- * ZRMOT_DUR (Register 34).
- *
- * Unlike Free Fall or Motion detection, Zero Motion detection triggers an
- * interrupt both when Zero Motion is first detected and when Zero Motion is no
- * longer detected.
- *
- * When a zero motion event is detected, a Zero Motion Status will be indicated
- * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion
- * condition is detected, the status bit is set to 1. When a zero-motion-to-
- * motion condition is detected, the status bit is set to 0.
- *
- * For more details on the Zero Motion detection interrupt, see Section 8.4 of
- * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
- * and 58 of this document.
- *
- * @return Current zero motion detection acceleration threshold value (LSB =
- * 2mg)
- * @see MPU6050_RA_ZRMOT_THR
- */
-uint8_t MPU6050::getZeroMotionDetectionThreshold()
-{
-	return readByte(MPU6050_RA_ZRMOT_THR);
-}
-/** Set zero motion detection event acceleration threshold.
- * @param threshold New zero motion detection acceleration threshold value (LSB
- * = 2mg)
- * @see getZeroMotionDetectionThreshold()
- * @see MPU6050_RA_ZRMOT_THR
- */
-void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
-}
-
-// ZRMOT_DUR register
-
-/** Get zero motion detection event duration threshold.
- * This register configures the duration counter threshold for Zero Motion
- * interrupt generation. The duration counter ticks at 16 Hz, therefore
- * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter
- * increments while the absolute value of the accelerometer measurements are
- * each less than the detection threshold (Register 33). The Zero Motion
- * interrupt is triggered when the Zero Motion duration counter reaches the time
- * count specified in this register.
- *
- * For more details on the Zero Motion detection interrupt, see Section 8.4 of
- * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56
- * and 58 of this document.
- *
- * @return Current zero motion detection duration threshold value (LSB = 64ms)
- * @see MPU6050_RA_ZRMOT_DUR
- */
-uint8_t MPU6050::getZeroMotionDetectionDuration()
-{
-	return readByte(MPU6050_RA_ZRMOT_DUR);
-}
-/** Set zero motion detection event duration threshold.
- * @param duration New zero motion detection duration threshold value (LSB =
- * 1ms)
- * @see getZeroMotionDetectionDuration()
- * @see MPU6050_RA_ZRMOT_DUR
- */
-void MPU6050::setZeroMotionDetectionDuration(uint8_t duration)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
-}
-
-// FIFO_EN register
-
-/** Get temperature FIFO enabled value.
- * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
- * 66) to be written into the FIFO buffer.
- * @return Current temperature FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getTempFIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT);
-}
-/** Set temperature FIFO enabled value.
- * @param enabled New temperature FIFO enabled value
- * @see getTempFIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setTempFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled);
-}
-/** Get gyroscope X-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
- * 68) to be written into the FIFO buffer.
- * @return Current gyroscope X-axis FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getXGyroFIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT);
-}
-/** Set gyroscope X-axis FIFO enabled value.
- * @param enabled New gyroscope X-axis FIFO enabled value
- * @see getXGyroFIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setXGyroFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
-}
-/** Get gyroscope Y-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
- * 70) to be written into the FIFO buffer.
- * @return Current gyroscope Y-axis FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getYGyroFIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT);
-}
-/** Set gyroscope Y-axis FIFO enabled value.
- * @param enabled New gyroscope Y-axis FIFO enabled value
- * @see getYGyroFIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setYGyroFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
-}
-/** Get gyroscope Z-axis FIFO enabled value.
- * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
- * 72) to be written into the FIFO buffer.
- * @return Current gyroscope Z-axis FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getZGyroFIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT);
-}
-/** Set gyroscope Z-axis FIFO enabled value.
- * @param enabled New gyroscope Z-axis FIFO enabled value
- * @see getZGyroFIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setZGyroFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
-}
-/** Get accelerometer FIFO enabled value.
- * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
- * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
- * written into the FIFO buffer.
- * @return Current accelerometer FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getAccelFIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT);
-}
-/** Set accelerometer FIFO enabled value.
- * @param enabled New accelerometer FIFO enabled value
- * @see getAccelFIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setAccelFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
-}
-/** Get Slave 2 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 2 to be written into the FIFO buffer.
- * @return Current Slave 2 FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getSlave2FIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT);
-}
-/** Set Slave 2 FIFO enabled value.
- * @param enabled New Slave 2 FIFO enabled value
- * @see getSlave2FIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setSlave2FIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled);
-}
-/** Get Slave 1 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 1 to be written into the FIFO buffer.
- * @return Current Slave 1 FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getSlave1FIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT);
-}
-/** Set Slave 1 FIFO enabled value.
- * @param enabled New Slave 1 FIFO enabled value
- * @see getSlave1FIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setSlave1FIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled);
-}
-/** Get Slave 0 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 0 to be written into the FIFO buffer.
- * @return Current Slave 0 FIFO enabled value
- * @see MPU6050_RA_FIFO_EN
- */
-bool MPU6050::getSlave0FIFOEnabled()
-{
-	return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT);
-}
-/** Set Slave 0 FIFO enabled value.
- * @param enabled New Slave 0 FIFO enabled value
- * @see getSlave0FIFOEnabled()
- * @see MPU6050_RA_FIFO_EN
- */
-void MPU6050::setSlave0FIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
-}
-
-// I2C_MST_CTRL register
-
-/** Get multi-master enabled value.
- * Multi-master capability allows multiple I2C masters to operate on the same
- * bus. In circuits where multi-master capability is required, set MULT_MST_EN
- * to 1. This will increase current drawn by approximately 30uA.
- *
- * In circuits where multi-master capability is required, the state of the I2C
- * bus must always be monitored by each separate I2C Master. Before an I2C
- * Master can assume arbitration of the bus, it must first confirm that no other
- * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the
- * MPU-60X0's bus arbitration detection logic is turned on, enabling it to
- * detect when the bus is available.
- *
- * @return Current multi-master enabled value
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-bool MPU6050::getMultiMasterEnabled()
-{
-	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT);
-}
-/** Set multi-master enabled value.
- * @param enabled New multi-master enabled value
- * @see getMultiMasterEnabled()
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-void MPU6050::setMultiMasterEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled);
-}
-/** Get wait-for-external-sensor-data enabled value.
- * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be
- * delayed until External Sensor data from the Slave Devices are loaded into the
- * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
- * data (i.e. from gyro and accel) and external sensor data have been loaded to
- * their respective data registers (i.e. the data is synced) when the Data Ready
- * interrupt is triggered.
- *
- * @return Current wait-for-external-sensor-data enabled value
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-bool MPU6050::getWaitForExternalSensorEnabled()
-{
-	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT);
-}
-/** Set wait-for-external-sensor-data enabled value.
- * @param enabled New wait-for-external-sensor-data enabled value
- * @see getWaitForExternalSensorEnabled()
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-void MPU6050::setWaitForExternalSensorEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled);
-}
-/** Get Slave 3 FIFO enabled value.
- * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96)
- * associated with Slave 3 to be written into the FIFO buffer.
- * @return Current Slave 3 FIFO enabled value
- * @see MPU6050_RA_MST_CTRL
- */
-bool MPU6050::getSlave3FIFOEnabled()
-{
-	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT);
-}
-/** Set Slave 3 FIFO enabled value.
- * @param enabled New Slave 3 FIFO enabled value
- * @see getSlave3FIFOEnabled()
- * @see MPU6050_RA_MST_CTRL
- */
-void MPU6050::setSlave3FIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled);
-}
-/** Get slave read/write transition enabled value.
- * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave
- * read to the next slave read. If the bit equals 0, there will be a restart
- * between reads. If the bit equals 1, there will be a stop followed by a start
- * of the following read. When a write transaction follows a read transaction,
- * the stop followed by a start of the successive write will be always used.
- *
- * @return Current slave read/write transition enabled value
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-bool MPU6050::getSlaveReadWriteTransitionEnabled()
-{
-	return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT);
-}
-/** Set slave read/write transition enabled value.
- * @param enabled New slave read/write transition enabled value
- * @see getSlaveReadWriteTransitionEnabled()
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled);
-}
-/** Get I2C master clock speed.
- * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the
- * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
- * the following table:
- *
- * <pre>
- * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
- * ------------+------------------------+-------------------
- * 0           | 348kHz                 | 23
- * 1           | 333kHz                 | 24
- * 2           | 320kHz                 | 25
- * 3           | 308kHz                 | 26
- * 4           | 296kHz                 | 27
- * 5           | 286kHz                 | 28
- * 6           | 276kHz                 | 29
- * 7           | 267kHz                 | 30
- * 8           | 258kHz                 | 31
- * 9           | 500kHz                 | 16
- * 10          | 471kHz                 | 17
- * 11          | 444kHz                 | 18
- * 12          | 421kHz                 | 19
- * 13          | 400kHz                 | 20
- * 14          | 381kHz                 | 21
- * 15          | 364kHz                 | 22
- * </pre>
- *
- * @return Current I2C master clock speed
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-uint8_t MPU6050::getMasterClockSpeed()
-{
-	return readBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH);
-}
-/** Set I2C master clock speed.
- * @reparam speed Current I2C master clock speed
- * @see MPU6050_RA_I2C_MST_CTRL
- */
-void MPU6050::setMasterClockSpeed(uint8_t speed)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
-}
-
 // I2C_SLV* registers (Slave 0-3)
 
 /** Get the I2C address of the specified slave (0-3).
@@ -1083,1936 +234,194 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
  * When set to 1, this bit enables byte swapping. When byte swapping is enabled,
  * the high and low bytes of a word pair are swapped. Please refer to
  * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
- * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
- * registers in the order they were transferred.
- *
- * @param num Slave number (0-3)
- * @return Current word pair byte-swapping enabled value for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-bool MPU6050::getSlaveWordByteSwap(uint8_t num)
-{
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
-}
-/** Set word pair byte-swapping enabled for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param enabled New word pair byte-swapping enabled value for specified slave
- * @see getSlaveWordByteSwap()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
-{
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
-}
-/** Get write mode for the specified slave (0-3).
- * When set to 1, the transaction will read or write data only. When cleared to
- * 0, the transaction will write a register address prior to reading or writing
- * data. This should equal 0 when specifying the register address within the
- * Slave device to/from which the ensuing data transaction will take place.
- *
- * @param num Slave number (0-3)
- * @return Current write mode for specified slave (0 = register address + data,
- * 1 = data only)
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-bool MPU6050::getSlaveWriteMode(uint8_t num)
-{
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
-}
-/** Set write mode for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param mode New write mode for specified slave (0 = register address + data,
- * 1 = data only)
- * @see getSlaveWriteMode()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
-{
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
-}
-/** Get word pair grouping order offset for the specified slave (0-3).
- * This sets specifies the grouping order of word pairs received from registers.
- * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
- * then odd register addresses) are paired to form a word. When set to 1, bytes
- * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
- * register addresses) are paired to form a word.
- *
- * @param num Slave number (0-3)
- * @return Current word pair grouping order offset for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
-{
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
-}
-/** Set word pair grouping order offset for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param enabled New word pair grouping order offset for specified slave
- * @see getSlaveWordGroupOffset()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
-{
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
-}
-/** 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
- * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
- * @param num Slave number (0-3)
- * @return Number of bytes to read for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-uint8_t MPU6050::getSlaveDataLength(uint8_t num)
-{
-	if(num > 3) {
-		return false;
-	}
-	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
-}
-/** Set number of bytes to read for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param length Number of bytes to read for specified slave
- * @see getSlaveDataLength()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
-void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
-{
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
-					  length);
-}
-
-// I2C_SLV* registers (Slave 4)
-
-/** Get the I2C address of Slave 4.
- * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
- * operation, and if it is cleared, then it's a write operation. The remaining
- * bits (6-0) are the 7-bit device address of the slave device.
- *
- * @return Current address for Slave 4
- * @see getSlaveAddress()
- * @see MPU6050_RA_I2C_SLV4_ADDR
- */
-uint8_t MPU6050::getSlave4Address()
-{
-	return readByte(MPU6050_RA_I2C_SLV4_ADDR);
-}
-/** Set the I2C address of Slave 4.
- * @param address New address for Slave 4
- * @see getSlave4Address()
- * @see MPU6050_RA_I2C_SLV4_ADDR
- */
-void MPU6050::setSlave4Address(uint8_t address)
-{
-	I2Cdev::writeByte(devAddr, 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
- * register address is stored in this MPU register.
- *
- * @return Current active register for Slave 4
- * @see MPU6050_RA_I2C_SLV4_REG
- */
-uint8_t MPU6050::getSlave4Register()
-{
-	return readByte(MPU6050_RA_I2C_SLV4_REG);
-}
-/** Set the active internal register for Slave 4.
- * @param reg New active register for Slave 4
- * @see getSlave4Register()
- * @see MPU6050_RA_I2C_SLV4_REG
- */
-void MPU6050::setSlave4Register(uint8_t reg)
-{
-	I2Cdev::writeByte(devAddr, 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
- * is set 1 (set to read), this register has no effect.
- * @param data New byte to write to Slave 4
- * @see MPU6050_RA_I2C_SLV4_DO
- */
-void MPU6050::setSlave4OutputByte(uint8_t data)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data);
-}
-/** Get the enabled value for the Slave 4.
- * When set to 1, this bit enables Slave 4 for data transfer operations. When
- * cleared to 0, this bit disables Slave 4 from data transfer operations.
- * @return Current enabled value for Slave 4
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-bool MPU6050::getSlave4Enabled()
-{
-	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT);
-}
-/** Set the enabled value for Slave 4.
- * @param enabled New enabled value for Slave 4
- * @see getSlave4Enabled()
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-void MPU6050::setSlave4Enabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled);
-}
-/** Get the enabled value for Slave 4 transaction interrupts.
- * When set to 1, this bit enables the generation of an interrupt signal upon
- * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
- * generation of an interrupt signal upon completion of a Slave 4 transaction.
- * The interrupt status can be observed in Register 54.
- *
- * @return Current enabled value for Slave 4 transaction interrupts.
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-bool MPU6050::getSlave4InterruptEnabled()
-{
-	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT);
-}
-/** Set the enabled value for Slave 4 transaction interrupts.
- * @param enabled New enabled value for Slave 4 transaction interrupts.
- * @see getSlave4InterruptEnabled()
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-void MPU6050::setSlave4InterruptEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled);
-}
-/** Get write mode for Slave 4.
- * When set to 1, the transaction will read or write data only. When cleared to
- * 0, the transaction will write a register address prior to reading or writing
- * data. This should equal 0 when specifying the register address within the
- * Slave device to/from which the ensuing data transaction will take place.
- *
- * @return Current write mode for Slave 4 (0 = register address + data, 1 = data
- * only)
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-bool MPU6050::getSlave4WriteMode()
-{
-	return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT);
-}
-/** Set write mode for the Slave 4.
- * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data
- * only)
- * @see getSlave4WriteMode()
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-void MPU6050::setSlave4WriteMode(bool mode)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode);
-}
-/** Get Slave 4 master delay value.
- * This configures the reduced access rate of I2C slaves relative to the Sample
- * Rate. When a slave's access rate is decreased relative to the Sample Rate,
- * the slave is accessed every:
- *
- *     1 / (1 + I2C_MST_DLY) samples
- *
- * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
- * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
- * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
- * further information regarding the Sample Rate, please refer to register 25.
- *
- * @return Current Slave 4 master delay value
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-uint8_t MPU6050::getSlave4MasterDelay()
-{
-	return readBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH);
-}
-/** Set Slave 4 master delay value.
- * @param delay New Slave 4 master delay value
- * @see getSlave4MasterDelay()
- * @see MPU6050_RA_I2C_SLV4_CTRL
- */
-void MPU6050::setSlave4MasterDelay(uint8_t delay)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH,
-					  delay);
-}
-/** Get last available byte read from Slave 4.
- * This register stores the data read from Slave 4. This field is populated
- * after a read transaction.
- * @return Last available byte read from to Slave 4
- * @see MPU6050_RA_I2C_SLV4_DI
- */
-uint8_t MPU6050::getSlate4InputByte()
-{
-	return readByte(MPU6050_RA_I2C_SLV4_DI);
-}
-
-// I2C_MST_STATUS register
-
-/** Get FSYNC interrupt status.
- * This bit reflects the status of the FSYNC interrupt from an external device
- * into the MPU-60X0. This is used as a way to pass an external interrupt
- * through the MPU-60X0 to the host application processor. When set to 1, this
- * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
- * (Register 55).
- * @return FSYNC interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getPassthroughStatus()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
-}
-/** Get Slave 4 transaction done status.
- * Automatically sets to 1 when a Slave 4 transaction has completed. This
- * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
- * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
- * I2C_SLV4_CTRL register (Register 52).
- * @return Slave 4 transaction done status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave4IsDone()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
-}
-/** Get master arbitration lost status.
- * This bit automatically sets to 1 when the I2C Master has lost arbitration of
- * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
- * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Master arbitration lost status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getLostArbitration()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
-}
-/** Get Slave 4 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Slave 4 NACK interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave4Nack()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT);
-}
-/** Get Slave 3 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Slave 3 NACK interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave3Nack()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
-}
-/** Get Slave 2 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Slave 2 NACK interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave2Nack()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
-}
-/** Get Slave 1 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Slave 1 NACK interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave1Nack()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
-}
-/** Get Slave 0 NACK status.
- * This bit automatically sets to 1 when the I2C Master receives a NACK in a
- * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
- * bit in the INT_ENABLE register (Register 56) is asserted.
- * @return Slave 0 NACK interrupt status
- * @see MPU6050_RA_I2C_MST_STATUS
- */
-bool MPU6050::getSlave0Nack()
-{
-	return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
-}
-
-// INT_PIN_CFG register
-
-/** Get interrupt logic level mode.
- * Will be set 0 for active-high, 1 for active-low.
- * @return Current interrupt mode (0=active-high, 1=active-low)
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_LEVEL_BIT
- */
-bool MPU6050::getInterruptMode()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT);
-}
-/** Set interrupt logic level mode.
- * @param mode New interrupt mode (0=active-high, 1=active-low)
- * @see getInterruptMode()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_LEVEL_BIT
- */
-void MPU6050::setInterruptMode(bool mode)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
-}
-/** Get interrupt drive mode.
- * Will be set 0 for push-pull, 1 for open-drain.
- * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_OPEN_BIT
- */
-bool MPU6050::getInterruptDrive()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT);
-}
-/** Set interrupt drive mode.
- * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
- * @see getInterruptDrive()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_OPEN_BIT
- */
-void MPU6050::setInterruptDrive(bool drive)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
-}
-/** Get interrupt latch mode.
- * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
- * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
- */
-bool MPU6050::getInterruptLatch()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT);
-}
-/** Set interrupt latch mode.
- * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
- * @see getInterruptLatch()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
- */
-void MPU6050::setInterruptLatch(bool latch)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch);
-}
-/** Get interrupt latch clear mode.
- * Will be set 0 for status-read-only, 1 for any-register-read.
- * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
- */
-bool MPU6050::getInterruptLatchClear()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT);
-}
-/** Set interrupt latch clear mode.
- * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
- * @see getInterruptLatchClear()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
- */
-void MPU6050::setInterruptLatchClear(bool clear)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
-}
-/** Get FSYNC interrupt logic level mode.
- * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
- * @see getFSyncInterruptMode()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
- */
-bool MPU6050::getFSyncInterruptLevel()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT);
-}
-/** Set FSYNC interrupt logic level mode.
- * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
- * @see getFSyncInterruptMode()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
- */
-void MPU6050::setFSyncInterruptLevel(bool level)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
-}
-/** Get FSYNC pin interrupt enabled setting.
- * Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled setting
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
- */
-bool MPU6050::getFSyncInterruptEnabled()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT);
-}
-/** Set FSYNC pin interrupt enabled setting.
- * @param enabled New FSYNC pin interrupt enabled setting
- * @see getFSyncInterruptEnabled()
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
- */
-void MPU6050::setFSyncInterruptEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled);
-}
-/** Get I2C bypass enabled status.
- * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
- * 0, the host application processor will be able to directly access the
- * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
- * application processor will not be able to directly access the auxiliary I2C
- * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
- * bit[5]).
- * @return Current I2C bypass enabled status
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
- */
-bool MPU6050::getI2CBypassEnabled()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT);
-}
-/** Set I2C bypass enabled status.
- * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
- * 0, the host application processor will be able to directly access the
- * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
- * application processor will not be able to directly access the auxiliary I2C
- * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
- * bit[5]).
- * @param enabled New I2C bypass enabled status
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
- */
-void MPU6050::setI2CBypassEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled);
-}
-/** Get reference clock output enabled status.
- * When this bit is equal to 1, a reference clock output is provided at the
- * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
- * further information regarding CLKOUT, please refer to the MPU-60X0 Product
- * Specification document.
- * @return Current reference clock output enabled status
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_CLKOUT_EN_BIT
- */
-bool MPU6050::getClockOutputEnabled()
-{
-	return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT);
-}
-/** Set reference clock output enabled status.
- * When this bit is equal to 1, a reference clock output is provided at the
- * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
- * further information regarding CLKOUT, please refer to the MPU-60X0 Product
- * Specification document.
- * @param enabled New reference clock output enabled status
- * @see MPU6050_RA_INT_PIN_CFG
- * @see MPU6050_INTCFG_CLKOUT_EN_BIT
- */
-void MPU6050::setClockOutputEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
-}
-
-// INT_ENABLE register
-
-/** Get full interrupt enabled status.
- * Full register byte for all interrupts, for quick reading. Each bit will be
- * set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FF_BIT
- **/
-uint8_t MPU6050::getIntEnabled()
-{
-	return readByte(MPU6050_RA_INT_ENABLE);
-}
-/** Set full interrupt enabled status.
- * Full register byte for all interrupts, for quick reading. Each bit should be
- * set 0 for disabled, 1 for enabled.
- * @param enabled New interrupt enabled status
- * @see getIntFreefallEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FF_BIT
- **/
-void MPU6050::setIntEnabled(uint8_t enabled)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
-}
-/** Get Free Fall interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FF_BIT
- **/
-bool MPU6050::getIntFreefallEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT);
-}
-/** Set Free Fall interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntFreefallEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FF_BIT
- **/
-void MPU6050::setIntFreefallEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
-}
-/** Get Motion Detection interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_MOT_BIT
- **/
-bool MPU6050::getIntMotionEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT);
-}
-/** Set Motion Detection interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntMotionEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_MOT_BIT
- **/
-void MPU6050::setIntMotionEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
-}
-/** Get Zero Motion Detection interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_ZMOT_BIT
- **/
-bool MPU6050::getIntZeroMotionEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT);
-}
-/** Set Zero Motion Detection interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntZeroMotionEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_ZMOT_BIT
- **/
-void MPU6050::setIntZeroMotionEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
-}
-/** Get FIFO Buffer Overflow interrupt enabled status.
- * Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
- **/
-bool MPU6050::getIntFIFOBufferOverflowEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
-}
-/** Set FIFO Buffer Overflow interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntFIFOBufferOverflowEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
- **/
-void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
-}
-/** Get I2C Master interrupt enabled status.
- * This enables any of the I2C Master interrupt sources to generate an
- * interrupt. Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
- **/
-bool MPU6050::getIntI2CMasterEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
-}
-/** Set I2C Master interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntI2CMasterEnabled()
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
- **/
-void MPU6050::setIntI2CMasterEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled);
-}
-/** Get Data Ready interrupt enabled setting.
- * This event occurs each time a write operation to all of the sensor registers
- * has been completed. Will be set 0 for disabled, 1 for enabled.
- * @return Current interrupt enabled status
- * @see MPU6050_RA_INT_ENABLE
- * @see MPU6050_INTERRUPT_DATA_RDY_BIT
- */
-bool MPU6050::getIntDataReadyEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT);
-}
-/** Set Data Ready interrupt enabled status.
- * @param enabled New interrupt enabled status
- * @see getIntDataReadyEnabled()
- * @see MPU6050_RA_INT_CFG
- * @see MPU6050_INTERRUPT_DATA_RDY_BIT
- */
-void MPU6050::setIntDataReadyEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
-}
-
-// INT_STATUS register
-
-/** Get full set of interrupt status bits.
- * These bits clear to 0 after the register has been read. Very useful
- * for getting multiple INT statuses, since each single bit read clears
- * all of them because it has to read the whole byte.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- */
-uint8_t MPU6050::getIntStatus()
-{
-	return readByte(MPU6050_RA_INT_STATUS);
-}
-/** Get Free Fall interrupt status.
- * This bit automatically sets to 1 when a Free Fall interrupt has been
- * generated. The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_FF_BIT
- */
-bool MPU6050::getIntFreefallStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT);
-}
-/** Get Motion Detection interrupt status.
- * This bit automatically sets to 1 when a Motion Detection interrupt has been
- * generated. The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_MOT_BIT
- */
-bool MPU6050::getIntMotionStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT);
-}
-/** Get Zero Motion Detection interrupt status.
- * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
- * been generated. The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_ZMOT_BIT
- */
-bool MPU6050::getIntZeroMotionStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT);
-}
-/** Get FIFO Buffer Overflow interrupt status.
- * This bit automatically sets to 1 when a Free Fall interrupt has been
- * generated. The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
- */
-bool MPU6050::getIntFIFOBufferOverflowStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
-}
-/** Get I2C Master interrupt status.
- * This bit automatically sets to 1 when an I2C Master interrupt has been
- * generated. For a list of I2C Master interrupts, please refer to Register 54.
- * The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
- */
-bool MPU6050::getIntI2CMasterStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
-}
-/** Get Data Ready interrupt status.
- * This bit automatically sets to 1 when a Data Ready interrupt has been
- * generated. The bit clears to 0 after the register has been read.
- * @return Current interrupt status
- * @see MPU6050_RA_INT_STATUS
- * @see MPU6050_INTERRUPT_DATA_RDY_BIT
- */
-bool MPU6050::getIntDataReadyStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
-}
-
-// ACCEL_*OUT_* registers
-
-/** Get raw 6-axis motion sensor readings (accel/gyro).
- * Retrieves all currently available motion sensor values.
- * @return container for 3-axis accelerometer and 3-axis gyroscope values
- * @see getAcceleration()
- * @see getAngularRate()
- * @see MPU6050_RA_ACCEL_XOUT_H
- */
-MPU6050::Motion6 MPU6050::getMotion6()
-{
-	Motion6 motion6;
-	uint8_t buffer[14] = {0};
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
-	motion6.accel.x = concat(buffer[0], buffer[1]);
-	motion6.accel.y = concat(buffer[2], buffer[3]);
-	motion6.accel.z = concat(buffer[4], buffer[5]);
-	motion6.gyro.x = concat(buffer[8], buffer[9]);
-	motion6.gyro.y = concat(buffer[10], buffer[11]);
-	motion6.gyro.z = concat(buffer[12], buffer[13]);
-	return motion6;
-}
-/** Get 3-axis accelerometer readings.
- * These registers store the most recent accelerometer measurements.
- * Accelerometer measurements are written to these registers at the Sample Rate
- * as defined in Register 25.
- *
- * The accelerometer measurement registers, along with the temperature
- * measurement registers, gyroscope measurement registers, and external sensor
- * data registers, are composed of two sets of registers: an internal register
- * set and a user-facing read register set.
- *
- * The data within the accelerometer sensors' internal register set is always
- * updated at the Sample Rate. Meanwhile, the user-facing read register set
- * duplicates the internal register set's data values whenever the serial
- * interface is idle. This guarantees that a burst read of sensor registers will
- * read measurements from the same sampling instant. Note that if burst reads
- * are not used, the user is responsible for ensuring a set of single byte reads
- * correspond to a single sampling instant by checking the Data Ready interrupt.
- *
- * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
- * (Register 28). For each full scale setting, the accelerometers' sensitivity
- * per LSB in ACCEL_xOUT is shown in the table below:
- *
- * <pre>
- * AFS_SEL | Full Scale Range | LSB Sensitivity
- * --------+------------------+----------------
- * 0       | +/- 2g           | 8192 LSB/mg
- * 1       | +/- 4g           | 4096 LSB/mg
- * 2       | +/- 8g           | 2048 LSB/mg
- * 3       | +/- 16g          | 1024 LSB/mg
- * </pre>
- *
- * @param x 16-bit signed integer container for X-axis acceleration
- * @param y 16-bit signed integer container for Y-axis acceleration
- * @param z 16-bit signed integer container for Z-axis acceleration
- * @see MPU6050_RA_GYRO_XOUT_H
- */
-
-MPU6050::Motion3 MPU6050::getAcceleration()
-{
-	Motion3 accel;
-	uint8_t buffer[6] = {0};
-	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
-	accel.x = concat(buffer[0], buffer[1]);
-	accel.y = concat(buffer[2], buffer[3]);
-	accel.z = concat(buffer[4], buffer[5]);
-	return accel;
-}
-
-/** Get X-axis accelerometer reading.
- * @return X-axis acceleration measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_ACCEL_XOUT_H
- */
-int16_t MPU6050::getAccelerationX()
-{
-	return readReg<int16_t>(MPU6050_RA_ACCEL_XOUT_H);
-}
-/** Get Y-axis accelerometer reading.
- * @return Y-axis acceleration measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_ACCEL_YOUT_H
- */
-int16_t MPU6050::getAccelerationY()
-{
-	return readReg<int16_t>(MPU6050_RA_ACCEL_YOUT_H);
-}
-/** Get Z-axis accelerometer reading.
- * @return Z-axis acceleration measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_ACCEL_ZOUT_H
- */
-int16_t MPU6050::getAccelerationZ()
-{
-	return readReg<int16_t>(MPU6050_RA_ACCEL_ZOUT_H);
-}
-
-// TEMP_OUT_* registers
-
-/** Get current internal temperature.
- * @return Temperature reading in 16-bit 2's complement format
- * @see MPU6050_RA_TEMP_OUT_H
- */
-int16_t MPU6050::getTemperature()
-{
-	return readReg<int16_t>(MPU6050_RA_TEMP_OUT_H);
-}
-
-// GYRO_*OUT_* registers
-
-/** Get 3-axis gyroscope readings.
- * These gyroscope measurement registers, along with the accelerometer
- * measurement registers, temperature measurement registers, and external sensor
- * data registers, are composed of two sets of registers: an internal register
- * set and a user-facing read register set.
- * The data within the gyroscope sensors' internal register set is always
- * updated at the Sample Rate. Meanwhile, the user-facing read register set
- * duplicates the internal register set's data values whenever the serial
- * interface is idle. This guarantees that a burst read of sensor registers will
- * read measurements from the same sampling instant. Note that if burst reads
- * are not used, the user is responsible for ensuring a set of single byte reads
- * correspond to a single sampling instant by checking the Data Ready interrupt.
- *
- * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
- * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
- * LSB in GYRO_xOUT is shown in the table below:
- *
- * <pre>
- * FS_SEL | Full Scale Range   | LSB Sensitivity
- * -------+--------------------+----------------
- * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
- * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
- * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
- * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
- * </pre>
- *
- * @return container for 3-axis gyro values
- * @see getMotion6()
- * @see MPU6050_RA_GYRO_XOUT_H
- */
-MPU6050::Motion3 MPU6050::getAngularRate()
-{
-	Motion3 angularRate;
-	uint8_t buffer[6] = {0};
-	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
-	angularRate.x = concat(buffer[0], buffer[1]);
-	angularRate.y = concat(buffer[2], buffer[3]);
-	angularRate.z = concat(buffer[4], buffer[5]);
-	return angularRate;
-}
-
-/** Get X-axis gyroscope reading.
- * @return X-axis rotation measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_GYRO_XOUT_H
- */
-int16_t MPU6050::getAngularRateX()
-{
-	return readReg<int16_t>(MPU6050_RA_GYRO_XOUT_H);
-}
-/** Get Y-axis gyroscope reading.
- * @return Y-axis rotation measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_GYRO_YOUT_H
- */
-int16_t MPU6050::getAngularRateY()
-{
-	return readReg<int16_t>(MPU6050_RA_GYRO_YOUT_H);
-}
-/** Get Z-axis gyroscope reading.
- * @return Z-axis rotation measurement in 16-bit 2's complement format
- * @see getMotion6()
- * @see MPU6050_RA_GYRO_ZOUT_H
- */
-int16_t MPU6050::getAngularRateZ()
-{
-	return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
-}
-
-int16_t MPU6050::getAngularRateZ2()
-{
-	return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
-}
-
-// EXT_SENS_DATA_* registers
-
-/** Read single byte from external sensor data register.
- * These registers store data read from external sensors by the Slave 0, 1, 2,
- * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
- * I2C_SLV4_DI (Register 53).
- *
- * External sensor data is written to these registers at the Sample Rate as
- * defined in Register 25. This access rate can be reduced by using the Slave
- * Delay Enable registers (Register 103).
- *
- * External sensor data registers, along with the gyroscope measurement
- * registers, accelerometer measurement registers, and temperature measurement
- * registers, are composed of two sets of registers: an internal register set
- * and a user-facing read register set.
- *
- * The data within the external sensors' internal register set is always updated
- * at the Sample Rate (or the reduced access rate) whenever the serial interface
- * is idle. This guarantees that a burst read of sensor registers will read
- * measurements from the same sampling instant. Note that if burst reads are not
- * used, the user is responsible for ensuring a set of single byte reads
- * correspond to a single sampling instant by checking the Data Ready interrupt.
- *
- * Data is placed in these external sensor data registers according to
- * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
- * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
- * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
- * defined in Register 25) or delayed rate (if specified in Register 52 and
- * 103). During each Sample cycle, slave reads are performed in order of Slave
- * number. If all slaves are enabled with more than zero bytes to be read, the
- * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
- *
- * Each enabled slave will have EXT_SENS_DATA registers associated with it by
- * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
- * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
- * change the higher numbered slaves' associated registers. Furthermore, if
- * fewer total bytes are being read from the external sensors as a result of
- * such a change, then the data remaining in the registers which no longer have
- * an associated slave device (i.e. high numbered registers) will remain in
- * these previously allocated registers unless reset.
- *
- * If the sum of the read lengths of all SLVx transactions exceed the number of
- * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
- * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
- * the slaves cannot be greater than 24 or some bytes will be lost.
- *
- * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
- * information regarding the characteristics of Slave 4, please refer to
- * Registers 49 to 53.
- *
- * EXAMPLE:
- * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
- * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
- * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
- * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
- * will be associated with Slave 1. If Slave 2 is enabled as well, registers
- * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
- *
- * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
- * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
- * instead.
- *
- * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
- * If a slave is disabled at any time, the space initially allocated to the
- * slave in the EXT_SENS_DATA register, will remain associated with that slave.
- * This is to avoid dynamic adjustment of the register allocation.
- *
- * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
- * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
- *
- * This above is also true if one of the slaves gets NACKed and stops
- * functioning.
- *
- * @param position Starting position (0-23)
- * @return Byte read from register
- */
-uint8_t MPU6050::getExternalSensorByte(int position)
-{
-	return readByte(MPU6050_RA_EXT_SENS_DATA_00 + position);
-}
-/** Read word (2 bytes) from external sensor data registers.
- * @param position Starting position (0-21)
- * @return Word read from register
- * @see getExternalSensorByte()
- */
-uint16_t MPU6050::getExternalSensorWord(int position)
-{
-	return readReg<uint16_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
-}
-/** Read double word (4 bytes) from external sensor data registers.
- * @param position Starting position (0-20)
- * @return Double word read from registers
- * @see getExternalSensorByte()
- */
-uint32_t MPU6050::getExternalSensorDWord(int position)
-{
-	return readReg<uint32_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
-}
-
-// MOT_DETECT_STATUS register
-
-/** Get full motion detection status register content (all bits).
- * @return Motion detection status byte
- * @see MPU6050_RA_MOT_DETECT_STATUS
- */
-uint8_t MPU6050::getMotionStatus()
-{
-	return readByte(MPU6050_RA_MOT_DETECT_STATUS);
-}
-/** Get X-axis negative motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_XNEG_BIT
- */
-bool MPU6050::getXNegMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT);
-}
-/** Get X-axis positive motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_XPOS_BIT
- */
-bool MPU6050::getXPosMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT);
-}
-/** Get Y-axis negative motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_YNEG_BIT
- */
-bool MPU6050::getYNegMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT);
-}
-/** Get Y-axis positive motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_YPOS_BIT
- */
-bool MPU6050::getYPosMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT);
-}
-/** Get Z-axis negative motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_ZNEG_BIT
- */
-bool MPU6050::getZNegMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT);
-}
-/** Get Z-axis positive motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_ZPOS_BIT
- */
-bool MPU6050::getZPosMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT);
-}
-/** Get zero motion detection interrupt status.
- * @return Motion detection status
- * @see MPU6050_RA_MOT_DETECT_STATUS
- * @see MPU6050_MOTION_MOT_ZRMOT_BIT
- */
-bool MPU6050::getZeroMotionDetected()
-{
-	return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT);
-}
-
-// I2C_SLV*_DO register
-
-/** Write byte to Data Output container for specified slave.
- * This register holds the output data written into Slave when Slave is set to
- * write mode. For further information regarding Slave control, please
- * refer to Registers 37 to 39 and immediately following.
- * @param num Slave number (0-3)
- * @param data Byte to write
- * @see MPU6050_RA_I2C_SLV0_DO
- */
-void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
-{
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
-}
-
-// I2C_MST_DELAY_CTRL register
-
-/** Get external data shadow delay enabled status.
- * This register is used to specify the timing of external sensor data
- * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
- * sensor data is delayed until all data has been received.
- * @return Current external data shadow delay enabled status.
- * @see MPU6050_RA_I2C_MST_DELAY_CTRL
- * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
- */
-bool MPU6050::getExternalShadowDelayEnabled()
-{
-	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT);
-}
-/** Set external data shadow delay enabled status.
- * @param enabled New external data shadow delay enabled status.
- * @see getExternalShadowDelayEnabled()
- * @see MPU6050_RA_I2C_MST_DELAY_CTRL
- * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
- */
-void MPU6050::setExternalShadowDelayEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled);
-}
-/** Get slave delay enabled status.
- * When a particular slave delay is enabled, the rate of access for the that
- * slave device is reduced. When a slave's access rate is decreased relative to
- * the Sample Rate, the slave is accessed every:
- *
- *     1 / (1 + I2C_MST_DLY) Samples
- *
- * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
- * and DLPF_CFG (register 26).
- *
- * For further information regarding I2C_MST_DLY, please refer to register 52.
- * For further information regarding the Sample Rate, please refer to
- * register 25.
- *
- * @param num Slave number (0-4)
- * @return Current slave delay enabled status.
- * @see MPU6050_RA_I2C_MST_DELAY_CTRL
- * @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 false;
-	}
-	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, num);
-}
-/** Set slave delay enabled status.
- * @param num Slave number (0-4)
- * @param enabled New slave delay enabled status.
- * @see MPU6050_RA_I2C_MST_DELAY_CTRL
- * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
- */
-void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
-}
-
-// SIGNAL_PATH_RESET register
-
-/** Reset gyroscope signal path.
- * The reset will revert the signal path analog to digital converters and
- * filters to their power up configurations.
- * @see MPU6050_RA_SIGNAL_PATH_RESET
- * @see MPU6050_PATHRESET_GYRO_RESET_BIT
- */
-void MPU6050::resetGyroscopePath()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
-}
-/** Reset accelerometer signal path.
- * The reset will revert the signal path analog to digital converters and
- * filters to their power up configurations.
- * @see MPU6050_RA_SIGNAL_PATH_RESET
- * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
- */
-void MPU6050::resetAccelerometerPath()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
-}
-/** Reset temperature sensor signal path.
- * The reset will revert the signal path analog to digital converters and
- * filters to their power up configurations.
- * @see MPU6050_RA_SIGNAL_PATH_RESET
- * @see MPU6050_PATHRESET_TEMP_RESET_BIT
- */
-void MPU6050::resetTemperaturePath()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
-}
-
-// MOT_DETECT_CTRL register
-
-/** Get accelerometer power-on delay.
- * The accelerometer data path provides samples to the sensor registers, Motion
- * detection, Zero Motion detection, and Free Fall detection modules. The
- * signal path contains filters which must be flushed on wake-up with new
- * samples before the detection modules begin operations. The default wake-up
- * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
- * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
- * any value above zero unless instructed otherwise by InvenSense. Please refer
- * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
- * further information regarding the detection modules.
- * @return Current accelerometer power-on delay
- * @see MPU6050_RA_MOT_DETECT_CTRL
- * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
- */
-uint8_t MPU6050::getAccelerometerPowerOnDelay()
-{
-	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
-					MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH);
-}
-/** Set accelerometer power-on delay.
- * @param delay New accelerometer power-on delay (0-3)
- * @see getAccelerometerPowerOnDelay()
- * @see MPU6050_RA_MOT_DETECT_CTRL
- * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
- */
-void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
-					  MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
-}
-/** Get Free Fall detection counter decrement configuration.
- * Detection is registered by the Free Fall detection module after accelerometer
- * measurements meet their respective threshold conditions over a specified
- * number of samples. When the threshold conditions are met, the corresponding
- * detection counter increments by 1. The user may control the rate at which the
- * detection counter decrements when the threshold condition is not met by
- * configuring FF_COUNT. The decrement rate can be set according to the
- * following table:
- *
- * <pre>
- * FF_COUNT | Counter Decrement
- * ---------+------------------
- * 0        | Reset
- * 1        | 1
- * 2        | 2
- * 3        | 4
- * </pre>
- *
- * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
- * reset the counter to 0. For further information on Free Fall detection,
- * please refer to Registers 29 to 32.
- *
- * @return Current decrement configuration
- * @see MPU6050_RA_MOT_DETECT_CTRL
- * @see MPU6050_DETECT_FF_COUNT_BIT
- */
-uint8_t MPU6050::getFreefallDetectionCounterDecrement()
-{
-	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH);
-}
-/** Set Free Fall detection counter decrement configuration.
- * @param decrement New decrement configuration value
- * @see getFreefallDetectionCounterDecrement()
- * @see MPU6050_RA_MOT_DETECT_CTRL
- * @see MPU6050_DETECT_FF_COUNT_BIT
- */
-void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
-					  decrement);
-}
-/** Get Motion detection counter decrement configuration.
- * Detection is registered by the Motion detection module after accelerometer
- * measurements meet their respective threshold conditions over a specified
- * number of samples. When the threshold conditions are met, the corresponding
- * detection counter increments by 1. The user may control the rate at which the
- * detection counter decrements when the threshold condition is not met by
- * configuring MOT_COUNT. The decrement rate can be set according to the
- * following table:
- *
- * <pre>
- * MOT_COUNT | Counter Decrement
- * ----------+------------------
- * 0         | Reset
- * 1         | 1
- * 2         | 2
- * 3         | 4
- * </pre>
- *
- * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
- * reset the counter to 0. For further information on Motion detection,
- * please refer to Registers 29 to 32.
- *
- */
-uint8_t MPU6050::getMotionDetectionCounterDecrement()
-{
-	return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH);
-}
-/** Set Motion detection counter decrement configuration.
- * @param decrement New decrement configuration value
- * @see getMotionDetectionCounterDecrement()
- * @see MPU6050_RA_MOT_DETECT_CTRL
- * @see MPU6050_DETECT_MOT_COUNT_BIT
- */
-void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
-					  MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
-}
-
-// USER_CTRL register
-
-/** Get FIFO enabled status.
- * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
- * cannot be written to or read from while disabled. The FIFO buffer's state
- * does not change unless the MPU-60X0 is power cycled.
- * @return Current FIFO enabled status
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_FIFO_EN_BIT
- */
-bool MPU6050::getFIFOEnabled()
-{
-	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT);
-}
-/** Set FIFO enabled status.
- * @param enabled New FIFO enabled status
- * @see getFIFOEnabled()
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_FIFO_EN_BIT
- */
-void MPU6050::setFIFOEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
-}
-/** Get I2C Master Mode enabled status.
- * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
- * external sensor slave devices on the auxiliary I2C bus. When this bit is
- * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
- * driven by the primary I2C bus (SDA and SCL). This is a precondition to
- * enabling Bypass Mode. For further information regarding Bypass Mode, please
- * refer to Register 55.
- * @return Current I2C Master Mode enabled status
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
- */
-bool MPU6050::getI2CMasterModeEnabled()
-{
-	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT);
-}
-/** Set I2C Master Mode enabled status.
- * @param enabled New I2C Master Mode enabled status
- * @see getI2CMasterModeEnabled()
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
- */
-void MPU6050::setI2CMasterModeEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled);
-}
-/** Switch from I2C to SPI mode (MPU-6000 only)
- * If this is set, the primary SPI interface will be enabled in place of the
- * disabled primary I2C interface.
- */
-void MPU6050::switchSPIEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
-}
-/** Reset the FIFO.
- * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
- * bit automatically clears to 0 after the reset has been triggered.
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_FIFO_RESET_BIT
- */
-void MPU6050::resetFIFO()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true);
-}
-/** Reset the I2C Master.
- * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
- * This bit automatically clears to 0 after the reset has been triggered.
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
- */
-void MPU6050::resetI2CMaster()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true);
-}
-/** Reset all sensor registers and signal paths.
- * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
- * accelerometers, and temperature sensor). This operation will also clear the
- * sensor registers. This bit automatically clears to 0 after the reset has been
- * triggered.
- *
- * When resetting only the signal path (and not the sensor registers), please
- * use Register 104, SIGNAL_PATH_RESET.
- *
- * @see MPU6050_RA_USER_CTRL
- * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
- */
-void MPU6050::resetSensors()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
-}
-
-// PWR_MGMT_1 register
-
-/** Trigger a full device reset.
- * A small delay of ~50ms may be desirable after triggering a reset.
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_DEVICE_RESET_BIT
- */
-void MPU6050::reset()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
-}
-/** Get sleep mode status.
- * Setting the SLEEP bit in the register puts the device into very low power
- * sleep mode. In this mode, only the serial interface and internal registers
- * remain active, allowing for a very low standby current. Clearing this bit
- * puts the device back into normal mode. To save power, the individual standby
- * selections for each of the gyros should be used if any gyro axis is not used
- * by the application.
- * @return Current sleep mode enabled status
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_SLEEP_BIT
- */
-bool MPU6050::getSleepEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT);
-}
-/** Set sleep mode status.
- * @param enabled New sleep mode enabled status
- * @see getSleepEnabled()
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_SLEEP_BIT
- */
-void MPU6050::setSleepEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
-}
-/** Get wake cycle enabled status.
- * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
- * between sleep mode and waking up to take a single sample of data from active
- * sensors at a rate determined by LP_WAKE_CTRL (register 108).
- * @return Current sleep mode enabled status
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_CYCLE_BIT
- */
-bool MPU6050::getWakeCycleEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT);
-}
-/** Set wake cycle enabled status.
- * @param enabled New sleep mode enabled status
- * @see getWakeCycleEnabled()
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_CYCLE_BIT
- */
-void MPU6050::setWakeCycleEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
-}
-/** Get temperature sensor enabled status.
- * Control the usage of the internal temperature sensor.
- *
- * Note: this register stores the *disabled* value, but for consistency with the
- * rest of the code, the function is named and used with standard true/false
- * values to indicate whether the sensor is enabled or disabled, respectively.
- *
- * @return Current temperature sensor enabled status
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_TEMP_DIS_BIT
- */
-bool MPU6050::getTempSensorEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT); // 1 is actually disabled here
-}
-/** Set temperature sensor enabled status.
- * Note: this register stores the *disabled* value, but for consistency with the
- * rest of the code, the function is named and used with standard true/false
- * values to indicate whether the sensor is enabled or disabled, respectively.
- *
- * @param enabled New temperature sensor enabled status
- * @see getTempSensorEnabled()
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_TEMP_DIS_BIT
- */
-void MPU6050::setTempSensorEnabled(bool enabled)
-{
-	// 1 is actually disabled here
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
-}
-/** Get clock source setting.
- * @return Current clock source setting
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_CLKSEL_BIT
- * @see MPU6050_PWR1_CLKSEL_LENGTH
- */
-uint8_t MPU6050::getClockSource()
-{
-	return readBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH);
-}
-/** Set clock source setting.
- * An internal 8MHz oscillator, gyroscope based clock, or external sources can
- * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
- * or an external source is chosen as the clock source, the MPU-60X0 can operate
- * in low power modes with the gyroscopes disabled.
- *
- * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
- * However, it is highly recommended that the device be configured to use one of
- * the gyroscopes (or an external clock source) as the clock reference for
- * improved stability. The clock source can be selected according to the
- * following table:
- *
- * <pre>
- * CLK_SEL | Clock Source
- * --------+--------------------------------------
- * 0       | Internal oscillator
- * 1       | PLL with X Gyro reference
- * 2       | PLL with Y Gyro reference
- * 3       | PLL with Z Gyro reference
- * 4       | PLL with external 32.768kHz reference
- * 5       | PLL with external 19.2MHz reference
- * 6       | Reserved
- * 7       | Stops the clock and keeps the timing generator in reset
- * </pre>
- *
- * @param source New clock source setting
- * @see getClockSource()
- * @see MPU6050_RA_PWR_MGMT_1
- * @see MPU6050_PWR1_CLKSEL_BIT
- * @see MPU6050_PWR1_CLKSEL_LENGTH
- */
-void MPU6050::setClockSource(uint8_t source)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
-}
-
-// PWR_MGMT_2 register
-
-/** Get wake frequency in Accel-Only Low Power Mode.
- * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
- * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
- * the device will power off all devices except for the primary I2C interface,
- * waking only the accelerometer at fixed intervals to take a single
- * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
- * as shown below:
- *
- * <pre>
- * LP_WAKE_CTRL | Wake-up Frequency
- * -------------+------------------
- * 0            | 1.25 Hz
- * 1            | 2.5 Hz
- * 2            | 5 Hz
- * 3            | 10 Hz
- * </pre>
- *
- * For further information regarding the MPU-60X0's power modes, please refer to
- * Register 107.
- *
- * @return Current wake frequency
- * @see MPU6050_RA_PWR_MGMT_2
- */
-uint8_t MPU6050::getWakeFrequency()
-{
-	return readBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH);
-}
-/** Set wake frequency in Accel-Only Low Power Mode.
- * @param frequency New wake frequency
- * @see MPU6050_RA_PWR_MGMT_2
- */
-void MPU6050::setWakeFrequency(uint8_t frequency)
-{
-	I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
-					  frequency);
-}
-
-/** Get X-axis accelerometer standby enabled status.
- * If enabled, the X-axis will not gather or report data (or use power).
- * @return Current X-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_XA_BIT
- */
-bool MPU6050::getStandbyXAccelEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT);
-}
-/** Set X-axis accelerometer standby enabled status.
- * @param New X-axis standby enabled status
- * @see getStandbyXAccelEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_XA_BIT
- */
-void MPU6050::setStandbyXAccelEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled);
-}
-/** Get Y-axis accelerometer standby enabled status.
- * If enabled, the Y-axis will not gather or report data (or use power).
- * @return Current Y-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_YA_BIT
- */
-bool MPU6050::getStandbyYAccelEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT);
-}
-/** Set Y-axis accelerometer standby enabled status.
- * @param New Y-axis standby enabled status
- * @see getStandbyYAccelEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_YA_BIT
- */
-void MPU6050::setStandbyYAccelEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled);
-}
-/** Get Z-axis accelerometer standby enabled status.
- * If enabled, the Z-axis will not gather or report data (or use power).
- * @return Current Z-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_ZA_BIT
- */
-bool MPU6050::getStandbyZAccelEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT);
-}
-/** Set Z-axis accelerometer standby enabled status.
- * @param New Z-axis standby enabled status
- * @see getStandbyZAccelEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_ZA_BIT
- */
-void MPU6050::setStandbyZAccelEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled);
-}
-/** Get X-axis gyroscope standby enabled status.
- * If enabled, the X-axis will not gather or report data (or use power).
- * @return Current X-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_XG_BIT
- */
-bool MPU6050::getStandbyXGyroEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT);
-}
-/** Set X-axis gyroscope standby enabled status.
- * @param New X-axis standby enabled status
- * @see getStandbyXGyroEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_XG_BIT
- */
-void MPU6050::setStandbyXGyroEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled);
-}
-/** Get Y-axis gyroscope standby enabled status.
- * If enabled, the Y-axis will not gather or report data (or use power).
- * @return Current Y-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_YG_BIT
- */
-bool MPU6050::getStandbyYGyroEnabled()
-{
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT);
-}
-/** Set Y-axis gyroscope standby enabled status.
- * @param New Y-axis standby enabled status
- * @see getStandbyYGyroEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_YG_BIT
+ * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
+ * registers in the order they were transferred.
+ *
+ * @param num Slave number (0-3)
+ * @return Current word pair byte-swapping enabled value for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setStandbyYGyroEnabled(bool enabled)
+bool MPU6050::getSlaveWordByteSwap(uint8_t num)
 {
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled);
+	if(num > 3) {
+		return false;
+	}
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
 }
-/** Get Z-axis gyroscope standby enabled status.
- * If enabled, the Z-axis will not gather or report data (or use power).
- * @return Current Z-axis standby enabled status
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_ZG_BIT
+/** Set word pair byte-swapping enabled for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param enabled New word pair byte-swapping enabled value for specified slave
+ * @see getSlaveWordByteSwap()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-bool MPU6050::getStandbyZGyroEnabled()
+void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
 {
-	return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT);
+	if(num > 3) {
+		return;
+	}
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
 }
-/** Set Z-axis gyroscope standby enabled status.
- * @param New Z-axis standby enabled status
- * @see getStandbyZGyroEnabled()
- * @see MPU6050_RA_PWR_MGMT_2
- * @see MPU6050_PWR2_STBY_ZG_BIT
+/** Get write mode for the specified slave (0-3).
+ * When set to 1, the transaction will read or write data only. When cleared to
+ * 0, the transaction will write a register address prior to reading or writing
+ * data. This should equal 0 when specifying the register address within the
+ * Slave device to/from which the ensuing data transaction will take place.
+ *
+ * @param num Slave number (0-3)
+ * @return Current write mode for specified slave (0 = register address + data,
+ * 1 = data only)
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setStandbyZGyroEnabled(bool enabled)
+bool MPU6050::getSlaveWriteMode(uint8_t num)
 {
-	I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+	if(num > 3) {
+		return false;
+	}
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
 }
-
-// FIFO_COUNT* registers
-
-/** Get current FIFO buffer size.
- * This value indicates the number of bytes stored in the FIFO buffer. This
- * number is in turn the number of bytes that can be read from the FIFO buffer
- * and it is directly proportional to the number of samples available given the
- * set of sensor data bound to be stored in the FIFO (register 35 and 36).
- * @return Current FIFO buffer size
+/** Set write mode for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param mode New write mode for specified slave (0 = register address + data,
+ * 1 = data only)
+ * @see getSlaveWriteMode()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-uint16_t MPU6050::getFIFOCount()
+void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
 {
-	uint8_t buffer[2] = {0};
-	I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
-	return (((uint16_t)buffer[0]) << 8) | buffer[1];
+	if(num > 3) {
+		return;
+	}
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
 }
-
-// FIFO_R_W register
-
-/** Get byte from FIFO buffer.
- * This register is used to read and write data from the FIFO buffer. Data is
- * written to the FIFO in order of register number (from lowest to highest). If
- * all the FIFO enable flags (see below) are enabled and all External Sensor
- * Data registers (Registers 73 to 96) are associated with a Slave device, the
- * contents of registers 59 through 96 will be written in order at the Sample
- * Rate.
- *
- * The contents of the sensor data registers (Registers 59 to 96) are written
- * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
- * in FIFO_EN (Register 35). An additional flag for the sensor data registers
- * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
- *
- * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
- * automatically set to 1. This bit is located in INT_STATUS (Register 58).
- * When the FIFO buffer has overflowed, the oldest data will be lost and new
- * data will be written to the FIFO.
- *
- * If the FIFO buffer is empty, reading this register will return the last byte
- * that was previously read from the FIFO until new data is available. The user
- * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
- * empty.
+/** Get word pair grouping order offset for the specified slave (0-3).
+ * This sets specifies the grouping order of word pairs received from registers.
+ * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
+ * then odd register addresses) are paired to form a word. When set to 1, bytes
+ * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
+ * register addresses) are paired to form a word.
  *
- * @return Byte from FIFO buffer
+ * @param num Slave number (0-3)
+ * @return Current word pair grouping order offset for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-uint8_t MPU6050::getFIFOByte()
-{
-	return readByte(MPU6050_RA_FIFO_R_W);
-}
-void MPU6050::getFIFOBytes(uint8_t* data, uint8_t length)
+bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
 {
-	if(length > 0) {
-		I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
-	} else {
-		*data = 0;
+	if(num > 3) {
+		return false;
 	}
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
 }
-/** Write byte to FIFO buffer.
- * @see getFIFOByte()
- * @see MPU6050_RA_FIFO_R_W
+/** Set word pair grouping order offset for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param enabled New word pair grouping order offset for specified slave
+ * @see getSlaveWordGroupOffset()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setFIFOByte(uint8_t data)
+void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
 {
-	I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
+	if(num > 3) {
+		return;
+	}
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
 }
-
-// WHO_AM_I register
-
-/** Get Device ID.
- * This register is used to verify the identity of the device (0b110100, 0x34).
- * @return Device ID (6 bits only! should be 0x34)
- * @see MPU6050_RA_WHO_AM_I
- * @see MPU6050_WHO_AM_I_BIT
- * @see MPU6050_WHO_AM_I_LENGTH
+/** 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
+ * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
+ * @param num Slave number (0-3)
+ * @return Number of bytes to read for specified slave
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-uint8_t MPU6050::getDeviceID()
+uint8_t MPU6050::getSlaveDataLength(uint8_t num)
 {
-	return readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH);
+	if(num > 3) {
+		return false;
+	}
+	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
 }
-/** Set Device ID.
- * Write a new ID into the WHO_AM_I register (no idea why this should ever be
- * necessary though).
- * @param id New device ID to set.
- * @see getDeviceID()
- * @see MPU6050_RA_WHO_AM_I
- * @see MPU6050_WHO_AM_I_BIT
- * @see MPU6050_WHO_AM_I_LENGTH
+/** Set number of bytes to read for the specified slave (0-3).
+ * @param num Slave number (0-3)
+ * @param length Number of bytes to read for specified slave
+ * @see getSlaveDataLength()
+ * @see MPU6050_RA_I2C_SLV0_CTRL
  */
-void MPU6050::setDeviceID(uint8_t id)
+void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
 {
-	I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
+	if(num > 3) {
+		return;
+	}
+	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
+					  length);
 }
 
-// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
-
-// XG_OFFS_TC register
-
-uint8_t MPU6050::getOTPBankValid()
-{
-	return readBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
-}
-void MPU6050::setOTPBankValid(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
-}
-int8_t MPU6050::getXGyroOffsetTC()
-{
-	return readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
-}
-void MPU6050::setXGyroOffsetTC(int8_t offset)
+MPU6050::Motion6 MPU6050::getMotion6()
 {
-	I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	Motion6 motion6;
+	uint8_t buffer[14] = {0};
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
+	motion6.accel.x = concat(buffer[0], buffer[1]);
+	motion6.accel.y = concat(buffer[2], buffer[3]);
+	motion6.accel.z = concat(buffer[4], buffer[5]);
+	motion6.gyro.x = concat(buffer[8], buffer[9]);
+	motion6.gyro.y = concat(buffer[10], buffer[11]);
+	motion6.gyro.z = concat(buffer[12], buffer[13]);
+	return motion6;
 }
 
-// YG_OFFS_TC register
-
-int8_t MPU6050::getYGyroOffsetTC()
-{
-	return readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
-}
-void MPU6050::setYGyroOffsetTC(int8_t offset)
+MPU6050::Motion3 MPU6050::getAcceleration()
 {
-	I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	Motion3 accel;
+	uint8_t buffer[6] = {0};
+	I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer);
+	accel.x = concat(buffer[0], buffer[1]);
+	accel.y = concat(buffer[2], buffer[3]);
+	accel.z = concat(buffer[4], buffer[5]);
+	return accel;
 }
 
-// ZG_OFFS_TC register
-
-int8_t MPU6050::getZGyroOffsetTC()
-{
-	return readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
-}
-void MPU6050::setZGyroOffsetTC(int8_t offset)
+MPU6050::Motion3 MPU6050::getAngularRate()
 {
-	I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	Motion3 angularRate;
+	uint8_t buffer[6] = {0};
+	I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer);
+	angularRate.x = concat(buffer[0], buffer[1]);
+	angularRate.y = concat(buffer[2], buffer[3]);
+	angularRate.z = concat(buffer[4], buffer[5]);
+	return angularRate;
 }
 
-// X_FINE_GAIN register
-
-int8_t MPU6050::getXFineGain()
-{
-	return readByte(MPU6050_RA_X_FINE_GAIN);
-}
-void MPU6050::setXFineGain(int8_t gain)
+void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
 {
-	I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+	if(num > 3) {
+		return;
+	}
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
 }
 
-// Y_FINE_GAIN register
-
-int8_t MPU6050::getYFineGain()
-{
-	return readByte(MPU6050_RA_Y_FINE_GAIN);
-}
-void MPU6050::setYFineGain(int8_t gain)
+bool MPU6050::getSlaveDelayEnabled(uint8_t num)
 {
-	I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+	// MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
+	if(num > 4) {
+		return false;
+	}
+	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, num);
 }
 
-// Z_FINE_GAIN register
-
-int8_t MPU6050::getZFineGain()
-{
-	return readByte(MPU6050_RA_Z_FINE_GAIN);
-}
-void MPU6050::setZFineGain(int8_t gain)
+void MPU6050::getFIFOBytes(uint8_t* data, uint8_t length)
 {
-	I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+	if(length > 0) {
+		I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
+	} else {
+		*data = 0;
+	}
 }
 
 // XA_OFFS_* registers
-
 int16_t MPU6050::getXAccelOffset()
 {
 	uint8_t buffer[2] = {0};
 	I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setXAccelOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
-}
 
 // YA_OFFS_* register
 
@@ -3022,10 +431,6 @@ int16_t MPU6050::getYAccelOffset()
 	I2Cdev::readBytes(devAddr, MPU6050_RA_YA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setYAccelOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
-}
 
 // ZA_OFFS_* register
 
@@ -3035,116 +440,6 @@ int16_t MPU6050::getZAccelOffset()
 	I2Cdev::readBytes(devAddr, MPU6050_RA_ZA_OFFS_H, 2, buffer);
 	return (((int16_t)buffer[0]) << 8) | buffer[1];
 }
-void MPU6050::setZAccelOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
-}
-
-// XG_OFFS_USR* registers
-
-int16_t MPU6050::getXGyroOffset()
-{
-	return readReg<int16_t>(MPU6050_RA_XG_OFFS_USRH);
-}
-void MPU6050::setXGyroOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
-}
-
-// YG_OFFS_USR* register
-
-int16_t MPU6050::getYGyroOffset()
-{
-	return readReg<int16_t>(MPU6050_RA_YG_OFFS_USRH);
-}
-
-void MPU6050::setYGyroOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
-}
-
-// ZG_OFFS_USR* register
-
-int16_t MPU6050::getZGyroOffset()
-{
-	return readReg<int16_t>(MPU6050_RA_ZG_OFFS_USRH);
-}
-
-void MPU6050::setZGyroOffset(int16_t offset)
-{
-	I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
-}
-
-// INT_ENABLE register (DMP functions)
-
-bool MPU6050::getIntPLLReadyEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
-}
-void MPU6050::setIntPLLReadyEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
-}
-bool MPU6050::getIntDMPEnabled()
-{
-	return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT);
-}
-void MPU6050::setIntDMPEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
-}
-
-// DMP_INT_STATUS
-bool MPU6050::getDMPInt5Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT);
-}
-bool MPU6050::getDMPInt4Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT);
-}
-bool MPU6050::getDMPInt3Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT);
-}
-bool MPU6050::getDMPInt2Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT);
-}
-bool MPU6050::getDMPInt1Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT);
-}
-bool MPU6050::getDMPInt0Status()
-{
-	return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT);
-}
-
-// INT_STATUS register (DMP functions)
-
-bool MPU6050::getIntPLLReadyStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
-}
-bool MPU6050::getIntDMPStatus()
-{
-	return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
-}
-
-// USER_CTRL register (DMP functions)
-
-bool MPU6050::getDMPEnabled()
-{
-	return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
-}
-void MPU6050::setDMPEnabled(bool enabled)
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
-}
-void MPU6050::resetDMP()
-{
-	I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
-}
 
 // BANK_SEL register
 
@@ -3384,34 +679,6 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, b
 	}
 	return true;
 }
-bool MPU6050::writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize)
-{
-	return writeDMPConfigurationSet(data, dataSize, true);
-}
-
-// DMP_CFG_1 register
-
-uint8_t MPU6050::getDMPConfig1()
-{
-	return readByte(MPU6050_RA_DMP_CFG_1);
-}
-
-void MPU6050::setDMPConfig1(uint8_t config)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
-}
-
-// DMP_CFG_2 register
-uint8_t MPU6050::getDMPConfig2()
-{
-	return readByte(MPU6050_RA_DMP_CFG_2);
-}
-
-void MPU6050::setDMPConfig2(uint8_t config)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
-}
-
 /**
  * calibration
  *
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 9b012ba41e..0cae21c339 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -12,28 +12,28 @@
 // ANYTHING.
 
 /* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
+   I2Cdev device library code is placed under the MIT license
+   Copyright (c) 2012 Jeff Rowberg
+
+   Permission is hereby granted, free of charge, to any person obtaining a copy
+   of this software and associated documentation files (the "Software"), to deal
+   in the Software without restriction, including without limitation the rights
+   to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+   copies of the Software, and to permit persons to whom the Software is
+   furnished to do so, subject to the following conditions:
+
+   The above copyright notice and this permission notice shall be included in
+   all copies or substantial portions of the Software.
+
+   THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+   IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+   FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+   AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+   LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+   OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+   THE SOFTWARE.
+   ===============================================
+   */
 
 #pragma once
 
@@ -43,22 +43,22 @@ THE SOFTWARE.
 #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
 #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
 
-#define MPU6050_RA_XG_OFFS_TC 0x00  //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
-#define MPU6050_RA_YG_OFFS_TC 0x01  //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
-#define MPU6050_RA_ZG_OFFS_TC 0x02  //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_XG_OFFS_TC 0x00	//[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_YG_OFFS_TC 0x01	//[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU6050_RA_ZG_OFFS_TC 0x02	//[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
 #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN
 #define MPU6050_RA_Y_FINE_GAIN 0x04 //[7:0] Y_FINE_GAIN
 #define MPU6050_RA_Z_FINE_GAIN 0x05 //[7:0] Z_FINE_GAIN
-#define MPU6050_RA_XA_OFFS_H 0x06   //[15:0] XA_OFFS
+#define MPU6050_RA_XA_OFFS_H 0x06	//[15:0] XA_OFFS
 #define MPU6050_RA_XA_OFFS_L_TC 0x07
 #define MPU6050_RA_YA_OFFS_H 0x08 //[15:0] YA_OFFS
 #define MPU6050_RA_YA_OFFS_L_TC 0x09
 #define MPU6050_RA_ZA_OFFS_H 0x0A //[15:0] ZA_OFFS
 #define MPU6050_RA_ZA_OFFS_L_TC 0x0B
-#define MPU6050_RA_SELF_TEST_X 0x0D  //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
-#define MPU6050_RA_SELF_TEST_Y 0x0E  //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
-#define MPU6050_RA_SELF_TEST_Z 0x0F  //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
-#define MPU6050_RA_SELF_TEST_A 0x10  //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
+#define MPU6050_RA_SELF_TEST_X 0x0D	 //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Y 0x0E	 //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_Z 0x0F	 //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
+#define MPU6050_RA_SELF_TEST_A 0x10	 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
 #define MPU6050_RA_XG_OFFS_USRH 0x13 //[15:0] XG_OFFS_USR
 #define MPU6050_RA_XG_OFFS_USRL 0x14
 #define MPU6050_RA_YG_OFFS_USRH 0x15 //[15:0] YG_OFFS_USR
@@ -433,126 +433,893 @@ class MPU6050
 		size_t printTo(Print& p) const;
 	};
 	/** Default constructor, uses default I2C address.
-     * @see MPU6050_DEFAULT_ADDRESS
-     */
+         * @see MPU6050_DEFAULT_ADDRESS
+         */
 	MPU6050() : devAddr{MPU6050_DEFAULT_ADDRESS}
 	{
 	}
 
 	/** Specific address constructor.
-     * @param address I2C address
-     * @see MPU6050_DEFAULT_ADDRESS
-     * @see MPU6050_ADDRESS_AD0_LOW
-     * @see MPU6050_ADDRESS_AD0_HIGH
-     */
+         * @param address I2C address
+         * @see MPU6050_DEFAULT_ADDRESS
+         * @see MPU6050_ADDRESS_AD0_LOW
+         * @see MPU6050_ADDRESS_AD0_HIGH
+         */
 	MPU6050(uint8_t address) : devAddr{address}
 	{
 	}
 
+	/** Power on and prepare for general usage.
+         * This will activate the device and take it out of sleep mode (which must be
+         * done after start-up). This function also sets both the accelerometer and the
+         * gyroscope to their most sensitive settings, namely +/- 2g and +/- 250
+         * degrees/sec, and sets the clock source to use the X Gyro for reference, which
+         * is slightly better than the default internal clock source.
+         */
 	void initialize();
 
 	/** Verify the I2C connection.
-     * Make sure the device is connected and responds as expected.
-     * @return True if connection is valid, false otherwise
-     */
+         * Make sure the device is connected and responds as expected.
+         * @return True if connection is valid, false otherwise
+         */
 	bool testConnection()
 	{
 		return getDeviceID() == 0x34;
 	}
 
-	// AUX_VDDIO register
-	uint8_t getAuxVDDIOLevel();
-	void setAuxVDDIOLevel(uint8_t level);
+	// AUX_VDDIO register (InvenSense demo code calls this RA_*G_OFFS_TC)
+	/** Get the auxiliary I2C supply voltage level.
+         * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
+         * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
+         * the MPU-6000, which does not have a VLOGIC pin.
+         * @return I2C supply voltage level (0=VLOGIC, 1=VDD)
+         */
+	uint8_t getAuxVDDIOLevel()
+	{
+		return readBit(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT);
+	}
+
+	/** Set the auxiliary I2C supply voltage level.
+         * When set to 1, the auxiliary I2C bus high logic level is VDD. When cleared to
+         * 0, the auxiliary I2C bus high logic level is VLOGIC. This does not apply to
+         * the MPU-6000, which does not have a VLOGIC pin.
+         * @param level I2C supply voltage level (0=VLOGIC, 1=VDD)
+         */
+	void setAuxVDDIOLevel(uint8_t level)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level);
+	}
 
 	// SMPLRT_DIV register
-	uint8_t getRate();
-	void setRate(uint8_t rate);
+	/** Get gyroscope output rate divider.
+         * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero
+         * Motion detection, and Free Fall detection are all based on the Sample Rate.
+         * The Sample Rate is generated by dividing the gyroscope output rate by
+         * SMPLRT_DIV:
+         *
+         * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
+         *
+         * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or
+         * 7), and 1kHz when the DLPF is enabled (see Register 26).
+         *
+         * Note: The accelerometer output rate is 1kHz. This means that for a Sample
+         * Rate greater than 1kHz, the same accelerometer sample may be output to the
+         * FIFO, DMP, and sensor registers more than once.
+         *
+         * For a diagram of the gyroscope and accelerometer signal paths, see Section 8
+         * of the MPU-6000/MPU-6050 Product Specification document.
+         *
+         * @return Current sample rate
+         * @see MPU6050_RA_SMPLRT_DIV
+         */
+	uint8_t getRate()
+	{
+		return readByte(MPU6050_RA_SMPLRT_DIV);
+	}
+
+	/** Set gyroscope sample rate divider.
+         * @param rate New sample rate divider
+         * @see getRate()
+         * @see MPU6050_RA_SMPLRT_DIV
+         */
+	void setRate(uint8_t rate)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
+	}
 
 	// CONFIG register
-	uint8_t getExternalFrameSync();
-	void setExternalFrameSync(uint8_t sync);
-	uint8_t getDLPFMode();
-	void setDLPFMode(uint8_t bandwidth);
+
+	/** Get external FSYNC configuration.
+         * Configures the external Frame Synchronization (FSYNC) pin sampling. An
+         * external signal connected to the FSYNC pin can be sampled by configuring
+         * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
+         * strobes may be captured. The latched FSYNC signal will be sampled at the
+         * Sampling Rate, as defined in register 25. After sampling, the latch will
+         * reset to the current FSYNC signal state.
+         *
+         * The sampled value will be reported in place of the least significant bit in
+         * a sensor data register determined by the value of EXT_SYNC_SET according to
+         * the following table.
+         *
+         * <pre>
+         * EXT_SYNC_SET | FSYNC Bit Location
+         * -------------+-------------------
+         * 0            | Input disabled
+         * 1            | TEMP_OUT_L[0]
+         * 2            | GYRO_XOUT_L[0]
+         * 3            | GYRO_YOUT_L[0]
+         * 4            | GYRO_ZOUT_L[0]
+         * 5            | ACCEL_XOUT_L[0]
+         * 6            | ACCEL_YOUT_L[0]
+         * 7            | ACCEL_ZOUT_L[0]
+         * </pre>
+         *
+         * @return FSYNC configuration value
+         */
+	uint8_t getExternalFrameSync()
+	{
+		return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH);
+	}
+	/** Set external FSYNC configuration.
+         * @see getExternalFrameSync()
+         * @see MPU6050_RA_CONFIG
+         * @param sync New FSYNC configuration value
+         */
+	void setExternalFrameSync(uint8_t sync)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, 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
+         * also determines the internal sampling rate used by the device as shown in
+         * the table below.
+         *
+         * Note: The accelerometer output rate is 1kHz. This means that for a Sample
+         * Rate greater than 1kHz, the same accelerometer sample may be output to the
+         * FIFO, DMP, and sensor registers more than once.
+         *
+         * <pre>
+         *          |   ACCELEROMETER    |           GYROSCOPE
+         * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
+         * ---------+-----------+--------+-----------+--------+-------------
+         * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
+         * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
+         * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
+         * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
+         * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
+         * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
+         * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
+         * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
+         * </pre>
+         *
+         * @return DLFP configuration
+         * @see MPU6050_RA_CONFIG
+         * @see MPU6050_CFG_DLPF_CFG_BIT
+         * @see MPU6050_CFG_DLPF_CFG_LENGTH
+         */
+	uint8_t getDLPFMode()
+	{
+		return readBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH);
+	}
+	/** Set digital low-pass filter configuration.
+         * @param mode New DLFP configuration setting
+         * @see getDLPFBandwidth()
+         * @see MPU6050_DLPF_BW_256
+         * @see MPU6050_RA_CONFIG
+         * @see MPU6050_CFG_DLPF_CFG_BIT
+         * @see MPU6050_CFG_DLPF_CFG_LENGTH
+         */
+	void setDLPFMode(uint8_t mode)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
+	}
 
 	// GYRO_CONFIG register
-	uint8_t getFullScaleGyroRange();
-	void setFullScaleGyroRange(uint8_t range);
+	/** Get full-scale gyroscope range.
+         * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
+         * as described in the table below.
+         *
+         * <pre>
+         * 0 = +/- 250 degrees/sec
+         * 1 = +/- 500 degrees/sec
+         * 2 = +/- 1000 degrees/sec
+         * 3 = +/- 2000 degrees/sec
+         * </pre>
+         *
+         * @return Current full-scale gyroscope range setting
+         * @see MPU6050_GYRO_FS_250
+         * @see MPU6050_RA_GYRO_CONFIG
+         * @see MPU6050_GCONFIG_FS_SEL_BIT
+         * @see MPU6050_GCONFIG_FS_SEL_LENGTH
+         */
+	uint8_t getFullScaleGyroRange()
+	{
+		return readBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH);
+	}
 
-	// SELF_TEST registers
+	/** Set full-scale gyroscope range.
+         * @param range New full-scale gyroscope range value
+         * @see getFullScaleRange()
+         * @see MPU6050_GYRO_FS_250
+         * @see MPU6050_RA_GYRO_CONFIG
+         * @see MPU6050_GCONFIG_FS_SEL_BIT
+         * @see MPU6050_GCONFIG_FS_SEL_LENGTH
+         */
+	void setFullScaleGyroRange(uint8_t range)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
+						  range);
+	}
+
+	// SELF TEST FACTORY TRIM VALUES
+	/** Get self-test factory trim value for accelerometer X axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_X
+         */
 	uint8_t getAccelXSelfTestFactoryTrim();
+
+	/** Get self-test factory trim value for accelerometer Y axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_Y
+         */
 	uint8_t getAccelYSelfTestFactoryTrim();
+
+	/** Get self-test factory trim value for accelerometer Z axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_Z
+         */
 	uint8_t getAccelZSelfTestFactoryTrim();
 
+	/** Get self-test factory trim value for gyro X axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_X
+         */
 	uint8_t getGyroXSelfTestFactoryTrim();
+
+	/** Get self-test factory trim value for gyro Y axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_Y
+         */
 	uint8_t getGyroYSelfTestFactoryTrim();
+
+	/** Get self-test factory trim value for gyro Z axis.
+         * @return factory trim value
+         * @see MPU6050_RA_SELF_TEST_Z
+         */
 	uint8_t getGyroZSelfTestFactoryTrim();
 
 	// ACCEL_CONFIG register
-	bool getAccelXSelfTest();
-	void setAccelXSelfTest(bool enabled);
-	bool getAccelYSelfTest();
-	void setAccelYSelfTest(bool enabled);
-	bool getAccelZSelfTest();
-	void setAccelZSelfTest(bool enabled);
-	uint8_t getFullScaleAccelRange();
-	void setFullScaleAccelRange(uint8_t range);
-	uint8_t getDHPFMode();
-	void setDHPFMode(uint8_t mode);
+
+	/** Get self-test enabled setting for accelerometer X axis.
+         * @return Self-test enabled value
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	bool getAccelXSelfTest()
+	{
+		return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT);
+	}
+	/** Get self-test enabled setting for accelerometer X axis.
+         * @param enabled Self-test enabled value
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	void setAccelXSelfTest(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, 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 getAccelYSelfTest()
+	{
+		return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT);
+	}
+	/** Get self-test enabled value for accelerometer Y axis.
+         * @param enabled Self-test enabled value
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	void setAccelYSelfTest(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, 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 getAccelZSelfTest()
+	{
+		return readBit(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT);
+	}
+	/** Set self-test enabled value for accelerometer Z axis.
+         * @param enabled Self-test enabled value
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	void setAccelZSelfTest(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled);
+	}
+	/** Get full-scale accelerometer range.
+         * The FS_SEL parameter allows setting the full-scale range of the accelerometer
+         * sensors, as described in the table below.
+         *
+         * <pre>
+         * 0 = +/- 2g
+         * 1 = +/- 4g
+         * 2 = +/- 8g
+         * 3 = +/- 16g
+         * </pre>
+         *
+         * @return Current full-scale accelerometer range setting
+         * @see MPU6050_ACCEL_FS_2
+         * @see MPU6050_RA_ACCEL_CONFIG
+         * @see MPU6050_ACONFIG_AFS_SEL_BIT
+         * @see MPU6050_ACONFIG_AFS_SEL_LENGTH
+         */
+	uint8_t getFullScaleAccelRange()
+	{
+		return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH);
+	}
+	/** Set full-scale accelerometer range.
+         * @param range New full-scale accelerometer range setting
+         * @see getFullScaleAccelRange()
+         */
+	void setFullScaleAccelRange(uint8_t range)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, 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
+         * Fall, Motion threshold, and Zero Motion). The high pass filter output is not
+         * available to the data registers (see Figure in Section 8 of the MPU-6000/
+         * MPU-6050 Product Specification document).
+         *
+         * The high pass filter has three modes:
+         *
+         * <pre>
+         *    Reset: The filter output settles to zero within one sample. This
+         *           effectively disables the high pass filter. This mode may be toggled
+         *           to quickly settle the filter.
+         *
+         *    On:    The high pass filter will pass signals above the cut off frequency.
+         *
+         *    Hold:  When triggered, the filter holds the present sample. The filter
+         *           output will be the difference between the input sample and the held
+         *           sample.
+         * </pre>
+         *
+         * <pre>
+         * ACCEL_HPF | Filter Mode | Cut-off Frequency
+         * ----------+-------------+------------------
+         * 0         | Reset       | None
+         * 1         | On          | 5Hz
+         * 2         | On          | 2.5Hz
+         * 3         | On          | 1.25Hz
+         * 4         | On          | 0.63Hz
+         * 7         | Hold        | None
+         * </pre>
+         *
+         * @return Current high-pass filter configuration
+         * @see MPU6050_DHPF_RESET
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	uint8_t getDHPFMode()
+	{
+		return readBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH);
+	}
+	/** Set the high-pass filter configuration.
+         * @param bandwidth New high-pass filter configuration
+         * @see setDHPFMode()
+         * @see MPU6050_DHPF_RESET
+         * @see MPU6050_RA_ACCEL_CONFIG
+         */
+	void setDHPFMode(uint8_t bandwidth)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT,
+						  MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth);
+	}
 
 	// FF_THR register
-	uint8_t getFreefallDetectionThreshold();
-	void setFreefallDetectionThreshold(uint8_t threshold);
+
+	/** Get free-fall event acceleration threshold.
+         * This register configures the detection threshold for Free Fall event
+         * detection. The unit of FF_THR is 1LSB = 2mg. Free Fall is detected when the
+         * absolute value of the accelerometer measurements for the three axes are each
+         * less than the detection threshold. This condition increments the Free Fall
+         * duration counter (Register 30). The Free Fall interrupt is triggered when the
+         * Free Fall duration counter reaches the time specified in FF_DUR.
+         *
+         * For more details on the Free Fall detection interrupt, see Section 8.2 of the
+         * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
+         * 58 of this document.
+         *
+         * @return Current free-fall acceleration threshold value (LSB = 2mg)
+         * @see MPU6050_RA_FF_THR
+         */
+	uint8_t getFreefallDetectionThreshold()
+	{
+		return readByte(MPU6050_RA_FF_THR);
+	}
+	/** Get free-fall event acceleration threshold.
+         * @param threshold New free-fall acceleration threshold value (LSB = 2mg)
+         * @see getFreefallDetectionThreshold()
+         * @see MPU6050_RA_FF_THR
+         */
+	void setFreefallDetectionThreshold(uint8_t threshold)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold);
+	}
 
 	// FF_DUR register
-	uint8_t getFreefallDetectionDuration();
-	void setFreefallDetectionDuration(uint8_t duration);
+
+	/** Get free-fall event duration threshold.
+         * This register configures the duration counter threshold for Free Fall event
+         * detection. The duration counter ticks at 1kHz, therefore FF_DUR has a unit
+         * of 1 LSB = 1 ms.
+         *
+         * The Free Fall duration counter increments while the absolute value of the
+         * accelerometer measurements are each less than the detection threshold
+         * (Register 29). The Free Fall interrupt is triggered when the Free Fall
+         * duration counter reaches the time specified in this register.
+         *
+         * For more details on the Free Fall detection interrupt, see Section 8.2 of
+         * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
+         * and 58 of this document.
+         *
+         * @return Current free-fall duration threshold value (LSB = 1ms)
+         * @see MPU6050_RA_FF_DUR
+         */
+	uint8_t getFreefallDetectionDuration()
+	{
+		return readByte(MPU6050_RA_FF_DUR);
+	}
+	/** Get free-fall event duration threshold.
+         * @param duration New free-fall duration threshold value (LSB = 1ms)
+         * @see getFreefallDetectionDuration()
+         * @see MPU6050_RA_FF_DUR
+         */
+	void setFreefallDetectionDuration(uint8_t duration)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration);
+	}
 
 	// MOT_THR register
-	uint8_t getMotionDetectionThreshold();
-	void setMotionDetectionThreshold(uint8_t threshold);
+
+	/** Get motion detection event acceleration threshold.
+         * This register configures the detection threshold for Motion interrupt
+         * generation. The unit of MOT_THR is 1LSB = 2mg. Motion is detected when the
+         * absolute value of any of the accelerometer measurements exceeds this Motion
+         * detection threshold. This condition increments the Motion detection duration
+         * counter (Register 32). The Motion detection interrupt is triggered when the
+         * Motion Detection counter reaches the time count specified in MOT_DUR
+         * (Register 32).
+         *
+         * The Motion interrupt will indicate the axis and polarity of detected motion
+         * in MOT_DETECT_STATUS (Register 97).
+         *
+         * For more details on the Motion detection interrupt, see Section 8.3 of the
+         * MPU-6000/MPU-6050 Product Specification document as well as Registers 56 and
+         * 58 of this document.
+         *
+         * @return Current motion detection acceleration threshold value (LSB = 2mg)
+         * @see MPU6050_RA_MOT_THR
+         */
+	uint8_t getMotionDetectionThreshold()
+	{
+		return readByte(MPU6050_RA_MOT_THR);
+	}
+	/** Set motion detection event acceleration threshold.
+         * @param threshold New motion detection acceleration threshold value (LSB =
+         * 2mg)
+         * @see getMotionDetectionThreshold()
+         * @see MPU6050_RA_MOT_THR
+         */
+	void setMotionDetectionThreshold(uint8_t threshold)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold);
+	}
 
 	// MOT_DUR register
-	uint8_t getMotionDetectionDuration();
-	void setMotionDetectionDuration(uint8_t duration);
+
+	/** Get motion detection event duration threshold.
+         * This register configures the duration counter threshold for Motion interrupt
+         * generation. The duration counter ticks at 1 kHz, therefore MOT_DUR has a unit
+         * of 1LSB = 1ms. The Motion detection duration counter increments when the
+         * absolute value of any of the accelerometer measurements exceeds the Motion
+         * detection threshold (Register 31). The Motion detection interrupt is
+         * triggered when the Motion detection counter reaches the time count specified
+         * in this register.
+         *
+         * For more details on the Motion detection interrupt, see Section 8.3 of the
+         * MPU-6000/MPU-6050 Product Specification document.
+         *
+         * @return Current motion detection duration threshold value (LSB = 1ms)
+         * @see MPU6050_RA_MOT_DUR
+         */
+	uint8_t getMotionDetectionDuration()
+	{
+		return readByte(MPU6050_RA_MOT_DUR);
+	}
+	/** Set motion detection event duration threshold.
+         * @param duration New motion detection duration threshold value (LSB = 1ms)
+         * @see getMotionDetectionDuration()
+         * @see MPU6050_RA_MOT_DUR
+         */
+	void setMotionDetectionDuration(uint8_t duration)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration);
+	}
 
 	// ZRMOT_THR register
-	uint8_t getZeroMotionDetectionThreshold();
-	void setZeroMotionDetectionThreshold(uint8_t threshold);
+
+	/** Get zero motion detection event acceleration threshold.
+         * This register configures the detection threshold for Zero Motion interrupt
+         * generation. The unit of ZRMOT_THR is 1LSB = 2mg. Zero Motion is detected when
+         * the absolute value of the accelerometer measurements for the 3 axes are each
+         * less than the detection threshold. This condition increments the Zero Motion
+         * duration counter (Register 34). The Zero Motion interrupt is triggered when
+         * the Zero Motion duration counter reaches the time count specified in
+         * ZRMOT_DUR (Register 34).
+         *
+         * Unlike Free Fall or Motion detection, Zero Motion detection triggers an
+         * interrupt both when Zero Motion is first detected and when Zero Motion is no
+         * longer detected.
+         *
+         * When a zero motion event is detected, a Zero Motion Status will be indicated
+         * in the MOT_DETECT_STATUS register (Register 97). When a motion-to-zero-motion
+         * condition is detected, the status bit is set to 1. When a zero-motion-to-
+         * motion condition is detected, the status bit is set to 0.
+         *
+         * For more details on the Zero Motion detection interrupt, see Section 8.4 of
+         * the MPU-6000/MPU-6050 Product Specification document as well as Registers 56
+         * and 58 of this document.
+         *
+         * @return Current zero motion detection acceleration threshold value (LSB =
+         * 2mg)
+         * @see MPU6050_RA_ZRMOT_THR
+         */
+	uint8_t getZeroMotionDetectionThreshold()
+	{
+		return readByte(MPU6050_RA_ZRMOT_THR);
+	}
+	/** Set zero motion detection event acceleration threshold.
+         * @param threshold New zero motion detection acceleration threshold value (LSB
+         * = 2mg)
+         * @see getZeroMotionDetectionThreshold()
+         * @see MPU6050_RA_ZRMOT_THR
+         */
+	void setZeroMotionDetectionThreshold(uint8_t threshold)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold);
+	}
 
 	// ZRMOT_DUR register
-	uint8_t getZeroMotionDetectionDuration();
-	void setZeroMotionDetectionDuration(uint8_t duration);
+
+	/** Get zero motion detection event duration threshold.
+         * This register configures the duration counter threshold for Zero Motion
+         * interrupt generation. The duration counter ticks at 16 Hz, therefore
+         * ZRMOT_DUR has a unit of 1 LSB = 64 ms. The Zero Motion duration counter
+         * increments while the absolute value of the accelerometer measurements are
+         * each less than the detection threshold (Register 33). The Zero Motion
+         * interrupt is triggered when the Zero Motion duration counter reaches the time
+         * count specified in this register.
+         *
+         * For more details on the Zero Motion detection interrupt, see Section 8.4 of
+         * the MPU-6000/MPU-6050 Product Specification document, as well as Registers 56
+         * and 58 of this document.
+         *
+         * @return Current zero motion detection duration threshold value (LSB = 64ms)
+         * @see MPU6050_RA_ZRMOT_DUR
+         */
+	uint8_t getZeroMotionDetectionDuration()
+	{
+		return readByte(MPU6050_RA_ZRMOT_DUR);
+	}
+	/** Set zero motion detection event duration threshold.
+         * @param duration New zero motion detection duration threshold value (LSB =
+         * 1ms)
+         * @see getZeroMotionDetectionDuration()
+         * @see MPU6050_RA_ZRMOT_DUR
+         */
+	void setZeroMotionDetectionDuration(uint8_t duration)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration);
+	}
 
 	// FIFO_EN register
-	bool getTempFIFOEnabled();
-	void setTempFIFOEnabled(bool enabled);
-	bool getXGyroFIFOEnabled();
-	void setXGyroFIFOEnabled(bool enabled);
-	bool getYGyroFIFOEnabled();
-	void setYGyroFIFOEnabled(bool enabled);
-	bool getZGyroFIFOEnabled();
-	void setZGyroFIFOEnabled(bool enabled);
-	bool getAccelFIFOEnabled();
-	void setAccelFIFOEnabled(bool enabled);
-	bool getSlave2FIFOEnabled();
-	void setSlave2FIFOEnabled(bool enabled);
-	bool getSlave1FIFOEnabled();
-	void setSlave1FIFOEnabled(bool enabled);
-	bool getSlave0FIFOEnabled();
-	void setSlave0FIFOEnabled(bool enabled);
+
+	/** Get temperature FIFO enabled value.
+         * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
+         * 66) to be written into the FIFO buffer.
+         * @return Current temperature FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getTempFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT);
+	}
+	/** Set temperature FIFO enabled value.
+         * @param enabled New temperature FIFO enabled value
+         * @see getTempFIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setTempFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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
+         * 68) to be written into the FIFO buffer.
+         * @return Current gyroscope X-axis FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getXGyroFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT);
+	}
+	/** Set gyroscope X-axis FIFO enabled value.
+         * @param enabled New gyroscope X-axis FIFO enabled value
+         * @see getXGyroFIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setXGyroFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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
+         * 70) to be written into the FIFO buffer.
+         * @return Current gyroscope Y-axis FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getYGyroFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT);
+	}
+	/** Set gyroscope Y-axis FIFO enabled value.
+         * @param enabled New gyroscope Y-axis FIFO enabled value
+         * @see getYGyroFIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setYGyroFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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
+         * 72) to be written into the FIFO buffer.
+         * @return Current gyroscope Z-axis FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getZGyroFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT);
+	}
+	/** Set gyroscope Z-axis FIFO enabled value.
+         * @param enabled New gyroscope Z-axis FIFO enabled value
+         * @see getZGyroFIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setZGyroFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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,
+         * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
+         * written into the FIFO buffer.
+         * @return Current accelerometer FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getAccelFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT);
+	}
+	/** Set accelerometer FIFO enabled value.
+         * @param enabled New accelerometer FIFO enabled value
+         * @see getAccelFIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setAccelFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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)
+         * associated with Slave 2 to be written into the FIFO buffer.
+         * @return Current Slave 2 FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getSlave2FIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT);
+	}
+	/** Set Slave 2 FIFO enabled value.
+         * @param enabled New Slave 2 FIFO enabled value
+         * @see getSlave2FIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setSlave2FIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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)
+         * associated with Slave 1 to be written into the FIFO buffer.
+         * @return Current Slave 1 FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getSlave1FIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT);
+	}
+	/** Set Slave 1 FIFO enabled value.
+         * @param enabled New Slave 1 FIFO enabled value
+         * @see getSlave1FIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setSlave1FIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, 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)
+         * associated with Slave 0 to be written into the FIFO buffer.
+         * @return Current Slave 0 FIFO enabled value
+         * @see MPU6050_RA_FIFO_EN
+         */
+	bool getSlave0FIFOEnabled()
+	{
+		return readBit(MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT);
+	}
+	/** Set Slave 0 FIFO enabled value.
+         * @param enabled New Slave 0 FIFO enabled value
+         * @see getSlave0FIFOEnabled()
+         * @see MPU6050_RA_FIFO_EN
+         */
+	void setSlave0FIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled);
+	}
 
 	// I2C_MST_CTRL register
-	bool getMultiMasterEnabled();
-	void setMultiMasterEnabled(bool enabled);
-	bool getWaitForExternalSensorEnabled();
-	void setWaitForExternalSensorEnabled(bool enabled);
-	bool getSlave3FIFOEnabled();
-	void setSlave3FIFOEnabled(bool enabled);
-	bool getSlaveReadWriteTransitionEnabled();
-	void setSlaveReadWriteTransitionEnabled(bool enabled);
-	uint8_t getMasterClockSpeed();
-	void setMasterClockSpeed(uint8_t speed);
 
+	/** Get multi-master enabled value.
+         * Multi-master capability allows multiple I2C masters to operate on the same
+         * bus. In circuits where multi-master capability is required, set MULT_MST_EN
+         * to 1. This will increase current drawn by approximately 30uA.
+         *
+         * In circuits where multi-master capability is required, the state of the I2C
+         * bus must always be monitored by each separate I2C Master. Before an I2C
+         * Master can assume arbitration of the bus, it must first confirm that no other
+         * I2C Master has arbitration of the bus. When MULT_MST_EN is set to 1, the
+         * MPU-60X0's bus arbitration detection logic is turned on, enabling it to
+         * detect when the bus is available.
+         *
+         * @return Current multi-master enabled value
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	bool getMultiMasterEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT);
+	}
+	/** Set multi-master enabled value.
+         * @param enabled New multi-master enabled value
+         * @see getMultiMasterEnabled()
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	void setMultiMasterEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, 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
+         * delayed until External Sensor data from the Slave Devices are loaded into the
+         * EXT_SENS_DATA registers. This is used to ensure that both the internal sensor
+         * data (i.e. from gyro and accel) and external sensor data have been loaded to
+         * their respective data registers (i.e. the data is synced) when the Data Ready
+         * interrupt is triggered.
+         *
+         * @return Current wait-for-external-sensor-data enabled value
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	bool getWaitForExternalSensorEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT);
+	}
+	/** Set wait-for-external-sensor-data enabled value.
+         * @param enabled New wait-for-external-sensor-data enabled value
+         * @see getWaitForExternalSensorEnabled()
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	void setWaitForExternalSensorEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, 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)
+         * associated with Slave 3 to be written into the FIFO buffer.
+         * @return Current Slave 3 FIFO enabled value
+         * @see MPU6050_RA_MST_CTRL
+         */
+	bool getSlave3FIFOEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT);
+	}
+	/** Set Slave 3 FIFO enabled value.
+         * @param enabled New Slave 3 FIFO enabled value
+         * @see getSlave3FIFOEnabled()
+         * @see MPU6050_RA_MST_CTRL
+         */
+	void setSlave3FIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, 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
+         * read to the next slave read. If the bit equals 0, there will be a restart
+         * between reads. If the bit equals 1, there will be a stop followed by a start
+         * of the following read. When a write transaction follows a read transaction,
+         * the stop followed by a start of the successive write will be always used.
+         *
+         * @return Current slave read/write transition enabled value
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	bool getSlaveReadWriteTransitionEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT);
+	}
+	/** Set slave read/write transition enabled value.
+         * @param enabled New slave read/write transition enabled value
+         * @see getSlaveReadWriteTransitionEnabled()
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	void setSlaveReadWriteTransitionEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, 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
+         * MPU-60X0 internal 8MHz clock. It sets the I2C master clock speed according to
+         * the following table:
+         *
+         * <pre>
+         * I2C_MST_CLK | I2C Master Clock Speed | 8MHz Clock Divider
+         * ------------+------------------------+-------------------
+         * 0           | 348kHz                 | 23
+         * 1           | 333kHz                 | 24
+         * 2           | 320kHz                 | 25
+         * 3           | 308kHz                 | 26
+         * 4           | 296kHz                 | 27
+         * 5           | 286kHz                 | 28
+         * 6           | 276kHz                 | 29
+         * 7           | 267kHz                 | 30
+         * 8           | 258kHz                 | 31
+         * 9           | 500kHz                 | 16
+         * 10          | 471kHz                 | 17
+         * 11          | 444kHz                 | 18
+         * 12          | 421kHz                 | 19
+         * 13          | 400kHz                 | 20
+         * 14          | 381kHz                 | 21
+         * 15          | 364kHz                 | 22
+         * </pre>
+         *
+         * @return Current I2C master clock speed
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	uint8_t getMasterClockSpeed()
+	{
+		return readBits(MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH);
+	}
+	/** Set I2C master clock speed.
+         * @reparam speed Current I2C master clock speed
+         * @see MPU6050_RA_I2C_MST_CTRL
+         */
+	void setMasterClockSpeed(uint8_t speed)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
+	}
 	// I2C_SLV* registers (Slave 0-3)
 	uint8_t getSlaveAddress(uint8_t num);
 	void setSlaveAddress(uint8_t num, uint8_t address);
@@ -570,253 +1337,1882 @@ class MPU6050
 	void setSlaveDataLength(uint8_t num, uint8_t length);
 
 	// I2C_SLV* registers (Slave 4)
-	uint8_t getSlave4Address();
-	void setSlave4Address(uint8_t address);
-	uint8_t getSlave4Register();
-	void setSlave4Register(uint8_t reg);
-	void setSlave4OutputByte(uint8_t data);
-	bool getSlave4Enabled();
-	void setSlave4Enabled(bool enabled);
-	bool getSlave4InterruptEnabled();
-	void setSlave4InterruptEnabled(bool enabled);
-	bool getSlave4WriteMode();
-	void setSlave4WriteMode(bool mode);
-	uint8_t getSlave4MasterDelay();
-	void setSlave4MasterDelay(uint8_t delay);
-	uint8_t getSlate4InputByte();
+
+	/** Get the I2C address of Slave 4.
+         * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
+         * operation, and if it is cleared, then it's a write operation. The remaining
+         * bits (6-0) are the 7-bit device address of the slave device.
+         *
+         * @return Current address for Slave 4
+         * @see getSlaveAddress()
+         * @see MPU6050_RA_I2C_SLV4_ADDR
+         */
+	uint8_t getSlave4Address()
+	{
+		return readByte(MPU6050_RA_I2C_SLV4_ADDR);
+	}
+	/** Set the I2C address of Slave 4.
+         * @param address New address for Slave 4
+         * @see getSlave4Address()
+         * @see MPU6050_RA_I2C_SLV4_ADDR
+         */
+	void setSlave4Address(uint8_t address)
+	{
+		I2Cdev::writeByte(devAddr, 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
+         * register address is stored in this MPU register.
+         *
+         * @return Current active register for Slave 4
+         * @see MPU6050_RA_I2C_SLV4_REG
+         */
+	uint8_t getSlave4Register()
+	{
+		return readByte(MPU6050_RA_I2C_SLV4_REG);
+	}
+	/** Set the active internal register for Slave 4.
+         * @param reg New active register for Slave 4
+         * @see getSlave4Register()
+         * @see MPU6050_RA_I2C_SLV4_REG
+         */
+	void setSlave4Register(uint8_t reg)
+	{
+		I2Cdev::writeByte(devAddr, 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
+         * is set 1 (set to read), this register has no effect.
+         * @param data New byte to write to Slave 4
+         * @see MPU6050_RA_I2C_SLV4_DO
+         */
+	void setSlave4OutputByte(uint8_t data)
+	{
+		I2Cdev::writeByte(devAddr, 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
+         * cleared to 0, this bit disables Slave 4 from data transfer operations.
+         * @return Current enabled value for Slave 4
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	bool getSlave4Enabled()
+	{
+		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT);
+	}
+	/** Set the enabled value for Slave 4.
+         * @param enabled New enabled value for Slave 4
+         * @see getSlave4Enabled()
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	void setSlave4Enabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
+         * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
+         * generation of an interrupt signal upon completion of a Slave 4 transaction.
+         * The interrupt status can be observed in Register 54.
+         *
+         * @return Current enabled value for Slave 4 transaction interrupts.
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	bool getSlave4InterruptEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT);
+	}
+	/** Set the enabled value for Slave 4 transaction interrupts.
+         * @param enabled New enabled value for Slave 4 transaction interrupts.
+         * @see getSlave4InterruptEnabled()
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	void setSlave4InterruptEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
+         * 0, the transaction will write a register address prior to reading or writing
+         * data. This should equal 0 when specifying the register address within the
+         * Slave device to/from which the ensuing data transaction will take place.
+         *
+         * @return Current write mode for Slave 4 (0 = register address + data, 1 = data
+         * only)
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	bool getSlave4WriteMode()
+	{
+		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT);
+	}
+	/** Set write mode for the Slave 4.
+         * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data
+         * only)
+         * @see getSlave4WriteMode()
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	void setSlave4WriteMode(bool mode)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
+         * Rate. When a slave's access rate is decreased relative to the Sample Rate,
+         * the slave is accessed every:
+         *
+         *     1 / (1 + I2C_MST_DLY) samples
+         *
+         * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
+         * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
+         * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
+         * further information regarding the Sample Rate, please refer to register 25.
+         *
+         * @return Current Slave 4 master delay value
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	uint8_t getSlave4MasterDelay()
+	{
+		return readBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH);
+	}
+	/** Set Slave 4 master delay value.
+         * @param delay New Slave 4 master delay value
+         * @see getSlave4MasterDelay()
+         * @see MPU6050_RA_I2C_SLV4_CTRL
+         */
+	void setSlave4MasterDelay(uint8_t delay)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
+						  MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
+	}
+	/** Get last available byte read from Slave 4.
+         * This register stores the data read from Slave 4. This field is populated
+         * after a read transaction.
+         * @return Last available byte read from to Slave 4
+         * @see MPU6050_RA_I2C_SLV4_DI
+         */
+	uint8_t getSlate4InputByte()
+	{
+		return readByte(MPU6050_RA_I2C_SLV4_DI);
+	}
 
 	// I2C_MST_STATUS register
-	bool getPassthroughStatus();
-	bool getSlave4IsDone();
-	bool getLostArbitration();
-	bool getSlave4Nack();
-	bool getSlave3Nack();
-	bool getSlave2Nack();
-	bool getSlave1Nack();
-	bool getSlave0Nack();
+
+	/** Get FSYNC interrupt status.
+         * This bit reflects the status of the FSYNC interrupt from an external device
+         * into the MPU-60X0. This is used as a way to pass an external interrupt
+         * through the MPU-60X0 to the host application processor. When set to 1, this
+         * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
+         * (Register 55).
+         * @return FSYNC interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getPassthroughStatus()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
+	}
+	/** Get Slave 4 transaction done status.
+         * Automatically sets to 1 when a Slave 4 transaction has completed. This
+         * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
+         * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
+         * I2C_SLV4_CTRL register (Register 52).
+         * @return Slave 4 transaction done status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave4IsDone()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
+	}
+	/** Get master arbitration lost status.
+         * This bit automatically sets to 1 when the I2C Master has lost arbitration of
+         * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
+         * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Master arbitration lost status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getLostArbitration()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
+	}
+	/** Get Slave 4 NACK status.
+         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+         * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
+         * bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Slave 4 NACK interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave4Nack()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT);
+	}
+	/** Get Slave 3 NACK status.
+         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+         * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
+         * bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Slave 3 NACK interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave3Nack()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
+	}
+	/** Get Slave 2 NACK status.
+         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+         * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
+         * bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Slave 2 NACK interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave2Nack()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
+	}
+	/** Get Slave 1 NACK status.
+         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+         * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
+         * bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Slave 1 NACK interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave1Nack()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
+	}
+	/** Get Slave 0 NACK status.
+         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+         * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
+         * bit in the INT_ENABLE register (Register 56) is asserted.
+         * @return Slave 0 NACK interrupt status
+         * @see MPU6050_RA_I2C_MST_STATUS
+         */
+	bool getSlave0Nack()
+	{
+		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
+	}
 
 	// INT_PIN_CFG register
-	bool getInterruptMode();
-	void setInterruptMode(bool mode);
-	bool getInterruptDrive();
-	void setInterruptDrive(bool drive);
-	bool getInterruptLatch();
-	void setInterruptLatch(bool latch);
-	bool getInterruptLatchClear();
-	void setInterruptLatchClear(bool clear);
-	bool getFSyncInterruptLevel();
-	void setFSyncInterruptLevel(bool level);
-	bool getFSyncInterruptEnabled();
-	void setFSyncInterruptEnabled(bool enabled);
-	bool getI2CBypassEnabled();
-	void setI2CBypassEnabled(bool enabled);
-	bool getClockOutputEnabled();
-	void setClockOutputEnabled(bool enabled);
+
+	/** Get interrupt logic level mode.
+         * Will be set 0 for active-high, 1 for active-low.
+         * @return Current interrupt mode (0=active-high, 1=active-low)
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_LEVEL_BIT
+         */
+	bool getInterruptMode()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT);
+	}
+	/** Set interrupt logic level mode.
+         * @param mode New interrupt mode (0=active-high, 1=active-low)
+         * @see getInterruptMode()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_LEVEL_BIT
+         */
+	void setInterruptMode(bool mode)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
+	}
+	/** Get interrupt drive mode.
+         * Will be set 0 for push-pull, 1 for open-drain.
+         * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_OPEN_BIT
+         */
+	bool getInterruptDrive()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT);
+	}
+	/** Set interrupt drive mode.
+         * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
+         * @see getInterruptDrive()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_OPEN_BIT
+         */
+	void setInterruptDrive(bool drive)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
+	}
+	/** Get interrupt latch mode.
+         * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
+         * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+         */
+	bool getInterruptLatch()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT);
+	}
+	/** Set interrupt latch mode.
+         * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+         * @see getInterruptLatch()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+         */
+	void setInterruptLatch(bool latch)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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.
+         * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+         */
+	bool getInterruptLatchClear()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT);
+	}
+	/** Set interrupt latch clear mode.
+         * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
+         * @see getInterruptLatchClear()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+         */
+	void setInterruptLatchClear(bool clear)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
+	}
+	/** Get FSYNC interrupt logic level mode.
+         * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
+         * @see getFSyncInterruptMode()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+         */
+	bool getFSyncInterruptLevel()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT);
+	}
+	/** Set FSYNC interrupt logic level mode.
+         * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
+         * @see getFSyncInterruptMode()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+         */
+	void setFSyncInterruptLevel(bool level)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
+	}
+	/** Get FSYNC pin interrupt enabled setting.
+         * Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled setting
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+         */
+	bool getFSyncInterruptEnabled()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT);
+	}
+	/** Set FSYNC pin interrupt enabled setting.
+         * @param enabled New FSYNC pin interrupt enabled setting
+         * @see getFSyncInterruptEnabled()
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+         */
+	void setFSyncInterruptEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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
+         * 0, the host application processor will be able to directly access the
+         * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+         * application processor will not be able to directly access the auxiliary I2C
+         * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+         * bit[5]).
+         * @return Current I2C bypass enabled status
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+         */
+	bool getI2CBypassEnabled()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT);
+	}
+	/** Set I2C bypass enabled status.
+         * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
+         * 0, the host application processor will be able to directly access the
+         * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+         * application processor will not be able to directly access the auxiliary I2C
+         * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+         * bit[5]).
+         * @param enabled New I2C bypass enabled status
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+         */
+	void setI2CBypassEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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
+         * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+         * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+         * Specification document.
+         * @return Current reference clock output enabled status
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+         */
+	bool getClockOutputEnabled()
+	{
+		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT);
+	}
+	/** Set reference clock output enabled status.
+         * When this bit is equal to 1, a reference clock output is provided at the
+         * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+         * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+         * Specification document.
+         * @param enabled New reference clock output enabled status
+         * @see MPU6050_RA_INT_PIN_CFG
+         * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+         */
+	void setClockOutputEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
+	}
 
 	// INT_ENABLE register
-	uint8_t getIntEnabled();
-	void setIntEnabled(uint8_t enabled);
-	bool getIntFreefallEnabled();
-	void setIntFreefallEnabled(bool enabled);
-	bool getIntMotionEnabled();
-	void setIntMotionEnabled(bool enabled);
-	bool getIntZeroMotionEnabled();
-	void setIntZeroMotionEnabled(bool enabled);
-	bool getIntFIFOBufferOverflowEnabled();
-	void setIntFIFOBufferOverflowEnabled(bool enabled);
-	bool getIntI2CMasterEnabled();
-	void setIntI2CMasterEnabled(bool enabled);
-	bool getIntDataReadyEnabled();
-	void setIntDataReadyEnabled(bool enabled);
+
+	/** Get full interrupt enabled status.
+         * Full register byte for all interrupts, for quick reading. Each bit will be
+         * set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FF_BIT
+         **/
+	uint8_t getIntEnabled()
+	{
+		return readByte(MPU6050_RA_INT_ENABLE);
+	}
+	/** Set full interrupt enabled status.
+         * Full register byte for all interrupts, for quick reading. Each bit should be
+         * set 0 for disabled, 1 for enabled.
+         * @param enabled New interrupt enabled status
+         * @see getIntFreefallEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FF_BIT
+         **/
+	void setIntEnabled(uint8_t enabled)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
+	}
+	/** Get Free Fall interrupt enabled status.
+         * Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FF_BIT
+         **/
+	bool getIntFreefallEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT);
+	}
+	/** Set Free Fall interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntFreefallEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FF_BIT
+         **/
+	void setIntFreefallEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
+	}
+	/** Get Motion Detection interrupt enabled status.
+         * Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_MOT_BIT
+         **/
+	bool getIntMotionEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT);
+	}
+	/** Set Motion Detection interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntMotionEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_MOT_BIT
+         **/
+	void setIntMotionEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
+	}
+	/** Get Zero Motion Detection interrupt enabled status.
+         * Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_ZMOT_BIT
+         **/
+	bool getIntZeroMotionEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT);
+	}
+	/** Set Zero Motion Detection interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntZeroMotionEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_ZMOT_BIT
+         **/
+	void setIntZeroMotionEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
+	}
+	/** Get FIFO Buffer Overflow interrupt enabled status.
+         * Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+         **/
+	bool getIntFIFOBufferOverflowEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
+	}
+	/** Set FIFO Buffer Overflow interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntFIFOBufferOverflowEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+         **/
+	void setIntFIFOBufferOverflowEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
+	}
+	/** Get I2C Master interrupt enabled status.
+         * This enables any of the I2C Master interrupt sources to generate an
+         * interrupt. Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+         **/
+	bool getIntI2CMasterEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
+	}
+	/** Set I2C Master interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntI2CMasterEnabled()
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+         **/
+	void setIntI2CMasterEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, 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
+         * has been completed. Will be set 0 for disabled, 1 for enabled.
+         * @return Current interrupt enabled status
+         * @see MPU6050_RA_INT_ENABLE
+         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+         */
+	bool getIntDataReadyEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT);
+	}
+	/** Set Data Ready interrupt enabled status.
+         * @param enabled New interrupt enabled status
+         * @see getIntDataReadyEnabled()
+         * @see MPU6050_RA_INT_CFG
+         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+         */
+	void setIntDataReadyEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
+	}
 
 	// INT_STATUS register
-	uint8_t getIntStatus();
-	bool getIntFreefallStatus();
-	bool getIntMotionStatus();
-	bool getIntZeroMotionStatus();
-	bool getIntFIFOBufferOverflowStatus();
-	bool getIntI2CMasterStatus();
-	bool getIntDataReadyStatus();
+
+	/** Get full set of interrupt status bits.
+         * These bits clear to 0 after the register has been read. Very useful
+         * for getting multiple INT statuses, since each single bit read clears
+         * all of them because it has to read the whole byte.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         */
+	uint8_t getIntStatus()
+	{
+		return readByte(MPU6050_RA_INT_STATUS);
+	}
+	/** Get Free Fall interrupt status.
+         * This bit automatically sets to 1 when a Free Fall interrupt has been
+         * generated. The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_FF_BIT
+         */
+	bool getIntFreefallStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT);
+	}
+	/** Get Motion Detection interrupt status.
+         * This bit automatically sets to 1 when a Motion Detection interrupt has been
+         * generated. The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_MOT_BIT
+         */
+	bool getIntMotionStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT);
+	}
+
+	/** Get Zero Motion Detection interrupt status.
+         * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
+         * been generated. The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_ZMOT_BIT
+         */
+	bool getIntZeroMotionStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT);
+	}
+
+	/** Get FIFO Buffer Overflow interrupt status.
+         * This bit automatically sets to 1 when a Free Fall interrupt has been
+         * generated. The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+         */
+	bool getIntFIFOBufferOverflowStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
+	}
+
+	/** Get I2C Master interrupt status.
+         * This bit automatically sets to 1 when an I2C Master interrupt has been
+         * generated. For a list of I2C Master interrupts, please refer to Register 54.
+         * The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+         */
+	bool getIntI2CMasterStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
+	}
+
+	/** Get Data Ready interrupt status.
+         * This bit automatically sets to 1 when a Data Ready interrupt has been
+         * generated. The bit clears to 0 after the register has been read.
+         * @return Current interrupt status
+         * @see MPU6050_RA_INT_STATUS
+         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+         */
+	bool getIntDataReadyStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
+	}
 
 	// ACCEL_*OUT_* registers
+
+	/** Get raw 6-axis motion sensor readings (accel/gyro).
+         * Retrieves all currently available motion sensor values.
+         * @return container for 3-axis accelerometer and 3-axis gyroscope values
+         * @see getAcceleration()
+         * @see getAngularRate()
+         * @see MPU6050_RA_ACCEL_XOUT_H
+         */
 	Motion6 getMotion6();
 
+	/** Get 3-axis accelerometer readings.
+         * These registers store the most recent accelerometer measurements.
+         * Accelerometer measurements are written to these registers at the Sample Rate
+         * as defined in Register 25.
+         *
+         * The accelerometer measurement registers, along with the temperature
+         * measurement registers, gyroscope measurement registers, and external sensor
+         * data registers, are composed of two sets of registers: an internal register
+         * set and a user-facing read register set.
+         *
+         * The data within the accelerometer sensors' internal register set is always
+         * updated at the Sample Rate. Meanwhile, the user-facing read register set
+         * duplicates the internal register set's data values whenever the serial
+         * interface is idle. This guarantees that a burst read of sensor registers will
+         * read measurements from the same sampling instant. Note that if burst reads
+         * are not used, the user is responsible for ensuring a set of single byte reads
+         * correspond to a single sampling instant by checking the Data Ready interrupt.
+         *
+         * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
+         * (Register 28). For each full scale setting, the accelerometers' sensitivity
+         * per LSB in ACCEL_xOUT is shown in the table below:
+         *
+         * <pre>
+         * AFS_SEL | Full Scale Range | LSB Sensitivity
+         * --------+------------------+----------------
+         * 0       | +/- 2g           | 8192 LSB/mg
+         * 1       | +/- 4g           | 4096 LSB/mg
+         * 2       | +/- 8g           | 2048 LSB/mg
+         * 3       | +/- 16g          | 1024 LSB/mg
+         * </pre>
+         *
+         * @param x 16-bit signed integer container for X-axis acceleration
+         * @param y 16-bit signed integer container for Y-axis acceleration
+         * @param z 16-bit signed integer container for Z-axis acceleration
+         * @see MPU6050_RA_GYRO_XOUT_H
+         */
 	Motion3 getAcceleration();
-	int16_t getAccelerationX();
-	int16_t getAccelerationY();
-	int16_t getAccelerationZ();
+
+	/** Get X-axis accelerometer reading.
+         * @return X-axis acceleration measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_ACCEL_XOUT_H
+         */
+	int16_t getAccelerationX()
+	{
+		return readReg<int16_t>(MPU6050_RA_ACCEL_XOUT_H);
+	}
+	/** Get Y-axis accelerometer reading.
+         * @return Y-axis acceleration measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_ACCEL_YOUT_H
+         */
+	int16_t getAccelerationY()
+	{
+		return readReg<int16_t>(MPU6050_RA_ACCEL_YOUT_H);
+	}
+	/** Get Z-axis accelerometer reading.
+         * @return Z-axis acceleration measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_ACCEL_ZOUT_H
+         */
+	int16_t getAccelerationZ()
+	{
+		return readReg<int16_t>(MPU6050_RA_ACCEL_ZOUT_H);
+	}
 
 	// TEMP_OUT_* registers
-	int16_t getTemperature();
+
+	/** Get current internal temperature.
+         * @return Temperature reading in 16-bit 2's complement format
+         * @see MPU6050_RA_TEMP_OUT_H
+         */
+	int16_t getTemperature()
+	{
+		return readReg<int16_t>(MPU6050_RA_TEMP_OUT_H);
+	}
 
 	// GYRO_*OUT_* registers
+
+	/** Get 3-axis gyroscope readings.
+         * These gyroscope measurement registers, along with the accelerometer
+         * measurement registers, temperature measurement registers, and external sensor
+         * data registers, are composed of two sets of registers: an internal register
+         * set and a user-facing read register set.
+         * The data within the gyroscope sensors' internal register set is always
+         * updated at the Sample Rate. Meanwhile, the user-facing read register set
+         * duplicates the internal register set's data values whenever the serial
+         * interface is idle. This guarantees that a burst read of sensor registers will
+         * read measurements from the same sampling instant. Note that if burst reads
+         * are not used, the user is responsible for ensuring a set of single byte reads
+         * correspond to a single sampling instant by checking the Data Ready interrupt.
+         *
+         * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
+         * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
+         * LSB in GYRO_xOUT is shown in the table below:
+         *
+         * <pre>
+         * FS_SEL | Full Scale Range   | LSB Sensitivity
+         * -------+--------------------+----------------
+         * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
+         * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
+         * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
+         * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
+         * </pre>
+         *
+         * @return container for 3-axis gyro values
+         * @see getMotion6()
+         * @see MPU6050_RA_GYRO_XOUT_H
+         */
 	Motion3 getAngularRate();
-	int16_t getAngularRateX();
-	int16_t getAngularRateY();
-	int16_t getAngularRateZ();
-	int16_t getAngularRateZ2();
+
+	/** Get X-axis gyroscope reading.
+         * @return X-axis rotation measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_GYRO_XOUT_H
+         */
+	int16_t getAngularRateX()
+	{
+		return readReg<int16_t>(MPU6050_RA_GYRO_XOUT_H);
+	}
+	/** Get Y-axis gyroscope reading.
+         * @return Y-axis rotation measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_GYRO_YOUT_H
+         */
+	int16_t getAngularRateY()
+	{
+		return readReg<int16_t>(MPU6050_RA_GYRO_YOUT_H);
+	}
+	/** Get Z-axis gyroscope reading.
+         * @return Z-axis rotation measurement in 16-bit 2's complement format
+         * @see getMotion6()
+         * @see MPU6050_RA_GYRO_ZOUT_H
+         */
+	int16_t getAngularRateZ()
+	{
+		return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
+	}
+
+	int16_t getAngularRateZ2()
+	{
+		return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
+	}
 
 	// EXT_SENS_DATA_* registers
-	uint8_t getExternalSensorByte(int position);
-	uint16_t getExternalSensorWord(int position);
-	uint32_t getExternalSensorDWord(int position);
+
+	/** Read single byte from external sensor data register.
+         * These registers store data read from external sensors by the Slave 0, 1, 2,
+         * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
+         * I2C_SLV4_DI (Register 53).
+         *
+         * External sensor data is written to these registers at the Sample Rate as
+         * defined in Register 25. This access rate can be reduced by using the Slave
+         * Delay Enable registers (Register 103).
+         *
+         * External sensor data registers, along with the gyroscope measurement
+         * registers, accelerometer measurement registers, and temperature measurement
+         * registers, are composed of two sets of registers: an internal register set
+         * and a user-facing read register set.
+         *
+         * The data within the external sensors' internal register set is always updated
+         * at the Sample Rate (or the reduced access rate) whenever the serial interface
+         * is idle. This guarantees that a burst read of sensor registers will read
+         * measurements from the same sampling instant. Note that if burst reads are not
+         * used, the user is responsible for ensuring a set of single byte reads
+         * correspond to a single sampling instant by checking the Data Ready interrupt.
+         *
+         * Data is placed in these external sensor data registers according to
+         * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
+         * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
+         * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
+         * defined in Register 25) or delayed rate (if specified in Register 52 and
+         * 103). During each Sample cycle, slave reads are performed in order of Slave
+         * number. If all slaves are enabled with more than zero bytes to be read, the
+         * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
+         *
+         * Each enabled slave will have EXT_SENS_DATA registers associated with it by
+         * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
+         * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
+         * change the higher numbered slaves' associated registers. Furthermore, if
+         * fewer total bytes are being read from the external sensors as a result of
+         * such a change, then the data remaining in the registers which no longer have
+         * an associated slave device (i.e. high numbered registers) will remain in
+         * these previously allocated registers unless reset.
+         *
+         * If the sum of the read lengths of all SLVx transactions exceed the number of
+         * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
+         * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
+         * the slaves cannot be greater than 24 or some bytes will be lost.
+         *
+         * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
+         * information regarding the characteristics of Slave 4, please refer to
+         * Registers 49 to 53.
+         *
+         * EXAMPLE:
+         * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
+         * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
+         * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
+         * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
+         * will be associated with Slave 1. If Slave 2 is enabled as well, registers
+         * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
+         *
+         * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
+         * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
+         * instead.
+         *
+         * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
+         * If a slave is disabled at any time, the space initially allocated to the
+         * slave in the EXT_SENS_DATA register, will remain associated with that slave.
+         * This is to avoid dynamic adjustment of the register allocation.
+         *
+         * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
+         * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
+         *
+         * This above is also true if one of the slaves gets NACKed and stops
+         * functioning.
+         *
+        * @param position Starting position (0-23)
+            * @return Byte read from register
+            */
+	uint8_t getExternalSensorByte(int position)
+	{
+		return readByte(MPU6050_RA_EXT_SENS_DATA_00 + position);
+	}
+	/** Read word (2 bytes) from external sensor data registers.
+         * @param position Starting position (0-21)
+         * @return Word read from register
+         * @see getExternalSensorByte()
+         */
+	uint16_t getExternalSensorWord(int position)
+	{
+		return readReg<uint16_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
+	}
+	/** Read double word (4 bytes) from external sensor data registers.
+         * @param position Starting position (0-20)
+         * @return Double word read from registers
+         * @see getExternalSensorByte()
+         */
+	uint32_t getExternalSensorDWord(int position)
+	{
+		return readReg<uint32_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
+	}
 
 	// MOT_DETECT_STATUS register
-	uint8_t getMotionStatus();
-	bool getXNegMotionDetected();
-	bool getXPosMotionDetected();
-	bool getYNegMotionDetected();
-	bool getYPosMotionDetected();
-	bool getZNegMotionDetected();
-	bool getZPosMotionDetected();
-	bool getZeroMotionDetected();
+
+	/** Get full motion detection status register content (all bits).
+         * @return Motion detection status byte
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         */
+	uint8_t getMotionStatus()
+	{
+		return readByte(MPU6050_RA_MOT_DETECT_STATUS);
+	}
+	/** Get X-axis negative motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_XNEG_BIT
+         */
+	bool getXNegMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT);
+	}
+	/** Get X-axis positive motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_XPOS_BIT
+         */
+	bool getXPosMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT);
+	}
+	/** Get Y-axis negative motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_YNEG_BIT
+         */
+	bool getYNegMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT);
+	}
+	/** Get Y-axis positive motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_YPOS_BIT
+         */
+	bool getYPosMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT);
+	}
+	/** Get Z-axis negative motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_ZNEG_BIT
+         */
+	bool getZNegMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT);
+	}
+	/** Get Z-axis positive motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_ZPOS_BIT
+         */
+	bool getZPosMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT);
+	}
+	/** Get zero motion detection interrupt status.
+         * @return Motion detection status
+         * @see MPU6050_RA_MOT_DETECT_STATUS
+         * @see MPU6050_MOTION_MOT_ZRMOT_BIT
+         */
+	bool getZeroMotionDetected()
+	{
+		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT);
+	}
 
 	// I2C_SLV*_DO register
+
+	/** Write byte to Data Output container for specified slave.
+         * This register holds the output data written into Slave when Slave is set to
+         * write mode. For further information regarding Slave control, please
+         * refer to Registers 37 to 39 and immediately following.
+         * @param num Slave number (0-3)
+         * @param data Byte to write
+         * @see MPU6050_RA_I2C_SLV0_DO
+         */
 	void setSlaveOutputByte(uint8_t num, uint8_t data);
 
 	// I2C_MST_DELAY_CTRL register
-	bool getExternalShadowDelayEnabled();
-	void setExternalShadowDelayEnabled(bool enabled);
+	/** Get external data shadow delay enabled status.
+         * This register is used to specify the timing of external sensor data
+         * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
+         * sensor data is delayed until all data has been received.
+         * @return Current external data shadow delay enabled status.
+         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+         * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+         */
+	bool getExternalShadowDelayEnabled()
+	{
+		return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT);
+	}
+	/** Set external data shadow delay enabled status.
+         * @param enabled New external data shadow delay enabled status.
+         * @see getExternalShadowDelayEnabled()
+         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+         * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+         */
+	void setExternalShadowDelayEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, 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
+         * slave device is reduced. When a slave's access rate is decreased relative to
+         * the Sample Rate, the slave is accessed every:
+         *
+         *     1 / (1 + I2C_MST_DLY) Samples
+         *
+         * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
+         * and DLPF_CFG (register 26).
+         *
+         * For further information regarding I2C_MST_DLY, please refer to register 52.
+         * For further information regarding the Sample Rate, please refer to
+         * register 25.
+         *
+         * @param num Slave number (0-4)
+         * @return Current slave delay enabled status.
+         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+         * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+         */
 	bool getSlaveDelayEnabled(uint8_t num);
-	void setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+	/** Set slave delay enabled status.
+         * @param num Slave number (0-4)
+         * @param enabled New slave delay enabled status.
+         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+         * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+         */
+	void setSlaveDelayEnabled(uint8_t num, bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
+	}
 
 	// SIGNAL_PATH_RESET register
-	void resetGyroscopePath();
-	void resetAccelerometerPath();
-	void resetTemperaturePath();
+
+	/** Reset gyroscope signal path.
+         * The reset will revert the signal path analog to digital converters and
+         * filters to their power up configurations.
+         * @see MPU6050_RA_SIGNAL_PATH_RESET
+         * @see MPU6050_PATHRESET_GYRO_RESET_BIT
+         */
+	void resetGyroscopePath()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
+	}
+	/** Reset accelerometer signal path.
+         * The reset will revert the signal path analog to digital converters and
+         * filters to their power up configurations.
+         * @see MPU6050_RA_SIGNAL_PATH_RESET
+         * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
+         */
+	void resetAccelerometerPath()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
+	}
+	/** Reset temperature sensor signal path.
+         * The reset will revert the signal path analog to digital converters and
+         * filters to their power up configurations.
+         * @see MPU6050_RA_SIGNAL_PATH_RESET
+         * @see MPU6050_PATHRESET_TEMP_RESET_BIT
+         */
+	void resetTemperaturePath()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
+	}
 
 	// MOT_DETECT_CTRL register
-	uint8_t getAccelerometerPowerOnDelay();
-	void setAccelerometerPowerOnDelay(uint8_t delay);
-	uint8_t getFreefallDetectionCounterDecrement();
-	void setFreefallDetectionCounterDecrement(uint8_t decrement);
-	uint8_t getMotionDetectionCounterDecrement();
-	void setMotionDetectionCounterDecrement(uint8_t decrement);
+
+	/** Get accelerometer power-on delay.
+         * The accelerometer data path provides samples to the sensor registers, Motion
+         * detection, Zero Motion detection, and Free Fall detection modules. The
+         * signal path contains filters which must be flushed on wake-up with new
+         * samples before the detection modules begin operations. The default wake-up
+         * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
+         * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
+         * any value above zero unless instructed otherwise by InvenSense. Please refer
+         * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
+         * further information regarding the detection modules.
+         * @return Current accelerometer power-on delay
+         * @see MPU6050_RA_MOT_DETECT_CTRL
+         * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+         */
+	uint8_t getAccelerometerPowerOnDelay()
+	{
+		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+						MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH);
+	}
+	/** Set accelerometer power-on delay.
+         * @param delay New accelerometer power-on delay (0-3)
+         * @see getAccelerometerPowerOnDelay()
+         * @see MPU6050_RA_MOT_DETECT_CTRL
+         * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+         */
+	void setAccelerometerPowerOnDelay(uint8_t delay)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+						  MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+	}
+	/** Get Free Fall detection counter decrement configuration.
+         * Detection is registered by the Free Fall detection module after accelerometer
+         * measurements meet their respective threshold conditions over a specified
+         * number of samples. When the threshold conditions are met, the corresponding
+         * detection counter increments by 1. The user may control the rate at which the
+         * detection counter decrements when the threshold condition is not met by
+         * configuring FF_COUNT. The decrement rate can be set according to the
+         * following table:
+         *
+         * <pre>
+         * FF_COUNT | Counter Decrement
+         * ---------+------------------
+         * 0        | Reset
+         * 1        | 1
+         * 2        | 2
+         * 3        | 4
+         * </pre>
+         *
+         * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
+         * reset the counter to 0. For further information on Free Fall detection,
+         * please refer to Registers 29 to 32.
+         *
+         * @return Current decrement configuration
+         * @see MPU6050_RA_MOT_DETECT_CTRL
+         * @see MPU6050_DETECT_FF_COUNT_BIT
+         */
+	uint8_t getFreefallDetectionCounterDecrement()
+	{
+		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH);
+	}
+	/** Set Free Fall detection counter decrement configuration.
+         * @param decrement New decrement configuration value
+         * @see getFreefallDetectionCounterDecrement()
+         * @see MPU6050_RA_MOT_DETECT_CTRL
+         * @see MPU6050_DETECT_FF_COUNT_BIT
+         */
+	void setFreefallDetectionCounterDecrement(uint8_t decrement)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
+						  MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
+	}
+	/** Get Motion detection counter decrement configuration.
+         * Detection is registered by the Motion detection module after accelerometer
+         * measurements meet their respective threshold conditions over a specified
+         * number of samples. When the threshold conditions are met, the corresponding
+         * detection counter increments by 1. The user may control the rate at which the
+         * detection counter decrements when the threshold condition is not met by
+         * configuring MOT_COUNT. The decrement rate can be set according to the
+         * following table:
+         *
+         * <pre>
+         * MOT_COUNT | Counter Decrement
+         * ----------+------------------
+         * 0         | Reset
+         * 1         | 1
+         * 2         | 2
+         * 3         | 4
+         * </pre>
+         *
+         * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
+         * reset the counter to 0. For further information on Motion detection,
+         * please refer to Registers 29 to 32.
+         *
+         */
+	uint8_t getMotionDetectionCounterDecrement()
+	{
+		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH);
+	}
+	/** Set Motion detection counter decrement configuration.
+         * @param decrement New decrement configuration value
+         * @see getMotionDetectionCounterDecrement()
+         * @see MPU6050_RA_MOT_DETECT_CTRL
+         * @see MPU6050_DETECT_MOT_COUNT_BIT
+         */
+	void setMotionDetectionCounterDecrement(uint8_t decrement)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
+						  MPU6050_DETECT_MOT_COUNT_LENGTH, decrement);
+	}
 
 	// USER_CTRL register
-	bool getFIFOEnabled();
-	void setFIFOEnabled(bool enabled);
-	bool getI2CMasterModeEnabled();
-	void setI2CMasterModeEnabled(bool enabled);
-	void switchSPIEnabled(bool enabled);
-	void resetFIFO();
-	void resetI2CMaster();
-	void resetSensors();
+
+	/** Get FIFO enabled status.
+         * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
+         * cannot be written to or read from while disabled. The FIFO buffer's state
+         * does not change unless the MPU-60X0 is power cycled.
+         * @return Current FIFO enabled status
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_FIFO_EN_BIT
+         */
+	bool getFIFOEnabled()
+	{
+		return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT);
+	}
+	/** Set FIFO enabled status.
+         * @param enabled New FIFO enabled status
+         * @see getFIFOEnabled()
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_FIFO_EN_BIT
+         */
+	void setFIFOEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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
+         * external sensor slave devices on the auxiliary I2C bus. When this bit is
+         * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
+         * driven by the primary I2C bus (SDA and SCL). This is a precondition to
+         * enabling Bypass Mode. For further information regarding Bypass Mode, please
+         * refer to Register 55.
+         * @return Current I2C Master Mode enabled status
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+         */
+	bool getI2CMasterModeEnabled()
+	{
+		return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT);
+	}
+	/** Set I2C Master Mode enabled status.
+         * @param enabled New I2C Master Mode enabled status
+         * @see getI2CMasterModeEnabled()
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+         */
+	void setI2CMasterModeEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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 switchSPIEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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
+         * bit automatically clears to 0 after the reset has been triggered.
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_FIFO_RESET_BIT
+         */
+	void resetFIFO()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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.
+         * This bit automatically clears to 0 after the reset has been triggered.
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
+         */
+	void resetI2CMaster()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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,
+         * accelerometers, and temperature sensor). This operation will also clear the
+         * sensor registers. This bit automatically clears to 0 after the reset has been
+         * triggered.
+         *
+         * When resetting only the signal path (and not the sensor registers), please
+         * use Register 104, SIGNAL_PATH_RESET.
+         *
+         * @see MPU6050_RA_USER_CTRL
+         * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
+         */
+	void resetSensors()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
+	}
 
 	// PWR_MGMT_1 register
-	void reset();
-	bool getSleepEnabled();
-	void setSleepEnabled(bool enabled);
-	bool getWakeCycleEnabled();
-	void setWakeCycleEnabled(bool enabled);
-	bool getTempSensorEnabled();
-	void setTempSensorEnabled(bool enabled);
-	uint8_t getClockSource();
-	void setClockSource(uint8_t source);
+
+	/** Trigger a full device reset.
+         * A small delay of ~50ms may be desirable after triggering a reset.
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_DEVICE_RESET_BIT
+         */
+	void reset()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
+	}
+	/** Get sleep mode status.
+         * Setting the SLEEP bit in the register puts the device into very low power
+         * sleep mode. In this mode, only the serial interface and internal registers
+         * remain active, allowing for a very low standby current. Clearing this bit
+         * puts the device back into normal mode. To save power, the individual standby
+         * selections for each of the gyros should be used if any gyro axis is not used
+         * by the application.
+         * @return Current sleep mode enabled status
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_SLEEP_BIT
+         */
+	bool getSleepEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT);
+	}
+	/** Set sleep mode status.
+         * @param enabled New sleep mode enabled status
+         * @see getSleepEnabled()
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_SLEEP_BIT
+         */
+	void setSleepEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, 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
+         * between sleep mode and waking up to take a single sample of data from active
+         * sensors at a rate determined by LP_WAKE_CTRL (register 108).
+         * @return Current sleep mode enabled status
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_CYCLE_BIT
+         */
+	bool getWakeCycleEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT);
+	}
+	/** Set wake cycle enabled status.
+         * @param enabled New sleep mode enabled status
+         * @see getWakeCycleEnabled()
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_CYCLE_BIT
+         */
+	void setWakeCycleEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
+	}
+	/** Get temperature sensor enabled status.
+         * Control the usage of the internal temperature sensor.
+         *
+         * Note: this register stores the *disabled* value, but for consistency with the
+         * rest of the code, the function is named and used with standard true/false
+         * values to indicate whether the sensor is enabled or disabled, respectively.
+         *
+         * @return Current temperature sensor enabled status
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_TEMP_DIS_BIT
+         */
+	bool getTempSensorEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT); // 1 is actually disabled here
+	}
+	/** Set temperature sensor enabled status.
+         * Note: this register stores the *disabled* value, but for consistency with the
+         * rest of the code, the function is named and used with standard true/false
+         * values to indicate whether the sensor is enabled or disabled, respectively.
+         *
+         * @param enabled New temperature sensor enabled status
+         * @see getTempSensorEnabled()
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_TEMP_DIS_BIT
+         */
+	void setTempSensorEnabled(bool enabled)
+	{
+		// 1 is actually disabled here
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
+	}
+	/** Get clock source setting.
+         * @return Current clock source setting
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_CLKSEL_BIT
+         * @see MPU6050_PWR1_CLKSEL_LENGTH
+         */
+	uint8_t getClockSource()
+	{
+		return readBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH);
+	}
+	/** Set clock source setting.
+         * An internal 8MHz oscillator, gyroscope based clock, or external sources can
+         * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
+         * or an external source is chosen as the clock source, the MPU-60X0 can operate
+         * in low power modes with the gyroscopes disabled.
+         *
+         * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
+         * However, it is highly recommended that the device be configured to use one of
+         * the gyroscopes (or an external clock source) as the clock reference for
+         * improved stability. The clock source can be selected according to the
+         * following table:
+         *
+         * <pre>
+         * CLK_SEL | Clock Source
+         * --------+--------------------------------------
+         * 0       | Internal oscillator
+         * 1       | PLL with X Gyro reference
+         * 2       | PLL with Y Gyro reference
+         * 3       | PLL with Z Gyro reference
+         * 4       | PLL with external 32.768kHz reference
+         * 5       | PLL with external 19.2MHz reference
+         * 6       | Reserved
+         * 7       | Stops the clock and keeps the timing generator in reset
+         * </pre>
+         *
+         * @param source New clock source setting
+         * @see getClockSource()
+         * @see MPU6050_RA_PWR_MGMT_1
+         * @see MPU6050_PWR1_CLKSEL_BIT
+         * @see MPU6050_PWR1_CLKSEL_LENGTH
+         */
+	void setClockSource(uint8_t source)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
+	}
 
 	// PWR_MGMT_2 register
-	uint8_t getWakeFrequency();
-	void setWakeFrequency(uint8_t frequency);
-	bool getStandbyXAccelEnabled();
-	void setStandbyXAccelEnabled(bool enabled);
-	bool getStandbyYAccelEnabled();
-	void setStandbyYAccelEnabled(bool enabled);
-	bool getStandbyZAccelEnabled();
-	void setStandbyZAccelEnabled(bool enabled);
-	bool getStandbyXGyroEnabled();
-	void setStandbyXGyroEnabled(bool enabled);
-	bool getStandbyYGyroEnabled();
-	void setStandbyYGyroEnabled(bool enabled);
-	bool getStandbyZGyroEnabled();
-	void setStandbyZGyroEnabled(bool enabled);
-
-	// FIFO_COUNT_* registers
-	uint16_t getFIFOCount();
+
+	/** Get wake frequency in Accel-Only Low Power Mode.
+         * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
+         * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
+         * the device will power off all devices except for the primary I2C interface,
+         * waking only the accelerometer at fixed intervals to take a single
+         * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
+         * as shown below:
+         *
+         * <pre>
+         * LP_WAKE_CTRL | Wake-up Frequency
+         * -------------+------------------
+         * 0            | 1.25 Hz
+         * 1            | 2.5 Hz
+         * 2            | 5 Hz
+         * 3            | 10 Hz
+         * </pre>
+         *
+         * For further information regarding the MPU-60X0's power modes, please refer to
+         * Register 107.
+         *
+         * @return Current wake frequency
+         * @see MPU6050_RA_PWR_MGMT_2
+         */
+	uint8_t getWakeFrequency()
+	{
+		return readBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH);
+	}
+	/** Set wake frequency in Accel-Only Low Power Mode.
+         * @param frequency New wake frequency
+         * @see MPU6050_RA_PWR_MGMT_2
+         */
+	void setWakeFrequency(uint8_t frequency)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
+						  MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
+	}
+
+	/** Get X-axis accelerometer standby enabled status.
+         * If enabled, the X-axis will not gather or report data (or use power).
+         * @return Current X-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_XA_BIT
+         */
+	bool getStandbyXAccelEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT);
+	}
+	/** Set X-axis accelerometer standby enabled status.
+         * @param New X-axis standby enabled status
+         * @see getStandbyXAccelEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_XA_BIT
+         */
+	void setStandbyXAccelEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
+         * @return Current Y-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_YA_BIT
+         */
+	bool getStandbyYAccelEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT);
+	}
+	/** Set Y-axis accelerometer standby enabled status.
+         * @param New Y-axis standby enabled status
+         * @see getStandbyYAccelEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_YA_BIT
+         */
+	void setStandbyYAccelEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
+         * @return Current Z-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_ZA_BIT
+         */
+	bool getStandbyZAccelEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT);
+	}
+	/** Set Z-axis accelerometer standby enabled status.
+         * @param New Z-axis standby enabled status
+         * @see getStandbyZAccelEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_ZA_BIT
+         */
+	void setStandbyZAccelEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
+         * @return Current X-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_XG_BIT
+         */
+	bool getStandbyXGyroEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT);
+	}
+	/** Set X-axis gyroscope standby enabled status.
+         * @param New X-axis standby enabled status
+         * @see getStandbyXGyroEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_XG_BIT
+         */
+	void setStandbyXGyroEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
+         * @return Current Y-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_YG_BIT
+         */
+	bool getStandbyYGyroEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT);
+	}
+	/** Set Y-axis gyroscope standby enabled status.
+         * @param New Y-axis standby enabled status
+         * @see getStandbyYGyroEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_YG_BIT
+         */
+	void setStandbyYGyroEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
+         * @return Current Z-axis standby enabled status
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_ZG_BIT
+         */
+	bool getStandbyZGyroEnabled()
+	{
+		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT);
+	}
+	/** Set Z-axis gyroscope standby enabled status.
+         * @param New Z-axis standby enabled status
+         * @see getStandbyZGyroEnabled()
+         * @see MPU6050_RA_PWR_MGMT_2
+         * @see MPU6050_PWR2_STBY_ZG_BIT
+         */
+	void setStandbyZGyroEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+	}
+
+	// FIFO_COUNT* registers
+
+	/** Get current FIFO buffer size.
+         * This value indicates the number of bytes stored in the FIFO buffer. This
+         * number is in turn the number of bytes that can be read from the FIFO buffer
+         * and it is directly proportional to the number of samples available given the
+         * set of sensor data bound to be stored in the FIFO (register 35 and 36).
+         * @return Current FIFO buffer size
+         */
+	uint16_t getFIFOCount()
+	{
+		uint8_t buffer[2] = {0};
+		I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer);
+		return (((uint16_t)buffer[0]) << 8) | buffer[1];
+	}
 
 	// FIFO_R_W register
-	uint8_t getFIFOByte();
-	void setFIFOByte(uint8_t data);
+
+	/** Get byte from FIFO buffer.
+         * This register is used to read and write data from the FIFO buffer. Data is
+         * written to the FIFO in order of register number (from lowest to highest). If
+         * all the FIFO enable flags (see below) are enabled and all External Sensor
+         * Data registers (Registers 73 to 96) are associated with a Slave device, the
+         * contents of registers 59 through 96 will be written in order at the Sample
+         * Rate.
+         *
+         * The contents of the sensor data registers (Registers 59 to 96) are written
+         * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
+         * in FIFO_EN (Register 35). An additional flag for the sensor data registers
+         * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
+         *
+         * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
+         * automatically set to 1. This bit is located in INT_STATUS (Register 58).
+         * When the FIFO buffer has overflowed, the oldest data will be lost and new
+         * data will be written to the FIFO.
+         *
+         * If the FIFO buffer is empty, reading this register will return the last byte
+         * that was previously read from the FIFO until new data is available. The user
+         * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
+         * empty.
+         *
+         * @return Byte from FIFO buffer
+         */
+	uint8_t getFIFOByte()
+	{
+		return readByte(MPU6050_RA_FIFO_R_W);
+	}
+
+	/** Write byte to FIFO buffer.
+         * @see getFIFOByte()
+         * @see MPU6050_RA_FIFO_R_W
+         */
+	void setFIFOByte(uint8_t data)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
+	}
+
 	void getFIFOBytes(uint8_t* data, uint8_t length);
 
 	// WHO_AM_I register
-	uint8_t getDeviceID();
-	void setDeviceID(uint8_t id);
+
+	/** Get Device ID.
+         * This register is used to verify the identity of the device (0b110100, 0x34).
+         * @return Device ID (6 bits only! should be 0x34)
+         * @see MPU6050_RA_WHO_AM_I
+         * @see MPU6050_WHO_AM_I_BIT
+         * @see MPU6050_WHO_AM_I_LENGTH
+         */
+	uint8_t getDeviceID()
+	{
+		return readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH);
+	}
+	/** Set Device ID.
+         * Write a new ID into the WHO_AM_I register (no idea why this should ever be
+         * necessary though).
+         * @param id New device ID to set.
+         * @see getDeviceID()
+         * @see MPU6050_RA_WHO_AM_I
+         * @see MPU6050_WHO_AM_I_BIT
+         * @see MPU6050_WHO_AM_I_LENGTH
+         */
+	void setDeviceID(uint8_t id)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
+	}
 
 	// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
 
 	// XG_OFFS_TC register
-	uint8_t getOTPBankValid();
-	void setOTPBankValid(bool enabled);
-	int8_t getXGyroOffsetTC();
-	void setXGyroOffsetTC(int8_t offset);
+
+	uint8_t getOTPBankValid()
+	{
+		return readBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT);
+	}
+	void setOTPBankValid(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+	}
+	int8_t getXGyroOffsetTC()
+	{
+		return readBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
+	}
+	void setXGyroOffsetTC(int8_t offset)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	}
 
 	// YG_OFFS_TC register
-	int8_t getYGyroOffsetTC();
-	void setYGyroOffsetTC(int8_t offset);
+
+	int8_t getYGyroOffsetTC()
+	{
+		return readBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
+	}
+	void setYGyroOffsetTC(int8_t offset)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	}
 
 	// ZG_OFFS_TC register
-	int8_t getZGyroOffsetTC();
-	void setZGyroOffsetTC(int8_t offset);
+
+	int8_t getZGyroOffsetTC()
+	{
+		return readBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH);
+	}
+	void setZGyroOffsetTC(int8_t offset)
+	{
+		I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+	}
 
 	// X_FINE_GAIN register
-	int8_t getXFineGain();
-	void setXFineGain(int8_t gain);
+
+	int8_t getXFineGain()
+	{
+		return readByte(MPU6050_RA_X_FINE_GAIN);
+	}
+	void setXFineGain(int8_t gain)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+	}
 
 	// Y_FINE_GAIN register
-	int8_t getYFineGain();
-	void setYFineGain(int8_t gain);
+
+	int8_t getYFineGain()
+	{
+		return readByte(MPU6050_RA_Y_FINE_GAIN);
+	}
+	void setYFineGain(int8_t gain)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+	}
 
 	// Z_FINE_GAIN register
-	int8_t getZFineGain();
-	void setZFineGain(int8_t gain);
+
+	int8_t getZFineGain()
+	{
+		return readByte(MPU6050_RA_Z_FINE_GAIN);
+	}
+	void setZFineGain(int8_t gain)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+	}
 
 	// XA_OFFS_* registers
 	int16_t getXAccelOffset();
-	void setXAccelOffset(int16_t offset);
+	void setXAccelOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset);
+	}
 
 	// YA_OFFS_* register
 	int16_t getYAccelOffset();
-	void setYAccelOffset(int16_t offset);
+	void setYAccelOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset);
+	}
 
 	// ZA_OFFS_* register
 	int16_t getZAccelOffset();
-	void setZAccelOffset(int16_t offset);
+	void setZAccelOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+	}
 
 	// XG_OFFS_USR* registers
-	int16_t getXGyroOffset();
-	void setXGyroOffset(int16_t offset);
-
+	int16_t getXGyroOffset()
+	{
+		return readReg<int16_t>(MPU6050_RA_XG_OFFS_USRH);
+	}
+	void setXGyroOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+	}
 	// YG_OFFS_USR* register
-	int16_t getYGyroOffset();
-	void setYGyroOffset(int16_t offset);
+	int16_t getYGyroOffset()
+	{
+		return readReg<int16_t>(MPU6050_RA_YG_OFFS_USRH);
+	}
+
+	void setYGyroOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+	}
 
 	// ZG_OFFS_USR* register
-	int16_t getZGyroOffset();
-	void setZGyroOffset(int16_t offset);
+	int16_t getZGyroOffset()
+	{
+		return readReg<int16_t>(MPU6050_RA_ZG_OFFS_USRH);
+	}
+
+	void setZGyroOffset(int16_t offset)
+	{
+		I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+	}
 
 	// INT_ENABLE register (DMP functions)
-	bool getIntPLLReadyEnabled();
-	void setIntPLLReadyEnabled(bool enabled);
-	bool getIntDMPEnabled();
-	void setIntDMPEnabled(bool enabled);
+	bool getIntPLLReadyEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
+	}
+	void setIntPLLReadyEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+	}
+	bool getIntDMPEnabled()
+	{
+		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT);
+	}
+	void setIntDMPEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+	}
 
 	// DMP_INT_STATUS
-	bool getDMPInt5Status();
-	bool getDMPInt4Status();
-	bool getDMPInt3Status();
-	bool getDMPInt2Status();
-	bool getDMPInt1Status();
-	bool getDMPInt0Status();
+	bool getDMPInt5Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT);
+	}
+	bool getDMPInt4Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT);
+	}
+	bool getDMPInt3Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT);
+	}
+	bool getDMPInt2Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT);
+	}
+	bool getDMPInt1Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT);
+	}
+	bool getDMPInt0Status()
+	{
+		return readBit(MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT);
+	}
 
 	// INT_STATUS register (DMP functions)
-	bool getIntPLLReadyStatus();
-	bool getIntDMPStatus();
+
+	bool getIntPLLReadyStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT);
+	}
+	bool getIntDMPStatus()
+	{
+		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT);
+	}
 
 	// USER_CTRL register (DMP functions)
-	bool getDMPEnabled();
-	void setDMPEnabled(bool enabled);
-	void resetDMP();
 
+	bool getDMPEnabled()
+	{
+		return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT);
+	}
+	void setDMPEnabled(bool enabled)
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+	}
+	void resetDMP()
+	{
+		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
+	}
 	// BANK_SEL register
 	void setMemoryBank(uint8_t bank, bool prefetchEnabled = false, bool userBank = false);
 
@@ -833,17 +3229,36 @@ class MPU6050
 							  bool verify = true);
 
 	bool writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, bool useProgMem = false);
-	bool writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize);
+
+	bool writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize)
+	{
+		return writeDMPConfigurationSet(data, dataSize, true);
+	}
 
 	// DMP_CFG_1 register
-	uint8_t getDMPConfig1();
-	void setDMPConfig1(uint8_t config);
+
+	uint8_t getDMPConfig1()
+	{
+		return readByte(MPU6050_RA_DMP_CFG_1);
+	}
+
+	void setDMPConfig1(uint8_t config)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
+	}
 
 	// DMP_CFG_2 register
-	uint8_t getDMPConfig2();
-	void setDMPConfig2(uint8_t config);
+	uint8_t getDMPConfig2()
+	{
+		return readByte(MPU6050_RA_DMP_CFG_2);
+	}
+
+	void setDMPConfig2(uint8_t config)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
+	}
 
-	void CalibrateGyro(uint8_t Loops = 15);  // Fine tune after setting offsets with less Loops.
+	void CalibrateGyro(uint8_t Loops = 15);	 // Fine tune after setting offsets with less Loops.
 	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
 	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
 

From c5a790962c8869fac1abb0a531e108a11d54f2ad Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Sat, 4 Nov 2023 10:45:00 +0100
Subject: [PATCH 16/21] Delete Calibration and Memory Block related code

For simplicity and code quality, they are deleted at the moment
---
 Sming/Libraries/MPU6050/MPU6050.cpp |  488 +------
 Sming/Libraries/MPU6050/MPU6050.h   | 2099 ++++++++++++++-------------
 2 files changed, 1136 insertions(+), 1451 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index dff39db572..2ae84f87b9 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -109,49 +109,6 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
 	return (z & 0x1F);
 }
 
-// I2C_SLV* registers (Slave 0-3)
-
-/** Get the I2C address of the specified slave (0-3).
- * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
- * operation, and if it is cleared, then it's a write operation. The remaining
- * bits (6-0) are the 7-bit device address of the slave device.
- *
- * In read mode, the result of the read is placed in the lowest available
- * EXT_SENS_DATA register. For further information regarding the allocation of
- * read results, please refer to the EXT_SENS_DATA register description
- * (Registers 73 - 96).
- *
- * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
- * characteristics, and so it has its own functions (getSlave4* and setSlave4*).
- *
- * I2C data transactions are performed at the Sample Rate, as defined in
- * Register 25. The user is responsible for ensuring that I2C data transactions
- * to and from each enabled Slave can be completed within a single period of the
- * Sample Rate.
- *
- * The I2C slave access rate can be reduced relative to the Sample Rate. This
- * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a
- * slave's access rate is reduced relative to the Sample Rate is determined by
- * I2C_MST_DELAY_CTRL (Register 103).
- *
- * The processing order for the slaves is fixed. The sequence followed for
- * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a
- * particular Slave is disabled it will be skipped.
- *
- * Each slave can either be accessed at the sample rate or at a reduced sample
- * rate. In a case where some slaves are accessed at the Sample Rate and some
- * slaves are accessed at the reduced rate, the sequence of accessing the slaves
- * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will
- * be skipped if their access rate dictates that they should not be accessed
- * during that particular cycle. For further information regarding the reduced
- * access rate, please refer to Register 52. Whether a slave is accessed at the
- * Sample Rate or at the reduced rate is determined by the Delay Enable bits in
- * Register 103.
- *
- * @param num Slave number (0-3)
- * @return Current address for specified slave
- * @see MPU6050_RA_I2C_SLV0_ADDR
- */
 uint8_t MPU6050::getSlaveAddress(uint8_t num)
 {
 	if(num > 3) {
@@ -159,12 +116,7 @@ uint8_t MPU6050::getSlaveAddress(uint8_t num)
 	}
 	return readByte(MPU6050_RA_I2C_SLV0_ADDR + num * 3);
 }
-/** Set the I2C address of the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param address New address for specified slave
- * @see getSlaveAddress()
- * @see MPU6050_RA_I2C_SLV0_ADDR
- */
+
 void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
 {
 	if(num > 3) {
@@ -172,17 +124,7 @@ void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
 	}
 	I2Cdev::writeByte(devAddr, 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
- * register address is stored in this MPU register.
- *
- * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
- * characteristics, and so it has its own functions.
- *
- * @param num Slave number (0-3)
- * @return Current active register for specified slave
- * @see MPU6050_RA_I2C_SLV0_REG
- */
+
 uint8_t MPU6050::getSlaveRegister(uint8_t num)
 {
 	if(num > 3) {
@@ -190,12 +132,7 @@ uint8_t MPU6050::getSlaveRegister(uint8_t num)
 	}
 	return readByte(MPU6050_RA_I2C_SLV0_REG + num * 3);
 }
-/** Set the active internal register for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param reg New active register for specified slave
- * @see getSlaveRegister()
- * @see MPU6050_RA_I2C_SLV0_REG
- */
+
 void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
 {
 	if(num > 3) {
@@ -203,13 +140,7 @@ void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
 	}
 	I2Cdev::writeByte(devAddr, 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
- * cleared to 0, this bit disables Slave 0 from data transfer operations.
- * @param num Slave number (0-3)
- * @return Current enabled value for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 bool MPU6050::getSlaveEnabled(uint8_t num)
 {
 	if(num > 3) {
@@ -217,12 +148,7 @@ bool MPU6050::getSlaveEnabled(uint8_t num)
 	}
 	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT);
 }
-/** Set the enabled value for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param enabled New enabled value for specified slave
- * @see getSlaveEnabled()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
 {
 	if(num > 3) {
@@ -230,17 +156,7 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
 	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, 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,
- * the high and low bytes of a word pair are swapped. Please refer to
- * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
- * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
- * registers in the order they were transferred.
- *
- * @param num Slave number (0-3)
- * @return Current word pair byte-swapping enabled value for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 bool MPU6050::getSlaveWordByteSwap(uint8_t num)
 {
 	if(num > 3) {
@@ -248,12 +164,7 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num)
 	}
 	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
 }
-/** Set word pair byte-swapping enabled for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param enabled New word pair byte-swapping enabled value for specified slave
- * @see getSlaveWordByteSwap()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
 {
 	if(num > 3) {
@@ -261,17 +172,7 @@ void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
 	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, 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
- * 0, the transaction will write a register address prior to reading or writing
- * data. This should equal 0 when specifying the register address within the
- * Slave device to/from which the ensuing data transaction will take place.
- *
- * @param num Slave number (0-3)
- * @return Current write mode for specified slave (0 = register address + data,
- * 1 = data only)
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 bool MPU6050::getSlaveWriteMode(uint8_t num)
 {
 	if(num > 3) {
@@ -279,13 +180,7 @@ bool MPU6050::getSlaveWriteMode(uint8_t num)
 	}
 	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
 }
-/** Set write mode for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param mode New write mode for specified slave (0 = register address + data,
- * 1 = data only)
- * @see getSlaveWriteMode()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
 {
 	if(num > 3) {
@@ -293,17 +188,7 @@ void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
 	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, 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.
- * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
- * then odd register addresses) are paired to form a word. When set to 1, bytes
- * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
- * register addresses) are paired to form a word.
- *
- * @param num Slave number (0-3)
- * @return Current word pair grouping order offset for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
 {
 	if(num > 3) {
@@ -311,12 +196,7 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
 	}
 	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
 }
-/** Set word pair grouping order offset for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param enabled New word pair grouping order offset for specified slave
- * @see getSlaveWordGroupOffset()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
 {
 	if(num > 3) {
@@ -324,13 +204,7 @@ void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
 	}
 	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, 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
- * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
- * @param num Slave number (0-3)
- * @return Number of bytes to read for specified slave
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 uint8_t MPU6050::getSlaveDataLength(uint8_t num)
 {
 	if(num > 3) {
@@ -338,12 +212,7 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num)
 	}
 	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
 }
-/** Set number of bytes to read for the specified slave (0-3).
- * @param num Slave number (0-3)
- * @param length Number of bytes to read for specified slave
- * @see getSlaveDataLength()
- * @see MPU6050_RA_I2C_SLV0_CTRL
- */
+
 void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
 {
 	if(num > 3) {
@@ -455,337 +324,6 @@ void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
 	I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
 }
 
-// MEM_START_ADDR register
-
-void MPU6050::setMemoryStartAddress(uint8_t address)
-{
-	I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
-}
-
-// MEM_R_W register
-
-uint8_t MPU6050::readMemoryByte()
-{
-	return readByte(MPU6050_RA_MEM_R_W);
-}
-
-void MPU6050::writeMemoryByte(uint8_t data)
-{
-	I2Cdev::writeByte(devAddr, 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);
-	for(uint16_t i = 0; i < dataSize;) {
-		// determine correct chunk size according to bank position and data size
-		uint8_t chunkSize = 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;
-		}
-
-		// read the chunk of data as specified
-		I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i);
-
-		// increase byte index by [chunkSize]
-		i += 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);
-		}
-	}
-}
-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* verifyBuffer = 0;
-	uint8_t* progBuffer = 0;
-	uint16_t i;
-	uint8_t j;
-	if(verify) {
-		verifyBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
-	}
-	if(useProgMem) {
-		progBuffer = static_cast<uint8_t*>(malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE));
-	}
-	for(i = 0; i < dataSize;) {
-		// determine correct chunk size according to bank position and data size
-		uint8_t chunkSize = 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 = const_cast<uint8_t*>(data) + i;
-		}
-
-		I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
-
-		// verify data if needed
-		if(verify && verifyBuffer) {
-			printf("VERIFY\n");
-			setMemoryBank(bank);
-			setMemoryStartAddress(address);
-			I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
-			if(memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
-				printf("Block write verification error, bank \n");
-				/*Serial.print("Block write verification error, bank ");*/
-				printf("bank %d", bank);
-				printf(", address ");
-				printf("%d", address);
-				printf("!\nExpected:");
-				for(j = 0; j < chunkSize; j++) {
-					printf("%#04x", progBuffer[j]);
-				}
-				printf("\nReceived:");
-				for(uint8_t j = 0; j < chunkSize; j++) {
-					printf("%#04x", verifyBuffer[i + j]);
-				}
-				printf("\n");
-			}
-		}
-
-		// increase byte index by [chunkSize]
-		i += 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(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, false);
-}
-bool MPU6050::writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, bool useProgMem)
-{
-	uint8_t* progBuffer = 0;
-	uint8_t success, special;
-	uint16_t i, j;
-	if(useProgMem) {
-		progBuffer = static_cast<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) {
-			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
-			if(useProgMem) {
-				if(sizeof(progBuffer) < length)
-					progBuffer = static_cast<uint8_t*>(realloc(progBuffer, length));
-				for(j = 0; j < length; j++) {
-					progBuffer[j] = pgm_read_byte(data + i + j);
-				}
-			} else {
-				progBuffer = const_cast<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++];
-			}
-			if(special == 0x01) {
-				// enable DMP-related interrupts
-
-				// setIntZeroMotionEnabled(true);
-				// setIntFIFOBufferOverflowEnabled(true);
-				// setIntDMPEnabled(true);
-				I2Cdev::writeByte(devAddr, 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(useProgMem) {
-		free(progBuffer);
-	}
-	return true;
-}
-/**
- * calibration
- *
- */
-
-/**
-  @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
-*/
-void MPU6050::CalibrateGyro(uint8_t Loops)
-{
-	double kP = 0.3;
-	double kI = 90;
-	float x;
-	x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
-	kP *= x;
-	kI *= x;
-
-	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);
-}
-
-/**
- *
- * @param ReadAddress
- * @param kP
- * @param kI
- * @param 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];
-	uint32_t eSum;
-	for(int i = 0; i < 3; i++) {
-		I2Cdev::readWord(devAddr, SaveAddress + (i * shift),
-						 (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++) {
-		int16_t eSample{0};
-		for(int c = 0; c < 100; c++) { // 100 PI Calculations
-			eSum = 0;
-			for(int i = 0; i < 3; i++) {
-				I2Cdev::readWord(devAddr, ReadAddress + (i * 2),
-								 (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::writeWord(devAddr, SaveAddress + (i * shift), Data);
-			}
-			if((c == 99) && eSum > 1000) { // Error is still to great to continue
-				c = 0;
-			}
-			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
-		}
-		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::writeWord(devAddr, SaveAddress + (i * shift), Data);
-		}
-	}
-	resetFIFO();
-	resetDMP();
-}
-
 uint8_t MPU6050::readBit(uint8_t regAddr, uint8_t bitNum)
 {
 	uint8_t bit;
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 0cae21c339..571fb2d4b8 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -1321,175 +1321,328 @@ class MPU6050
 		I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
 	}
 	// I2C_SLV* registers (Slave 0-3)
+
+	/** Get the I2C address of the specified slave (0-3).
+     * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
+     * operation, and if it is cleared, then it's a write operation. The remaining
+     * bits (6-0) are the 7-bit device address of the slave device.
+     *
+     * In read mode, the result of the read is placed in the lowest available
+     * EXT_SENS_DATA register. For further information regarding the allocation of
+     * read results, please refer to the EXT_SENS_DATA register description
+     * (Registers 73 - 96).
+     *
+     * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
+     * characteristics, and so it has its own functions (getSlave4* and setSlave4*).
+     *
+     * I2C data transactions are performed at the Sample Rate, as defined in
+     * Register 25. The user is responsible for ensuring that I2C data transactions
+     * to and from each enabled Slave can be completed within a single period of the
+     * Sample Rate.
+     *
+     * The I2C slave access rate can be reduced relative to the Sample Rate. This
+     * reduced access rate is determined by I2C_MST_DLY (Register 52). Whether a
+     * slave's access rate is reduced relative to the Sample Rate is determined by
+     * I2C_MST_DELAY_CTRL (Register 103).
+     *
+     * The processing order for the slaves is fixed. The sequence followed for
+     * processing the slaves is Slave 0, Slave 1, Slave 2, Slave 3 and Slave 4. If a
+     * particular Slave is disabled it will be skipped.
+     *
+     * Each slave can either be accessed at the sample rate or at a reduced sample
+     * rate. In a case where some slaves are accessed at the Sample Rate and some
+     * slaves are accessed at the reduced rate, the sequence of accessing the slaves
+     * (Slave 0 to Slave 4) is still followed. However, the reduced rate slaves will
+     * be skipped if their access rate dictates that they should not be accessed
+     * during that particular cycle. For further information regarding the reduced
+     * access rate, please refer to Register 52. Whether a slave is accessed at the
+     * Sample Rate or at the reduced rate is determined by the Delay Enable bits in
+     * Register 103.
+     *
+     * @param num Slave number (0-3)
+     * @return Current address for specified slave
+     * @see MPU6050_RA_I2C_SLV0_ADDR
+     */
 	uint8_t getSlaveAddress(uint8_t num);
+	/** Set the I2C address of the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param address New address for specified slave
+     * @see getSlaveAddress()
+     * @see MPU6050_RA_I2C_SLV0_ADDR
+     */
 	void setSlaveAddress(uint8_t num, uint8_t address);
+
+	/** Get the active internal register for the specified slave (0-3).
+     * Read/write operations for this slave will be done to whatever internal
+     * register address is stored in this MPU register.
+     *
+     * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
+     * characteristics, and so it has its own functions.
+     *
+     * @param num Slave number (0-3)
+     * @return Current active register for specified slave
+     * @see MPU6050_RA_I2C_SLV0_REG
+     */
 	uint8_t getSlaveRegister(uint8_t num);
+
+	/** Set the active internal register for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param reg New active register for specified slave
+     * @see getSlaveRegister()
+     * @see MPU6050_RA_I2C_SLV0_REG
+     */
 	void setSlaveRegister(uint8_t num, uint8_t 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
+     * cleared to 0, this bit disables Slave 0 from data transfer operations.
+     * @param num Slave number (0-3)
+     * @return Current enabled value for specified slave
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	bool getSlaveEnabled(uint8_t num);
+	/** Set the enabled value for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param enabled New enabled value for specified slave
+     * @see getSlaveEnabled()
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	void setSlaveEnabled(uint8_t num, bool 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,
+     * the high and low bytes of a word pair are swapped. Please refer to
+     * I2C_SLV0_GRP for the pairing convention of the word pairs. When cleared to 0,
+     * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
+     * registers in the order they were transferred.
+     *
+     * @param num Slave number (0-3)
+     * @return Current word pair byte-swapping enabled value for specified slave
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	bool getSlaveWordByteSwap(uint8_t num);
+
+	/** Set word pair byte-swapping enabled for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param enabled New word pair byte-swapping enabled value for specified slave
+     * @see getSlaveWordByteSwap()
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	void setSlaveWordByteSwap(uint8_t num, bool 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
+     * 0, the transaction will write a register address prior to reading or writing
+     * data. This should equal 0 when specifying the register address within the
+     * Slave device to/from which the ensuing data transaction will take place.
+     *
+     * @param num Slave number (0-3)
+     * @return Current write mode for specified slave (0 = register address + data,
+     * 1 = data only)
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	bool getSlaveWriteMode(uint8_t num);
+	/** Set write mode for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param mode New write mode for specified slave (0 = register address + data,
+     * 1 = data only)
+     * @see getSlaveWriteMode()
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	void setSlaveWriteMode(uint8_t num, bool 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.
+     * When cleared to 0, bytes from register addresses 0 and 1, 2 and 3, etc (even,
+     * then odd register addresses) are paired to form a word. When set to 1, bytes
+     * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
+     * register addresses) are paired to form a word.
+     *
+     * @param num Slave number (0-3)
+     * @return Current word pair grouping order offset for specified slave
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	bool getSlaveWordGroupOffset(uint8_t num);
+
+	/** Set word pair grouping order offset for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param enabled New word pair grouping order offset for specified slave
+     * @see getSlaveWordGroupOffset()
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	void setSlaveWordGroupOffset(uint8_t num, bool 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
+     * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
+     * @param num Slave number (0-3)
+     * @return Number of bytes to read for specified slave
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	uint8_t getSlaveDataLength(uint8_t num);
+
+	/** Set number of bytes to read for the specified slave (0-3).
+     * @param num Slave number (0-3)
+     * @param length Number of bytes to read for specified slave
+     * @see getSlaveDataLength()
+     * @see MPU6050_RA_I2C_SLV0_CTRL
+     */
 	void setSlaveDataLength(uint8_t num, uint8_t length);
 
 	// I2C_SLV* registers (Slave 4)
 
 	/** Get the I2C address of Slave 4.
-         * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
-         * operation, and if it is cleared, then it's a write operation. The remaining
-         * bits (6-0) are the 7-bit device address of the slave device.
-         *
-         * @return Current address for Slave 4
-         * @see getSlaveAddress()
-         * @see MPU6050_RA_I2C_SLV4_ADDR
-         */
+     * Note that Bit 7 (MSB) controls read/write mode. If Bit 7 is set, it's a read
+     * operation, and if it is cleared, then it's a write operation. The remaining
+     * bits (6-0) are the 7-bit device address of the slave device.
+     *
+     * @return Current address for Slave 4
+     * @see getSlaveAddress()
+     * @see MPU6050_RA_I2C_SLV4_ADDR
+     */
 	uint8_t getSlave4Address()
 	{
 		return readByte(MPU6050_RA_I2C_SLV4_ADDR);
 	}
 	/** Set the I2C address of Slave 4.
-         * @param address New address for Slave 4
-         * @see getSlave4Address()
-         * @see MPU6050_RA_I2C_SLV4_ADDR
-         */
+     * @param address New address for Slave 4
+     * @see getSlave4Address()
+     * @see MPU6050_RA_I2C_SLV4_ADDR
+     */
 	void setSlave4Address(uint8_t address)
 	{
 		I2Cdev::writeByte(devAddr, 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
-         * register address is stored in this MPU register.
-         *
-         * @return Current active register for Slave 4
-         * @see MPU6050_RA_I2C_SLV4_REG
-         */
+     * Read/write operations for this slave will be done to whatever internal
+     * register address is stored in this MPU register.
+     *
+     * @return Current active register for Slave 4
+     * @see MPU6050_RA_I2C_SLV4_REG
+     */
 	uint8_t getSlave4Register()
 	{
 		return readByte(MPU6050_RA_I2C_SLV4_REG);
 	}
 	/** Set the active internal register for Slave 4.
-         * @param reg New active register for Slave 4
-         * @see getSlave4Register()
-         * @see MPU6050_RA_I2C_SLV4_REG
-         */
+     * @param reg New active register for Slave 4
+     * @see getSlave4Register()
+     * @see MPU6050_RA_I2C_SLV4_REG
+     */
 	void setSlave4Register(uint8_t reg)
 	{
 		I2Cdev::writeByte(devAddr, 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
-         * is set 1 (set to read), this register has no effect.
-         * @param data New byte to write to Slave 4
-         * @see MPU6050_RA_I2C_SLV4_DO
-         */
+     * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW
+     * is set 1 (set to read), this register has no effect.
+     * @param data New byte to write to Slave 4
+     * @see MPU6050_RA_I2C_SLV4_DO
+     */
 	void setSlave4OutputByte(uint8_t data)
 	{
 		I2Cdev::writeByte(devAddr, 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
-         * cleared to 0, this bit disables Slave 4 from data transfer operations.
-         * @return Current enabled value for Slave 4
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * When set to 1, this bit enables Slave 4 for data transfer operations. When
+     * cleared to 0, this bit disables Slave 4 from data transfer operations.
+     * @return Current enabled value for Slave 4
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	bool getSlave4Enabled()
 	{
 		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT);
 	}
 	/** Set the enabled value for Slave 4.
-         * @param enabled New enabled value for Slave 4
-         * @see getSlave4Enabled()
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * @param enabled New enabled value for Slave 4
+     * @see getSlave4Enabled()
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	void setSlave4Enabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
-         * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
-         * generation of an interrupt signal upon completion of a Slave 4 transaction.
-         * The interrupt status can be observed in Register 54.
-         *
-         * @return Current enabled value for Slave 4 transaction interrupts.
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * When set to 1, this bit enables the generation of an interrupt signal upon
+     * completion of a Slave 4 transaction. When cleared to 0, this bit disables the
+     * generation of an interrupt signal upon completion of a Slave 4 transaction.
+     * The interrupt status can be observed in Register 54.
+     *
+     * @return Current enabled value for Slave 4 transaction interrupts.
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	bool getSlave4InterruptEnabled()
 	{
 		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT);
 	}
 	/** Set the enabled value for Slave 4 transaction interrupts.
-         * @param enabled New enabled value for Slave 4 transaction interrupts.
-         * @see getSlave4InterruptEnabled()
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * @param enabled New enabled value for Slave 4 transaction interrupts.
+     * @see getSlave4InterruptEnabled()
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	void setSlave4InterruptEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
-         * 0, the transaction will write a register address prior to reading or writing
-         * data. This should equal 0 when specifying the register address within the
-         * Slave device to/from which the ensuing data transaction will take place.
-         *
-         * @return Current write mode for Slave 4 (0 = register address + data, 1 = data
-         * only)
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * When set to 1, the transaction will read or write data only. When cleared to
+     * 0, the transaction will write a register address prior to reading or writing
+     * data. This should equal 0 when specifying the register address within the
+     * Slave device to/from which the ensuing data transaction will take place.
+     *
+     * @return Current write mode for Slave 4 (0 = register address + data, 1 = data
+     * only)
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	bool getSlave4WriteMode()
 	{
 		return readBit(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT);
 	}
 	/** Set write mode for the Slave 4.
-         * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data
-         * only)
-         * @see getSlave4WriteMode()
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * @param mode New write mode for Slave 4 (0 = register address + data, 1 = data
+     * only)
+     * @see getSlave4WriteMode()
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	void setSlave4WriteMode(bool mode)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, 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
-         * Rate. When a slave's access rate is decreased relative to the Sample Rate,
-         * the slave is accessed every:
-         *
-         *     1 / (1 + I2C_MST_DLY) samples
-         *
-         * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
-         * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
-         * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
-         * further information regarding the Sample Rate, please refer to register 25.
-         *
-         * @return Current Slave 4 master delay value
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * This configures the reduced access rate of I2C slaves relative to the Sample
+     * Rate. When a slave's access rate is decreased relative to the Sample Rate,
+     * the slave is accessed every:
+     *
+     *     1 / (1 + I2C_MST_DLY) samples
+     *
+     * This base Sample Rate in turn is determined by SMPLRT_DIV (register 25) and
+     * DLPF_CFG (register 26). Whether a slave's access rate is reduced relative to
+     * the Sample Rate is determined by I2C_MST_DELAY_CTRL (register 103). For
+     * further information regarding the Sample Rate, please refer to register 25.
+     *
+     * @return Current Slave 4 master delay value
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	uint8_t getSlave4MasterDelay()
 	{
 		return readBits(MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH);
 	}
 	/** Set Slave 4 master delay value.
-         * @param delay New Slave 4 master delay value
-         * @see getSlave4MasterDelay()
-         * @see MPU6050_RA_I2C_SLV4_CTRL
-         */
+     * @param delay New Slave 4 master delay value
+     * @see getSlave4MasterDelay()
+     * @see MPU6050_RA_I2C_SLV4_CTRL
+     */
 	void setSlave4MasterDelay(uint8_t delay)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT,
 						  MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
 	}
 	/** Get last available byte read from Slave 4.
-         * This register stores the data read from Slave 4. This field is populated
-         * after a read transaction.
-         * @return Last available byte read from to Slave 4
-         * @see MPU6050_RA_I2C_SLV4_DI
-         */
+     * This register stores the data read from Slave 4. This field is populated
+     * after a read transaction.
+     * @return Last available byte read from to Slave 4
+     * @see MPU6050_RA_I2C_SLV4_DI
+     */
 	uint8_t getSlate4InputByte()
 	{
 		return readByte(MPU6050_RA_I2C_SLV4_DI);
@@ -1498,92 +1651,92 @@ class MPU6050
 	// I2C_MST_STATUS register
 
 	/** Get FSYNC interrupt status.
-         * This bit reflects the status of the FSYNC interrupt from an external device
-         * into the MPU-60X0. This is used as a way to pass an external interrupt
-         * through the MPU-60X0 to the host application processor. When set to 1, this
-         * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
-         * (Register 55).
-         * @return FSYNC interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit reflects the status of the FSYNC interrupt from an external device
+     * into the MPU-60X0. This is used as a way to pass an external interrupt
+     * through the MPU-60X0 to the host application processor. When set to 1, this
+     * bit will cause an interrupt if FSYNC_INT_EN is asserted in INT_PIN_CFG
+     * (Register 55).
+     * @return FSYNC interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getPassthroughStatus()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT);
 	}
 	/** Get Slave 4 transaction done status.
-         * Automatically sets to 1 when a Slave 4 transaction has completed. This
-         * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
-         * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
-         * I2C_SLV4_CTRL register (Register 52).
-         * @return Slave 4 transaction done status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * Automatically sets to 1 when a Slave 4 transaction has completed. This
+     * triggers an interrupt if the I2C_MST_INT_EN bit in the INT_ENABLE register
+     * (Register 56) is asserted and if the SLV_4_DONE_INT bit is asserted in the
+     * I2C_SLV4_CTRL register (Register 52).
+     * @return Slave 4 transaction done status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave4IsDone()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT);
 	}
 	/** Get master arbitration lost status.
-         * This bit automatically sets to 1 when the I2C Master has lost arbitration of
-         * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
-         * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Master arbitration lost status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master has lost arbitration of
+     * the auxiliary I2C bus (an error condition). This triggers an interrupt if the
+     * I2C_MST_INT_EN bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Master arbitration lost status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getLostArbitration()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT);
 	}
 	/** Get Slave 4 NACK status.
-         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
-         * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
-         * bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Slave 4 NACK interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+     * transaction with Slave 4. This triggers an interrupt if the I2C_MST_INT_EN
+     * bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Slave 4 NACK interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave4Nack()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT);
 	}
 	/** Get Slave 3 NACK status.
-         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
-         * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
-         * bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Slave 3 NACK interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+     * transaction with Slave 3. This triggers an interrupt if the I2C_MST_INT_EN
+     * bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Slave 3 NACK interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave3Nack()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT);
 	}
 	/** Get Slave 2 NACK status.
-         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
-         * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
-         * bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Slave 2 NACK interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+     * transaction with Slave 2. This triggers an interrupt if the I2C_MST_INT_EN
+     * bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Slave 2 NACK interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave2Nack()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT);
 	}
 	/** Get Slave 1 NACK status.
-         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
-         * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
-         * bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Slave 1 NACK interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+     * transaction with Slave 1. This triggers an interrupt if the I2C_MST_INT_EN
+     * bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Slave 1 NACK interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave1Nack()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT);
 	}
 	/** Get Slave 0 NACK status.
-         * This bit automatically sets to 1 when the I2C Master receives a NACK in a
-         * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
-         * bit in the INT_ENABLE register (Register 56) is asserted.
-         * @return Slave 0 NACK interrupt status
-         * @see MPU6050_RA_I2C_MST_STATUS
-         */
+     * This bit automatically sets to 1 when the I2C Master receives a NACK in a
+     * transaction with Slave 0. This triggers an interrupt if the I2C_MST_INT_EN
+     * bit in the INT_ENABLE register (Register 56) is asserted.
+     * @return Slave 0 NACK interrupt status
+     * @see MPU6050_RA_I2C_MST_STATUS
+     */
 	bool getSlave0Nack()
 	{
 		return readBit(MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT);
@@ -1592,177 +1745,177 @@ class MPU6050
 	// INT_PIN_CFG register
 
 	/** Get interrupt logic level mode.
-         * Will be set 0 for active-high, 1 for active-low.
-         * @return Current interrupt mode (0=active-high, 1=active-low)
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_LEVEL_BIT
-         */
+     * Will be set 0 for active-high, 1 for active-low.
+     * @return Current interrupt mode (0=active-high, 1=active-low)
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_LEVEL_BIT
+     */
 	bool getInterruptMode()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT);
 	}
 	/** Set interrupt logic level mode.
-         * @param mode New interrupt mode (0=active-high, 1=active-low)
-         * @see getInterruptMode()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_LEVEL_BIT
-         */
+     * @param mode New interrupt mode (0=active-high, 1=active-low)
+     * @see getInterruptMode()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_LEVEL_BIT
+     */
 	void setInterruptMode(bool mode)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode);
 	}
 	/** Get interrupt drive mode.
-         * Will be set 0 for push-pull, 1 for open-drain.
-         * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_OPEN_BIT
-         */
+     * Will be set 0 for push-pull, 1 for open-drain.
+     * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_OPEN_BIT
+     */
 	bool getInterruptDrive()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT);
 	}
 	/** Set interrupt drive mode.
-         * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
-         * @see getInterruptDrive()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_OPEN_BIT
-         */
+     * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
+     * @see getInterruptDrive()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_OPEN_BIT
+     */
 	void setInterruptDrive(bool drive)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive);
 	}
 	/** Get interrupt latch mode.
-         * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
-         * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
-         */
+     * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
+     * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+     */
 	bool getInterruptLatch()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT);
 	}
 	/** Set interrupt latch mode.
-         * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
-         * @see getInterruptLatch()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
-         */
+     * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
+     * @see getInterruptLatch()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_LATCH_INT_EN_BIT
+     */
 	void setInterruptLatch(bool latch)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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.
-         * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
-         */
+     * Will be set 0 for status-read-only, 1 for any-register-read.
+     * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+     */
 	bool getInterruptLatchClear()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT);
 	}
 	/** Set interrupt latch clear mode.
-         * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
-         * @see getInterruptLatchClear()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
-         */
+     * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
+     * @see getInterruptLatchClear()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT
+     */
 	void setInterruptLatchClear(bool clear)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear);
 	}
 	/** Get FSYNC interrupt logic level mode.
-         * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
-         * @see getFSyncInterruptMode()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
-         */
+     * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
+     * @see getFSyncInterruptMode()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+     */
 	bool getFSyncInterruptLevel()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT);
 	}
 	/** Set FSYNC interrupt logic level mode.
-         * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
-         * @see getFSyncInterruptMode()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
-         */
+     * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
+     * @see getFSyncInterruptMode()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT
+     */
 	void setFSyncInterruptLevel(bool level)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level);
 	}
 	/** Get FSYNC pin interrupt enabled setting.
-         * Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled setting
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
-         */
+     * Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled setting
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+     */
 	bool getFSyncInterruptEnabled()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT);
 	}
 	/** Set FSYNC pin interrupt enabled setting.
-         * @param enabled New FSYNC pin interrupt enabled setting
-         * @see getFSyncInterruptEnabled()
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
-         */
+     * @param enabled New FSYNC pin interrupt enabled setting
+     * @see getFSyncInterruptEnabled()
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT
+     */
 	void setFSyncInterruptEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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
-         * 0, the host application processor will be able to directly access the
-         * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
-         * application processor will not be able to directly access the auxiliary I2C
-         * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
-         * bit[5]).
-         * @return Current I2C bypass enabled status
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
-         */
+     * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
+     * 0, the host application processor will be able to directly access the
+     * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+     * application processor will not be able to directly access the auxiliary I2C
+     * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+     * bit[5]).
+     * @return Current I2C bypass enabled status
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+     */
 	bool getI2CBypassEnabled()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT);
 	}
 	/** Set I2C bypass enabled status.
-         * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
-         * 0, the host application processor will be able to directly access the
-         * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
-         * application processor will not be able to directly access the auxiliary I2C
-         * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
-         * bit[5]).
-         * @param enabled New I2C bypass enabled status
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
-         */
+     * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to
+     * 0, the host application processor will be able to directly access the
+     * auxiliary I2C bus of the MPU-60X0. When this bit is equal to 0, the host
+     * application processor will not be able to directly access the auxiliary I2C
+     * bus of the MPU-60X0 regardless of the state of I2C_MST_EN (Register 106
+     * bit[5]).
+     * @param enabled New I2C bypass enabled status
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT
+     */
 	void setI2CBypassEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, 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
-         * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
-         * further information regarding CLKOUT, please refer to the MPU-60X0 Product
-         * Specification document.
-         * @return Current reference clock output enabled status
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_CLKOUT_EN_BIT
-         */
+     * When this bit is equal to 1, a reference clock output is provided at the
+     * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+     * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+     * Specification document.
+     * @return Current reference clock output enabled status
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+     */
 	bool getClockOutputEnabled()
 	{
 		return readBit(MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT);
 	}
 	/** Set reference clock output enabled status.
-         * When this bit is equal to 1, a reference clock output is provided at the
-         * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
-         * further information regarding CLKOUT, please refer to the MPU-60X0 Product
-         * Specification document.
-         * @param enabled New reference clock output enabled status
-         * @see MPU6050_RA_INT_PIN_CFG
-         * @see MPU6050_INTCFG_CLKOUT_EN_BIT
-         */
+     * When this bit is equal to 1, a reference clock output is provided at the
+     * CLKOUT pin. When this bit is equal to 0, the clock output is disabled. For
+     * further information regarding CLKOUT, please refer to the MPU-60X0 Product
+     * Specification document.
+     * @param enabled New reference clock output enabled status
+     * @see MPU6050_RA_INT_PIN_CFG
+     * @see MPU6050_INTCFG_CLKOUT_EN_BIT
+     */
 	void setClockOutputEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled);
@@ -1771,146 +1924,146 @@ class MPU6050
 	// INT_ENABLE register
 
 	/** Get full interrupt enabled status.
-         * Full register byte for all interrupts, for quick reading. Each bit will be
-         * set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FF_BIT
-         **/
+     * Full register byte for all interrupts, for quick reading. Each bit will be
+     * set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FF_BIT
+     **/
 	uint8_t getIntEnabled()
 	{
 		return readByte(MPU6050_RA_INT_ENABLE);
 	}
 	/** Set full interrupt enabled status.
-         * Full register byte for all interrupts, for quick reading. Each bit should be
-         * set 0 for disabled, 1 for enabled.
-         * @param enabled New interrupt enabled status
-         * @see getIntFreefallEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FF_BIT
-         **/
+     * Full register byte for all interrupts, for quick reading. Each bit should be
+     * set 0 for disabled, 1 for enabled.
+     * @param enabled New interrupt enabled status
+     * @see getIntFreefallEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FF_BIT
+     **/
 	void setIntEnabled(uint8_t enabled)
 	{
 		I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled);
 	}
 	/** Get Free Fall interrupt enabled status.
-         * Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FF_BIT
-         **/
+     * Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FF_BIT
+     **/
 	bool getIntFreefallEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT);
 	}
 	/** Set Free Fall interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntFreefallEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FF_BIT
-         **/
+     * @param enabled New interrupt enabled status
+     * @see getIntFreefallEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FF_BIT
+     **/
 	void setIntFreefallEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled);
 	}
 	/** Get Motion Detection interrupt enabled status.
-         * Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_MOT_BIT
-         **/
+     * Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_MOT_BIT
+     **/
 	bool getIntMotionEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT);
 	}
 	/** Set Motion Detection interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntMotionEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_MOT_BIT
-         **/
+     * @param enabled New interrupt enabled status
+     * @see getIntMotionEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_MOT_BIT
+     **/
 	void setIntMotionEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled);
 	}
 	/** Get Zero Motion Detection interrupt enabled status.
-         * Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_ZMOT_BIT
-         **/
+     * Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_ZMOT_BIT
+     **/
 	bool getIntZeroMotionEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT);
 	}
 	/** Set Zero Motion Detection interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntZeroMotionEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_ZMOT_BIT
-         **/
+     * @param enabled New interrupt enabled status
+     * @see getIntZeroMotionEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_ZMOT_BIT
+     **/
 	void setIntZeroMotionEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled);
 	}
 	/** Get FIFO Buffer Overflow interrupt enabled status.
-         * Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
-         **/
+     * Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+     **/
 	bool getIntFIFOBufferOverflowEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
 	}
 	/** Set FIFO Buffer Overflow interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntFIFOBufferOverflowEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
-         **/
+     * @param enabled New interrupt enabled status
+     * @see getIntFIFOBufferOverflowEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+     **/
 	void setIntFIFOBufferOverflowEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled);
 	}
 	/** Get I2C Master interrupt enabled status.
-         * This enables any of the I2C Master interrupt sources to generate an
-         * interrupt. Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
-         **/
+     * This enables any of the I2C Master interrupt sources to generate an
+     * interrupt. Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+     **/
 	bool getIntI2CMasterEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
 	}
 	/** Set I2C Master interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntI2CMasterEnabled()
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
-         **/
+     * @param enabled New interrupt enabled status
+     * @see getIntI2CMasterEnabled()
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+     **/
 	void setIntI2CMasterEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, 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
-         * has been completed. Will be set 0 for disabled, 1 for enabled.
-         * @return Current interrupt enabled status
-         * @see MPU6050_RA_INT_ENABLE
-         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
-         */
+     * This event occurs each time a write operation to all of the sensor registers
+     * has been completed. Will be set 0 for disabled, 1 for enabled.
+     * @return Current interrupt enabled status
+     * @see MPU6050_RA_INT_ENABLE
+     * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+     */
 	bool getIntDataReadyEnabled()
 	{
 		return readBit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT);
 	}
 	/** Set Data Ready interrupt enabled status.
-         * @param enabled New interrupt enabled status
-         * @see getIntDataReadyEnabled()
-         * @see MPU6050_RA_INT_CFG
-         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
-         */
+     * @param enabled New interrupt enabled status
+     * @see getIntDataReadyEnabled()
+     * @see MPU6050_RA_INT_CFG
+     * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+     */
 	void setIntDataReadyEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled);
@@ -1919,83 +2072,83 @@ class MPU6050
 	// INT_STATUS register
 
 	/** Get full set of interrupt status bits.
-         * These bits clear to 0 after the register has been read. Very useful
-         * for getting multiple INT statuses, since each single bit read clears
-         * all of them because it has to read the whole byte.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         */
+     * These bits clear to 0 after the register has been read. Very useful
+     * for getting multiple INT statuses, since each single bit read clears
+     * all of them because it has to read the whole byte.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     */
 	uint8_t getIntStatus()
 	{
 		return readByte(MPU6050_RA_INT_STATUS);
 	}
 	/** Get Free Fall interrupt status.
-         * This bit automatically sets to 1 when a Free Fall interrupt has been
-         * generated. The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_FF_BIT
-         */
+     * This bit automatically sets to 1 when a Free Fall interrupt has been
+     * generated. The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_FF_BIT
+     */
 	bool getIntFreefallStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT);
 	}
 	/** Get Motion Detection interrupt status.
-         * This bit automatically sets to 1 when a Motion Detection interrupt has been
-         * generated. The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_MOT_BIT
-         */
+     * This bit automatically sets to 1 when a Motion Detection interrupt has been
+     * generated. The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_MOT_BIT
+     */
 	bool getIntMotionStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT);
 	}
 
 	/** Get Zero Motion Detection interrupt status.
-         * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
-         * been generated. The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_ZMOT_BIT
-         */
+     * This bit automatically sets to 1 when a Zero Motion Detection interrupt has
+     * been generated. The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_ZMOT_BIT
+     */
 	bool getIntZeroMotionStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT);
 	}
 
 	/** Get FIFO Buffer Overflow interrupt status.
-         * This bit automatically sets to 1 when a Free Fall interrupt has been
-         * generated. The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
-         */
+     * This bit automatically sets to 1 when a Free Fall interrupt has been
+     * generated. The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT
+     */
 	bool getIntFIFOBufferOverflowStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT);
 	}
 
 	/** Get I2C Master interrupt status.
-         * This bit automatically sets to 1 when an I2C Master interrupt has been
-         * generated. For a list of I2C Master interrupts, please refer to Register 54.
-         * The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
-         */
+     * This bit automatically sets to 1 when an I2C Master interrupt has been
+     * generated. For a list of I2C Master interrupts, please refer to Register 54.
+     * The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT
+     */
 	bool getIntI2CMasterStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT);
 	}
 
 	/** Get Data Ready interrupt status.
-         * This bit automatically sets to 1 when a Data Ready interrupt has been
-         * generated. The bit clears to 0 after the register has been read.
-         * @return Current interrupt status
-         * @see MPU6050_RA_INT_STATUS
-         * @see MPU6050_INTERRUPT_DATA_RDY_BIT
-         */
+     * This bit automatically sets to 1 when a Data Ready interrupt has been
+     * generated. The bit clears to 0 after the register has been read.
+     * @return Current interrupt status
+     * @see MPU6050_RA_INT_STATUS
+     * @see MPU6050_INTERRUPT_DATA_RDY_BIT
+     */
 	bool getIntDataReadyStatus()
 	{
 		return readBit(MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT);
@@ -2004,75 +2157,75 @@ class MPU6050
 	// ACCEL_*OUT_* registers
 
 	/** Get raw 6-axis motion sensor readings (accel/gyro).
-         * Retrieves all currently available motion sensor values.
-         * @return container for 3-axis accelerometer and 3-axis gyroscope values
-         * @see getAcceleration()
-         * @see getAngularRate()
-         * @see MPU6050_RA_ACCEL_XOUT_H
-         */
+     * Retrieves all currently available motion sensor values.
+     * @return container for 3-axis accelerometer and 3-axis gyroscope values
+     * @see getAcceleration()
+     * @see getAngularRate()
+     * @see MPU6050_RA_ACCEL_XOUT_H
+     */
 	Motion6 getMotion6();
 
 	/** Get 3-axis accelerometer readings.
-         * These registers store the most recent accelerometer measurements.
-         * Accelerometer measurements are written to these registers at the Sample Rate
-         * as defined in Register 25.
-         *
-         * The accelerometer measurement registers, along with the temperature
-         * measurement registers, gyroscope measurement registers, and external sensor
-         * data registers, are composed of two sets of registers: an internal register
-         * set and a user-facing read register set.
-         *
-         * The data within the accelerometer sensors' internal register set is always
-         * updated at the Sample Rate. Meanwhile, the user-facing read register set
-         * duplicates the internal register set's data values whenever the serial
-         * interface is idle. This guarantees that a burst read of sensor registers will
-         * read measurements from the same sampling instant. Note that if burst reads
-         * are not used, the user is responsible for ensuring a set of single byte reads
-         * correspond to a single sampling instant by checking the Data Ready interrupt.
-         *
-         * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
-         * (Register 28). For each full scale setting, the accelerometers' sensitivity
-         * per LSB in ACCEL_xOUT is shown in the table below:
-         *
-         * <pre>
-         * AFS_SEL | Full Scale Range | LSB Sensitivity
-         * --------+------------------+----------------
-         * 0       | +/- 2g           | 8192 LSB/mg
-         * 1       | +/- 4g           | 4096 LSB/mg
-         * 2       | +/- 8g           | 2048 LSB/mg
-         * 3       | +/- 16g          | 1024 LSB/mg
-         * </pre>
-         *
-         * @param x 16-bit signed integer container for X-axis acceleration
-         * @param y 16-bit signed integer container for Y-axis acceleration
-         * @param z 16-bit signed integer container for Z-axis acceleration
-         * @see MPU6050_RA_GYRO_XOUT_H
-         */
+     * These registers store the most recent accelerometer measurements.
+     * Accelerometer measurements are written to these registers at the Sample Rate
+     * as defined in Register 25.
+     *
+     * The accelerometer measurement registers, along with the temperature
+     * measurement registers, gyroscope measurement registers, and external sensor
+     * data registers, are composed of two sets of registers: an internal register
+     * set and a user-facing read register set.
+     *
+     * The data within the accelerometer sensors' internal register set is always
+     * updated at the Sample Rate. Meanwhile, the user-facing read register set
+     * duplicates the internal register set's data values whenever the serial
+     * interface is idle. This guarantees that a burst read of sensor registers will
+     * read measurements from the same sampling instant. Note that if burst reads
+     * are not used, the user is responsible for ensuring a set of single byte reads
+     * correspond to a single sampling instant by checking the Data Ready interrupt.
+     *
+     * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
+     * (Register 28). For each full scale setting, the accelerometers' sensitivity
+     * per LSB in ACCEL_xOUT is shown in the table below:
+     *
+     * <pre>
+     * AFS_SEL | Full Scale Range | LSB Sensitivity
+     * --------+------------------+----------------
+     * 0       | +/- 2g           | 8192 LSB/mg
+     * 1       | +/- 4g           | 4096 LSB/mg
+     * 2       | +/- 8g           | 2048 LSB/mg
+     * 3       | +/- 16g          | 1024 LSB/mg
+     * </pre>
+     *
+     * @param x 16-bit signed integer container for X-axis acceleration
+     * @param y 16-bit signed integer container for Y-axis acceleration
+     * @param z 16-bit signed integer container for Z-axis acceleration
+     * @see MPU6050_RA_GYRO_XOUT_H
+     */
 	Motion3 getAcceleration();
 
 	/** Get X-axis accelerometer reading.
-         * @return X-axis acceleration measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_ACCEL_XOUT_H
-         */
+     * @return X-axis acceleration measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_ACCEL_XOUT_H
+     */
 	int16_t getAccelerationX()
 	{
 		return readReg<int16_t>(MPU6050_RA_ACCEL_XOUT_H);
 	}
 	/** Get Y-axis accelerometer reading.
-         * @return Y-axis acceleration measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_ACCEL_YOUT_H
-         */
+     * @return Y-axis acceleration measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_ACCEL_YOUT_H
+     */
 	int16_t getAccelerationY()
 	{
 		return readReg<int16_t>(MPU6050_RA_ACCEL_YOUT_H);
 	}
 	/** Get Z-axis accelerometer reading.
-         * @return Z-axis acceleration measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_ACCEL_ZOUT_H
-         */
+     * @return Z-axis acceleration measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_ACCEL_ZOUT_H
+     */
 	int16_t getAccelerationZ()
 	{
 		return readReg<int16_t>(MPU6050_RA_ACCEL_ZOUT_H);
@@ -2081,9 +2234,9 @@ class MPU6050
 	// TEMP_OUT_* registers
 
 	/** Get current internal temperature.
-         * @return Temperature reading in 16-bit 2's complement format
-         * @see MPU6050_RA_TEMP_OUT_H
-         */
+     * @return Temperature reading in 16-bit 2's complement format
+     * @see MPU6050_RA_TEMP_OUT_H
+     */
 	int16_t getTemperature()
 	{
 		return readReg<int16_t>(MPU6050_RA_TEMP_OUT_H);
@@ -2092,60 +2245,60 @@ class MPU6050
 	// GYRO_*OUT_* registers
 
 	/** Get 3-axis gyroscope readings.
-         * These gyroscope measurement registers, along with the accelerometer
-         * measurement registers, temperature measurement registers, and external sensor
-         * data registers, are composed of two sets of registers: an internal register
-         * set and a user-facing read register set.
-         * The data within the gyroscope sensors' internal register set is always
-         * updated at the Sample Rate. Meanwhile, the user-facing read register set
-         * duplicates the internal register set's data values whenever the serial
-         * interface is idle. This guarantees that a burst read of sensor registers will
-         * read measurements from the same sampling instant. Note that if burst reads
-         * are not used, the user is responsible for ensuring a set of single byte reads
-         * correspond to a single sampling instant by checking the Data Ready interrupt.
-         *
-         * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
-         * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
-         * LSB in GYRO_xOUT is shown in the table below:
-         *
-         * <pre>
-         * FS_SEL | Full Scale Range   | LSB Sensitivity
-         * -------+--------------------+----------------
-         * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
-         * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
-         * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
-         * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
-         * </pre>
-         *
-         * @return container for 3-axis gyro values
-         * @see getMotion6()
-         * @see MPU6050_RA_GYRO_XOUT_H
-         */
+     * These gyroscope measurement registers, along with the accelerometer
+     * measurement registers, temperature measurement registers, and external sensor
+     * data registers, are composed of two sets of registers: an internal register
+     * set and a user-facing read register set.
+     * The data within the gyroscope sensors' internal register set is always
+     * updated at the Sample Rate. Meanwhile, the user-facing read register set
+     * duplicates the internal register set's data values whenever the serial
+     * interface is idle. This guarantees that a burst read of sensor registers will
+     * read measurements from the same sampling instant. Note that if burst reads
+     * are not used, the user is responsible for ensuring a set of single byte reads
+     * correspond to a single sampling instant by checking the Data Ready interrupt.
+     *
+     * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
+     * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
+     * LSB in GYRO_xOUT is shown in the table below:
+     *
+     * <pre>
+     * FS_SEL | Full Scale Range   | LSB Sensitivity
+     * -------+--------------------+----------------
+     * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
+     * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
+     * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
+     * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
+     * </pre>
+     *
+     * @return container for 3-axis gyro values
+     * @see getMotion6()
+     * @see MPU6050_RA_GYRO_XOUT_H
+     */
 	Motion3 getAngularRate();
 
 	/** Get X-axis gyroscope reading.
-         * @return X-axis rotation measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_GYRO_XOUT_H
-         */
+     * @return X-axis rotation measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_GYRO_XOUT_H
+     */
 	int16_t getAngularRateX()
 	{
 		return readReg<int16_t>(MPU6050_RA_GYRO_XOUT_H);
 	}
 	/** Get Y-axis gyroscope reading.
-         * @return Y-axis rotation measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_GYRO_YOUT_H
-         */
+     * @return Y-axis rotation measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_GYRO_YOUT_H
+     */
 	int16_t getAngularRateY()
 	{
 		return readReg<int16_t>(MPU6050_RA_GYRO_YOUT_H);
 	}
 	/** Get Z-axis gyroscope reading.
-         * @return Z-axis rotation measurement in 16-bit 2's complement format
-         * @see getMotion6()
-         * @see MPU6050_RA_GYRO_ZOUT_H
-         */
+     * @return Z-axis rotation measurement in 16-bit 2's complement format
+     * @see getMotion6()
+     * @see MPU6050_RA_GYRO_ZOUT_H
+     */
 	int16_t getAngularRateZ()
 	{
 		return readReg<int16_t>(MPU6050_RA_GYRO_ZOUT_H);
@@ -2159,97 +2312,97 @@ class MPU6050
 	// EXT_SENS_DATA_* registers
 
 	/** Read single byte from external sensor data register.
-         * These registers store data read from external sensors by the Slave 0, 1, 2,
-         * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
-         * I2C_SLV4_DI (Register 53).
-         *
-         * External sensor data is written to these registers at the Sample Rate as
-         * defined in Register 25. This access rate can be reduced by using the Slave
-         * Delay Enable registers (Register 103).
-         *
-         * External sensor data registers, along with the gyroscope measurement
-         * registers, accelerometer measurement registers, and temperature measurement
-         * registers, are composed of two sets of registers: an internal register set
-         * and a user-facing read register set.
-         *
-         * The data within the external sensors' internal register set is always updated
-         * at the Sample Rate (or the reduced access rate) whenever the serial interface
-         * is idle. This guarantees that a burst read of sensor registers will read
-         * measurements from the same sampling instant. Note that if burst reads are not
-         * used, the user is responsible for ensuring a set of single byte reads
-         * correspond to a single sampling instant by checking the Data Ready interrupt.
-         *
-         * Data is placed in these external sensor data registers according to
-         * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
-         * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
-         * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
-         * defined in Register 25) or delayed rate (if specified in Register 52 and
-         * 103). During each Sample cycle, slave reads are performed in order of Slave
-         * number. If all slaves are enabled with more than zero bytes to be read, the
-         * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
-         *
-         * Each enabled slave will have EXT_SENS_DATA registers associated with it by
-         * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
-         * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
-         * change the higher numbered slaves' associated registers. Furthermore, if
-         * fewer total bytes are being read from the external sensors as a result of
-         * such a change, then the data remaining in the registers which no longer have
-         * an associated slave device (i.e. high numbered registers) will remain in
-         * these previously allocated registers unless reset.
-         *
-         * If the sum of the read lengths of all SLVx transactions exceed the number of
-         * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
-         * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
-         * the slaves cannot be greater than 24 or some bytes will be lost.
-         *
-         * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
-         * information regarding the characteristics of Slave 4, please refer to
-         * Registers 49 to 53.
-         *
-         * EXAMPLE:
-         * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
-         * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
-         * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
-         * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
-         * will be associated with Slave 1. If Slave 2 is enabled as well, registers
-         * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
-         *
-         * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
-         * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
-         * instead.
-         *
-         * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
-         * If a slave is disabled at any time, the space initially allocated to the
-         * slave in the EXT_SENS_DATA register, will remain associated with that slave.
-         * This is to avoid dynamic adjustment of the register allocation.
-         *
-         * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
-         * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
-         *
-         * This above is also true if one of the slaves gets NACKed and stops
-         * functioning.
-         *
-        * @param position Starting position (0-23)
-            * @return Byte read from register
-            */
+     * These registers store data read from external sensors by the Slave 0, 1, 2,
+     * and 3 on the auxiliary I2C interface. Data read by Slave 4 is stored in
+     * I2C_SLV4_DI (Register 53).
+     *
+     * External sensor data is written to these registers at the Sample Rate as
+     * defined in Register 25. This access rate can be reduced by using the Slave
+     * Delay Enable registers (Register 103).
+     *
+     * External sensor data registers, along with the gyroscope measurement
+     * registers, accelerometer measurement registers, and temperature measurement
+     * registers, are composed of two sets of registers: an internal register set
+     * and a user-facing read register set.
+     *
+     * The data within the external sensors' internal register set is always updated
+     * at the Sample Rate (or the reduced access rate) whenever the serial interface
+     * is idle. This guarantees that a burst read of sensor registers will read
+     * measurements from the same sampling instant. Note that if burst reads are not
+     * used, the user is responsible for ensuring a set of single byte reads
+     * correspond to a single sampling instant by checking the Data Ready interrupt.
+     *
+     * Data is placed in these external sensor data registers according to
+     * I2C_SLV0_CTRL, I2C_SLV1_CTRL, I2C_SLV2_CTRL, and I2C_SLV3_CTRL (Registers 39,
+     * 42, 45, and 48). When more than zero bytes are read (I2C_SLVx_LEN > 0) from
+     * an enabled slave (I2C_SLVx_EN = 1), the slave is read at the Sample Rate (as
+     * defined in Register 25) or delayed rate (if specified in Register 52 and
+     * 103). During each Sample cycle, slave reads are performed in order of Slave
+     * number. If all slaves are enabled with more than zero bytes to be read, the
+     * order will be Slave 0, followed by Slave 1, Slave 2, and Slave 3.
+     *
+     * Each enabled slave will have EXT_SENS_DATA registers associated with it by
+     * number of bytes read (I2C_SLVx_LEN) in order of slave number, starting from
+     * EXT_SENS_DATA_00. Note that this means enabling or disabling a slave may
+     * change the higher numbered slaves' associated registers. Furthermore, if
+     * fewer total bytes are being read from the external sensors as a result of
+     * such a change, then the data remaining in the registers which no longer have
+     * an associated slave device (i.e. high numbered registers) will remain in
+     * these previously allocated registers unless reset.
+     *
+     * If the sum of the read lengths of all SLVx transactions exceed the number of
+     * available EXT_SENS_DATA registers, the excess bytes will be dropped. There
+     * are 24 EXT_SENS_DATA registers and hence the total read lengths between all
+     * the slaves cannot be greater than 24 or some bytes will be lost.
+     *
+     * Note: Slave 4's behavior is distinct from that of Slaves 0-3. For further
+     * information regarding the characteristics of Slave 4, please refer to
+     * Registers 49 to 53.
+     *
+     * EXAMPLE:
+     * Suppose that Slave 0 is enabled with 4 bytes to be read (I2C_SLV0_EN = 1 and
+     * I2C_SLV0_LEN = 4) while Slave 1 is enabled with 2 bytes to be read so that
+     * I2C_SLV1_EN = 1 and I2C_SLV1_LEN = 2. In such a situation, EXT_SENS_DATA _00
+     * through _03 will be associated with Slave 0, while EXT_SENS_DATA _04 and 05
+     * will be associated with Slave 1. If Slave 2 is enabled as well, registers
+     * starting from EXT_SENS_DATA_06 will be allocated to Slave 2.
+     *
+     * If Slave 2 is disabled while Slave 3 is enabled in this same situation, then
+     * registers starting from EXT_SENS_DATA_06 will be allocated to Slave 3
+     * instead.
+     *
+     * REGISTER ALLOCATION FOR DYNAMIC DISABLE VS. NORMAL DISABLE:
+     * If a slave is disabled at any time, the space initially allocated to the
+     * slave in the EXT_SENS_DATA register, will remain associated with that slave.
+     * This is to avoid dynamic adjustment of the register allocation.
+     *
+     * The allocation of the EXT_SENS_DATA registers is recomputed only when (1) all
+     * slaves are disabled, or (2) the I2C_MST_RST bit is set (Register 106).
+     *
+     * This above is also true if one of the slaves gets NACKed and stops
+     * functioning.
+     *
+    * @param position Starting position (0-23)
+        * @return Byte read from register
+        */
 	uint8_t getExternalSensorByte(int position)
 	{
 		return readByte(MPU6050_RA_EXT_SENS_DATA_00 + position);
 	}
 	/** Read word (2 bytes) from external sensor data registers.
-         * @param position Starting position (0-21)
-         * @return Word read from register
-         * @see getExternalSensorByte()
-         */
+     * @param position Starting position (0-21)
+     * @return Word read from register
+     * @see getExternalSensorByte()
+     */
 	uint16_t getExternalSensorWord(int position)
 	{
 		return readReg<uint16_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
 	}
 	/** Read double word (4 bytes) from external sensor data registers.
-         * @param position Starting position (0-20)
-         * @return Double word read from registers
-         * @see getExternalSensorByte()
-         */
+     * @param position Starting position (0-20)
+     * @return Double word read from registers
+     * @see getExternalSensorByte()
+     */
 	uint32_t getExternalSensorDWord(int position)
 	{
 		return readReg<uint32_t>(MPU6050_RA_EXT_SENS_DATA_00 + position);
@@ -2258,72 +2411,72 @@ class MPU6050
 	// MOT_DETECT_STATUS register
 
 	/** Get full motion detection status register content (all bits).
-         * @return Motion detection status byte
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         */
+     * @return Motion detection status byte
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     */
 	uint8_t getMotionStatus()
 	{
 		return readByte(MPU6050_RA_MOT_DETECT_STATUS);
 	}
 	/** Get X-axis negative motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_XNEG_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_XNEG_BIT
+     */
 	bool getXNegMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT);
 	}
 	/** Get X-axis positive motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_XPOS_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_XPOS_BIT
+     */
 	bool getXPosMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT);
 	}
 	/** Get Y-axis negative motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_YNEG_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_YNEG_BIT
+     */
 	bool getYNegMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT);
 	}
 	/** Get Y-axis positive motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_YPOS_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_YPOS_BIT
+     */
 	bool getYPosMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT);
 	}
 	/** Get Z-axis negative motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_ZNEG_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_ZNEG_BIT
+     */
 	bool getZNegMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT);
 	}
 	/** Get Z-axis positive motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_ZPOS_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_ZPOS_BIT
+     */
 	bool getZPosMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT);
 	}
 	/** Get zero motion detection interrupt status.
-         * @return Motion detection status
-         * @see MPU6050_RA_MOT_DETECT_STATUS
-         * @see MPU6050_MOTION_MOT_ZRMOT_BIT
-         */
+     * @return Motion detection status
+     * @see MPU6050_RA_MOT_DETECT_STATUS
+     * @see MPU6050_MOTION_MOT_ZRMOT_BIT
+     */
 	bool getZeroMotionDetected()
 	{
 		return readBit(MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT);
@@ -2332,66 +2485,66 @@ class MPU6050
 	// I2C_SLV*_DO register
 
 	/** Write byte to Data Output container for specified slave.
-         * This register holds the output data written into Slave when Slave is set to
-         * write mode. For further information regarding Slave control, please
-         * refer to Registers 37 to 39 and immediately following.
-         * @param num Slave number (0-3)
-         * @param data Byte to write
-         * @see MPU6050_RA_I2C_SLV0_DO
-         */
+     * This register holds the output data written into Slave when Slave is set to
+     * write mode. For further information regarding Slave control, please
+     * refer to Registers 37 to 39 and immediately following.
+     * @param num Slave number (0-3)
+     * @param data Byte to write
+     * @see MPU6050_RA_I2C_SLV0_DO
+     */
 	void setSlaveOutputByte(uint8_t num, uint8_t data);
 
 	// I2C_MST_DELAY_CTRL register
 	/** Get external data shadow delay enabled status.
-         * This register is used to specify the timing of external sensor data
-         * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
-         * sensor data is delayed until all data has been received.
-         * @return Current external data shadow delay enabled status.
-         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
-         * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
-         */
+     * This register is used to specify the timing of external sensor data
+     * shadowing. When DELAY_ES_SHADOW is set to 1, shadowing of external
+     * sensor data is delayed until all data has been received.
+     * @return Current external data shadow delay enabled status.
+     * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+     * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+     */
 	bool getExternalShadowDelayEnabled()
 	{
 		return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT);
 	}
 	/** Set external data shadow delay enabled status.
-         * @param enabled New external data shadow delay enabled status.
-         * @see getExternalShadowDelayEnabled()
-         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
-         * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
-         */
+     * @param enabled New external data shadow delay enabled status.
+     * @see getExternalShadowDelayEnabled()
+     * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+     * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT
+     */
 	void setExternalShadowDelayEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, 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
-         * slave device is reduced. When a slave's access rate is decreased relative to
-         * the Sample Rate, the slave is accessed every:
-         *
-         *     1 / (1 + I2C_MST_DLY) Samples
-         *
-         * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
-         * and DLPF_CFG (register 26).
-         *
-         * For further information regarding I2C_MST_DLY, please refer to register 52.
-         * For further information regarding the Sample Rate, please refer to
-         * register 25.
-         *
-         * @param num Slave number (0-4)
-         * @return Current slave delay enabled status.
-         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
-         * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
-         */
+     * When a particular slave delay is enabled, the rate of access for the that
+     * slave device is reduced. When a slave's access rate is decreased relative to
+     * the Sample Rate, the slave is accessed every:
+     *
+     *     1 / (1 + I2C_MST_DLY) Samples
+     *
+     * This base Sample Rate in turn is determined by SMPLRT_DIV (register  * 25)
+     * and DLPF_CFG (register 26).
+     *
+     * For further information regarding I2C_MST_DLY, please refer to register 52.
+     * For further information regarding the Sample Rate, please refer to
+     * register 25.
+     *
+     * @param num Slave number (0-4)
+     * @return Current slave delay enabled status.
+     * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+     * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+     */
 	bool getSlaveDelayEnabled(uint8_t num);
 
 	/** Set slave delay enabled status.
-         * @param num Slave number (0-4)
-         * @param enabled New slave delay enabled status.
-         * @see MPU6050_RA_I2C_MST_DELAY_CTRL
-         * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
-         */
+     * @param num Slave number (0-4)
+     * @param enabled New slave delay enabled status.
+     * @see MPU6050_RA_I2C_MST_DELAY_CTRL
+     * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+     */
 	void setSlaveDelayEnabled(uint8_t num, bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
@@ -2400,31 +2553,31 @@ class MPU6050
 	// SIGNAL_PATH_RESET register
 
 	/** Reset gyroscope signal path.
-         * The reset will revert the signal path analog to digital converters and
-         * filters to their power up configurations.
-         * @see MPU6050_RA_SIGNAL_PATH_RESET
-         * @see MPU6050_PATHRESET_GYRO_RESET_BIT
-         */
+     * The reset will revert the signal path analog to digital converters and
+     * filters to their power up configurations.
+     * @see MPU6050_RA_SIGNAL_PATH_RESET
+     * @see MPU6050_PATHRESET_GYRO_RESET_BIT
+     */
 	void resetGyroscopePath()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true);
 	}
 	/** Reset accelerometer signal path.
-         * The reset will revert the signal path analog to digital converters and
-         * filters to their power up configurations.
-         * @see MPU6050_RA_SIGNAL_PATH_RESET
-         * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
-         */
+     * The reset will revert the signal path analog to digital converters and
+     * filters to their power up configurations.
+     * @see MPU6050_RA_SIGNAL_PATH_RESET
+     * @see MPU6050_PATHRESET_ACCEL_RESET_BIT
+     */
 	void resetAccelerometerPath()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true);
 	}
 	/** Reset temperature sensor signal path.
-         * The reset will revert the signal path analog to digital converters and
-         * filters to their power up configurations.
-         * @see MPU6050_RA_SIGNAL_PATH_RESET
-         * @see MPU6050_PATHRESET_TEMP_RESET_BIT
-         */
+     * The reset will revert the signal path analog to digital converters and
+     * filters to their power up configurations.
+     * @see MPU6050_RA_SIGNAL_PATH_RESET
+     * @see MPU6050_PATHRESET_TEMP_RESET_BIT
+     */
 	void resetTemperaturePath()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true);
@@ -2433,109 +2586,109 @@ class MPU6050
 	// MOT_DETECT_CTRL register
 
 	/** Get accelerometer power-on delay.
-         * The accelerometer data path provides samples to the sensor registers, Motion
-         * detection, Zero Motion detection, and Free Fall detection modules. The
-         * signal path contains filters which must be flushed on wake-up with new
-         * samples before the detection modules begin operations. The default wake-up
-         * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
-         * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
-         * any value above zero unless instructed otherwise by InvenSense. Please refer
-         * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
-         * further information regarding the detection modules.
-         * @return Current accelerometer power-on delay
-         * @see MPU6050_RA_MOT_DETECT_CTRL
-         * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
-         */
+     * The accelerometer data path provides samples to the sensor registers, Motion
+     * detection, Zero Motion detection, and Free Fall detection modules. The
+     * signal path contains filters which must be flushed on wake-up with new
+     * samples before the detection modules begin operations. The default wake-up
+     * delay, of 4ms can be lengthened by up to 3ms. This additional delay is
+     * specified in ACCEL_ON_DELAY in units of 1 LSB = 1 ms. The user may select
+     * any value above zero unless instructed otherwise by InvenSense. Please refer
+     * to Section 8 of the MPU-6000/MPU-6050 Product Specification document for
+     * further information regarding the detection modules.
+     * @return Current accelerometer power-on delay
+     * @see MPU6050_RA_MOT_DETECT_CTRL
+     * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+     */
 	uint8_t getAccelerometerPowerOnDelay()
 	{
 		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
 						MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH);
 	}
 	/** Set accelerometer power-on delay.
-         * @param delay New accelerometer power-on delay (0-3)
-         * @see getAccelerometerPowerOnDelay()
-         * @see MPU6050_RA_MOT_DETECT_CTRL
-         * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
-         */
+     * @param delay New accelerometer power-on delay (0-3)
+     * @see getAccelerometerPowerOnDelay()
+     * @see MPU6050_RA_MOT_DETECT_CTRL
+     * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT
+     */
 	void setAccelerometerPowerOnDelay(uint8_t delay)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
 						  MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
 	}
 	/** Get Free Fall detection counter decrement configuration.
-         * Detection is registered by the Free Fall detection module after accelerometer
-         * measurements meet their respective threshold conditions over a specified
-         * number of samples. When the threshold conditions are met, the corresponding
-         * detection counter increments by 1. The user may control the rate at which the
-         * detection counter decrements when the threshold condition is not met by
-         * configuring FF_COUNT. The decrement rate can be set according to the
-         * following table:
-         *
-         * <pre>
-         * FF_COUNT | Counter Decrement
-         * ---------+------------------
-         * 0        | Reset
-         * 1        | 1
-         * 2        | 2
-         * 3        | 4
-         * </pre>
-         *
-         * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
-         * reset the counter to 0. For further information on Free Fall detection,
-         * please refer to Registers 29 to 32.
-         *
-         * @return Current decrement configuration
-         * @see MPU6050_RA_MOT_DETECT_CTRL
-         * @see MPU6050_DETECT_FF_COUNT_BIT
-         */
+     * Detection is registered by the Free Fall detection module after accelerometer
+     * measurements meet their respective threshold conditions over a specified
+     * number of samples. When the threshold conditions are met, the corresponding
+     * detection counter increments by 1. The user may control the rate at which the
+     * detection counter decrements when the threshold condition is not met by
+     * configuring FF_COUNT. The decrement rate can be set according to the
+     * following table:
+     *
+     * <pre>
+     * FF_COUNT | Counter Decrement
+     * ---------+------------------
+     * 0        | Reset
+     * 1        | 1
+     * 2        | 2
+     * 3        | 4
+     * </pre>
+     *
+     * When FF_COUNT is configured to 0 (reset), any non-qualifying sample will
+     * reset the counter to 0. For further information on Free Fall detection,
+     * please refer to Registers 29 to 32.
+     *
+     * @return Current decrement configuration
+     * @see MPU6050_RA_MOT_DETECT_CTRL
+     * @see MPU6050_DETECT_FF_COUNT_BIT
+     */
 	uint8_t getFreefallDetectionCounterDecrement()
 	{
 		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH);
 	}
 	/** Set Free Fall detection counter decrement configuration.
-         * @param decrement New decrement configuration value
-         * @see getFreefallDetectionCounterDecrement()
-         * @see MPU6050_RA_MOT_DETECT_CTRL
-         * @see MPU6050_DETECT_FF_COUNT_BIT
-         */
+     * @param decrement New decrement configuration value
+     * @see getFreefallDetectionCounterDecrement()
+     * @see MPU6050_RA_MOT_DETECT_CTRL
+     * @see MPU6050_DETECT_FF_COUNT_BIT
+     */
 	void setFreefallDetectionCounterDecrement(uint8_t decrement)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT,
 						  MPU6050_DETECT_FF_COUNT_LENGTH, decrement);
 	}
 	/** Get Motion detection counter decrement configuration.
-         * Detection is registered by the Motion detection module after accelerometer
-         * measurements meet their respective threshold conditions over a specified
-         * number of samples. When the threshold conditions are met, the corresponding
-         * detection counter increments by 1. The user may control the rate at which the
-         * detection counter decrements when the threshold condition is not met by
-         * configuring MOT_COUNT. The decrement rate can be set according to the
-         * following table:
-         *
-         * <pre>
-         * MOT_COUNT | Counter Decrement
-         * ----------+------------------
-         * 0         | Reset
-         * 1         | 1
-         * 2         | 2
-         * 3         | 4
-         * </pre>
-         *
-         * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
-         * reset the counter to 0. For further information on Motion detection,
-         * please refer to Registers 29 to 32.
-         *
-         */
+     * Detection is registered by the Motion detection module after accelerometer
+     * measurements meet their respective threshold conditions over a specified
+     * number of samples. When the threshold conditions are met, the corresponding
+     * detection counter increments by 1. The user may control the rate at which the
+     * detection counter decrements when the threshold condition is not met by
+     * configuring MOT_COUNT. The decrement rate can be set according to the
+     * following table:
+     *
+     * <pre>
+     * MOT_COUNT | Counter Decrement
+     * ----------+------------------
+     * 0         | Reset
+     * 1         | 1
+     * 2         | 2
+     * 3         | 4
+     * </pre>
+     *
+     * When MOT_COUNT is configured to 0 (reset), any non-qualifying sample will
+     * reset the counter to 0. For further information on Motion detection,
+     * please refer to Registers 29 to 32.
+     *
+     */
 	uint8_t getMotionDetectionCounterDecrement()
 	{
 		return readBits(MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH);
 	}
 	/** Set Motion detection counter decrement configuration.
-         * @param decrement New decrement configuration value
-         * @see getMotionDetectionCounterDecrement()
-         * @see MPU6050_RA_MOT_DETECT_CTRL
-         * @see MPU6050_DETECT_MOT_COUNT_BIT
-         */
+     * @param decrement New decrement configuration value
+     * @see getMotionDetectionCounterDecrement()
+     * @see MPU6050_RA_MOT_DETECT_CTRL
+     * @see MPU6050_DETECT_MOT_COUNT_BIT
+     */
 	void setMotionDetectionCounterDecrement(uint8_t decrement)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT,
@@ -2545,92 +2698,92 @@ class MPU6050
 	// USER_CTRL register
 
 	/** Get FIFO enabled status.
-         * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
-         * cannot be written to or read from while disabled. The FIFO buffer's state
-         * does not change unless the MPU-60X0 is power cycled.
-         * @return Current FIFO enabled status
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_FIFO_EN_BIT
-         */
+     * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
+     * cannot be written to or read from while disabled. The FIFO buffer's state
+     * does not change unless the MPU-60X0 is power cycled.
+     * @return Current FIFO enabled status
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_FIFO_EN_BIT
+     */
 	bool getFIFOEnabled()
 	{
 		return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT);
 	}
 	/** Set FIFO enabled status.
-         * @param enabled New FIFO enabled status
-         * @see getFIFOEnabled()
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_FIFO_EN_BIT
-         */
+     * @param enabled New FIFO enabled status
+     * @see getFIFOEnabled()
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_FIFO_EN_BIT
+     */
 	void setFIFOEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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
-         * external sensor slave devices on the auxiliary I2C bus. When this bit is
-         * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
-         * driven by the primary I2C bus (SDA and SCL). This is a precondition to
-         * enabling Bypass Mode. For further information regarding Bypass Mode, please
-         * refer to Register 55.
-         * @return Current I2C Master Mode enabled status
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
-         */
+     * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the
+     * external sensor slave devices on the auxiliary I2C bus. When this bit is
+     * cleared to 0, the auxiliary I2C bus lines (AUX_DA and AUX_CL) are logically
+     * driven by the primary I2C bus (SDA and SCL). This is a precondition to
+     * enabling Bypass Mode. For further information regarding Bypass Mode, please
+     * refer to Register 55.
+     * @return Current I2C Master Mode enabled status
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+     */
 	bool getI2CMasterModeEnabled()
 	{
 		return readBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT);
 	}
 	/** Set I2C Master Mode enabled status.
-         * @param enabled New I2C Master Mode enabled status
-         * @see getI2CMasterModeEnabled()
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
-         */
+     * @param enabled New I2C Master Mode enabled status
+     * @see getI2CMasterModeEnabled()
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_I2C_MST_EN_BIT
+     */
 	void setI2CMasterModeEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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.
-         */
+     * If this is set, the primary SPI interface will be enabled in place of the
+     * disabled primary I2C interface.
+     */
 	void switchSPIEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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
-         * bit automatically clears to 0 after the reset has been triggered.
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_FIFO_RESET_BIT
-         */
+     * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
+     * bit automatically clears to 0 after the reset has been triggered.
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_FIFO_RESET_BIT
+     */
 	void resetFIFO()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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.
-         * This bit automatically clears to 0 after the reset has been triggered.
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
-         */
+     * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0.
+     * This bit automatically clears to 0 after the reset has been triggered.
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT
+     */
 	void resetI2CMaster()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, 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,
-         * accelerometers, and temperature sensor). This operation will also clear the
-         * sensor registers. This bit automatically clears to 0 after the reset has been
-         * triggered.
-         *
-         * When resetting only the signal path (and not the sensor registers), please
-         * use Register 104, SIGNAL_PATH_RESET.
-         *
-         * @see MPU6050_RA_USER_CTRL
-         * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
-         */
+     * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
+     * accelerometers, and temperature sensor). This operation will also clear the
+     * sensor registers. This bit automatically clears to 0 after the reset has been
+     * triggered.
+     *
+     * When resetting only the signal path (and not the sensor registers), please
+     * use Register 104, SIGNAL_PATH_RESET.
+     *
+     * @see MPU6050_RA_USER_CTRL
+     * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT
+     */
 	void resetSensors()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true);
@@ -2639,132 +2792,132 @@ class MPU6050
 	// PWR_MGMT_1 register
 
 	/** Trigger a full device reset.
-         * A small delay of ~50ms may be desirable after triggering a reset.
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_DEVICE_RESET_BIT
-         */
+     * A small delay of ~50ms may be desirable after triggering a reset.
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_DEVICE_RESET_BIT
+     */
 	void reset()
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true);
 	}
 	/** Get sleep mode status.
-         * Setting the SLEEP bit in the register puts the device into very low power
-         * sleep mode. In this mode, only the serial interface and internal registers
-         * remain active, allowing for a very low standby current. Clearing this bit
-         * puts the device back into normal mode. To save power, the individual standby
-         * selections for each of the gyros should be used if any gyro axis is not used
-         * by the application.
-         * @return Current sleep mode enabled status
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_SLEEP_BIT
-         */
+     * Setting the SLEEP bit in the register puts the device into very low power
+     * sleep mode. In this mode, only the serial interface and internal registers
+     * remain active, allowing for a very low standby current. Clearing this bit
+     * puts the device back into normal mode. To save power, the individual standby
+     * selections for each of the gyros should be used if any gyro axis is not used
+     * by the application.
+     * @return Current sleep mode enabled status
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_SLEEP_BIT
+     */
 	bool getSleepEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT);
 	}
 	/** Set sleep mode status.
-         * @param enabled New sleep mode enabled status
-         * @see getSleepEnabled()
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_SLEEP_BIT
-         */
+     * @param enabled New sleep mode enabled status
+     * @see getSleepEnabled()
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_SLEEP_BIT
+     */
 	void setSleepEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, 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
-         * between sleep mode and waking up to take a single sample of data from active
-         * sensors at a rate determined by LP_WAKE_CTRL (register 108).
-         * @return Current sleep mode enabled status
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_CYCLE_BIT
-         */
+     * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
+     * between sleep mode and waking up to take a single sample of data from active
+     * sensors at a rate determined by LP_WAKE_CTRL (register 108).
+     * @return Current sleep mode enabled status
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_CYCLE_BIT
+     */
 	bool getWakeCycleEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT);
 	}
 	/** Set wake cycle enabled status.
-         * @param enabled New sleep mode enabled status
-         * @see getWakeCycleEnabled()
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_CYCLE_BIT
-         */
+     * @param enabled New sleep mode enabled status
+     * @see getWakeCycleEnabled()
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_CYCLE_BIT
+     */
 	void setWakeCycleEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled);
 	}
 	/** Get temperature sensor enabled status.
-         * Control the usage of the internal temperature sensor.
-         *
-         * Note: this register stores the *disabled* value, but for consistency with the
-         * rest of the code, the function is named and used with standard true/false
-         * values to indicate whether the sensor is enabled or disabled, respectively.
-         *
-         * @return Current temperature sensor enabled status
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_TEMP_DIS_BIT
-         */
+     * Control the usage of the internal temperature sensor.
+     *
+     * Note: this register stores the *disabled* value, but for consistency with the
+     * rest of the code, the function is named and used with standard true/false
+     * values to indicate whether the sensor is enabled or disabled, respectively.
+     *
+     * @return Current temperature sensor enabled status
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_TEMP_DIS_BIT
+     */
 	bool getTempSensorEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT); // 1 is actually disabled here
 	}
 	/** Set temperature sensor enabled status.
-         * Note: this register stores the *disabled* value, but for consistency with the
-         * rest of the code, the function is named and used with standard true/false
-         * values to indicate whether the sensor is enabled or disabled, respectively.
-         *
-         * @param enabled New temperature sensor enabled status
-         * @see getTempSensorEnabled()
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_TEMP_DIS_BIT
-         */
+     * Note: this register stores the *disabled* value, but for consistency with the
+     * rest of the code, the function is named and used with standard true/false
+     * values to indicate whether the sensor is enabled or disabled, respectively.
+     *
+     * @param enabled New temperature sensor enabled status
+     * @see getTempSensorEnabled()
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_TEMP_DIS_BIT
+     */
 	void setTempSensorEnabled(bool enabled)
 	{
 		// 1 is actually disabled here
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled);
 	}
 	/** Get clock source setting.
-         * @return Current clock source setting
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_CLKSEL_BIT
-         * @see MPU6050_PWR1_CLKSEL_LENGTH
-         */
+     * @return Current clock source setting
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_CLKSEL_BIT
+     * @see MPU6050_PWR1_CLKSEL_LENGTH
+     */
 	uint8_t getClockSource()
 	{
 		return readBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH);
 	}
 	/** Set clock source setting.
-         * An internal 8MHz oscillator, gyroscope based clock, or external sources can
-         * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
-         * or an external source is chosen as the clock source, the MPU-60X0 can operate
-         * in low power modes with the gyroscopes disabled.
-         *
-         * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
-         * However, it is highly recommended that the device be configured to use one of
-         * the gyroscopes (or an external clock source) as the clock reference for
-         * improved stability. The clock source can be selected according to the
-         * following table:
-         *
-         * <pre>
-         * CLK_SEL | Clock Source
-         * --------+--------------------------------------
-         * 0       | Internal oscillator
-         * 1       | PLL with X Gyro reference
-         * 2       | PLL with Y Gyro reference
-         * 3       | PLL with Z Gyro reference
-         * 4       | PLL with external 32.768kHz reference
-         * 5       | PLL with external 19.2MHz reference
-         * 6       | Reserved
-         * 7       | Stops the clock and keeps the timing generator in reset
-         * </pre>
-         *
-         * @param source New clock source setting
-         * @see getClockSource()
-         * @see MPU6050_RA_PWR_MGMT_1
-         * @see MPU6050_PWR1_CLKSEL_BIT
-         * @see MPU6050_PWR1_CLKSEL_LENGTH
-         */
+     * An internal 8MHz oscillator, gyroscope based clock, or external sources can
+     * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
+     * or an external source is chosen as the clock source, the MPU-60X0 can operate
+     * in low power modes with the gyroscopes disabled.
+     *
+     * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
+     * However, it is highly recommended that the device be configured to use one of
+     * the gyroscopes (or an external clock source) as the clock reference for
+     * improved stability. The clock source can be selected according to the
+     * following table:
+     *
+     * <pre>
+     * CLK_SEL | Clock Source
+     * --------+--------------------------------------
+     * 0       | Internal oscillator
+     * 1       | PLL with X Gyro reference
+     * 2       | PLL with Y Gyro reference
+     * 3       | PLL with Z Gyro reference
+     * 4       | PLL with external 32.768kHz reference
+     * 5       | PLL with external 19.2MHz reference
+     * 6       | Reserved
+     * 7       | Stops the clock and keeps the timing generator in reset
+     * </pre>
+     *
+     * @param source New clock source setting
+     * @see getClockSource()
+     * @see MPU6050_RA_PWR_MGMT_1
+     * @see MPU6050_PWR1_CLKSEL_BIT
+     * @see MPU6050_PWR1_CLKSEL_LENGTH
+     */
 	void setClockSource(uint8_t source)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
@@ -2773,36 +2926,36 @@ class MPU6050
 	// PWR_MGMT_2 register
 
 	/** Get wake frequency in Accel-Only Low Power Mode.
-         * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
-         * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
-         * the device will power off all devices except for the primary I2C interface,
-         * waking only the accelerometer at fixed intervals to take a single
-         * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
-         * as shown below:
-         *
-         * <pre>
-         * LP_WAKE_CTRL | Wake-up Frequency
-         * -------------+------------------
-         * 0            | 1.25 Hz
-         * 1            | 2.5 Hz
-         * 2            | 5 Hz
-         * 3            | 10 Hz
-         * </pre>
-         *
-         * For further information regarding the MPU-60X0's power modes, please refer to
-         * Register 107.
-         *
-         * @return Current wake frequency
-         * @see MPU6050_RA_PWR_MGMT_2
-         */
+     * The MPU-60X0 can be put into Accerlerometer Only Low Power Mode by setting
+     * PWRSEL to 1 in the Power Management 1 register (Register 107). In this mode,
+     * the device will power off all devices except for the primary I2C interface,
+     * waking only the accelerometer at fixed intervals to take a single
+     * measurement. The frequency of wake-ups can be configured with LP_WAKE_CTRL
+     * as shown below:
+     *
+     * <pre>
+     * LP_WAKE_CTRL | Wake-up Frequency
+     * -------------+------------------
+     * 0            | 1.25 Hz
+     * 1            | 2.5 Hz
+     * 2            | 5 Hz
+     * 3            | 10 Hz
+     * </pre>
+     *
+     * For further information regarding the MPU-60X0's power modes, please refer to
+     * Register 107.
+     *
+     * @return Current wake frequency
+     * @see MPU6050_RA_PWR_MGMT_2
+     */
 	uint8_t getWakeFrequency()
 	{
 		return readBits(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH);
 	}
 	/** Set wake frequency in Accel-Only Low Power Mode.
-         * @param frequency New wake frequency
-         * @see MPU6050_RA_PWR_MGMT_2
-         */
+     * @param frequency New wake frequency
+     * @see MPU6050_RA_PWR_MGMT_2
+     */
 	void setWakeFrequency(uint8_t frequency)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT,
@@ -2810,121 +2963,121 @@ class MPU6050
 	}
 
 	/** Get X-axis accelerometer standby enabled status.
-         * If enabled, the X-axis will not gather or report data (or use power).
-         * @return Current X-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_XA_BIT
-         */
+     * If enabled, the X-axis will not gather or report data (or use power).
+     * @return Current X-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_XA_BIT
+     */
 	bool getStandbyXAccelEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT);
 	}
 	/** Set X-axis accelerometer standby enabled status.
-         * @param New X-axis standby enabled status
-         * @see getStandbyXAccelEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_XA_BIT
-         */
+     * @param New X-axis standby enabled status
+     * @see getStandbyXAccelEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_XA_BIT
+     */
 	void setStandbyXAccelEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
-         * @return Current Y-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_YA_BIT
-         */
+     * If enabled, the Y-axis will not gather or report data (or use power).
+     * @return Current Y-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_YA_BIT
+     */
 	bool getStandbyYAccelEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT);
 	}
 	/** Set Y-axis accelerometer standby enabled status.
-         * @param New Y-axis standby enabled status
-         * @see getStandbyYAccelEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_YA_BIT
-         */
+     * @param New Y-axis standby enabled status
+     * @see getStandbyYAccelEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_YA_BIT
+     */
 	void setStandbyYAccelEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
-         * @return Current Z-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_ZA_BIT
-         */
+     * If enabled, the Z-axis will not gather or report data (or use power).
+     * @return Current Z-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_ZA_BIT
+     */
 	bool getStandbyZAccelEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT);
 	}
 	/** Set Z-axis accelerometer standby enabled status.
-         * @param New Z-axis standby enabled status
-         * @see getStandbyZAccelEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_ZA_BIT
-         */
+     * @param New Z-axis standby enabled status
+     * @see getStandbyZAccelEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_ZA_BIT
+     */
 	void setStandbyZAccelEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
-         * @return Current X-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_XG_BIT
-         */
+     * If enabled, the X-axis will not gather or report data (or use power).
+     * @return Current X-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_XG_BIT
+     */
 	bool getStandbyXGyroEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT);
 	}
 	/** Set X-axis gyroscope standby enabled status.
-         * @param New X-axis standby enabled status
-         * @see getStandbyXGyroEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_XG_BIT
-         */
+     * @param New X-axis standby enabled status
+     * @see getStandbyXGyroEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_XG_BIT
+     */
 	void setStandbyXGyroEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
-         * @return Current Y-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_YG_BIT
-         */
+     * If enabled, the Y-axis will not gather or report data (or use power).
+     * @return Current Y-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_YG_BIT
+     */
 	bool getStandbyYGyroEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT);
 	}
 	/** Set Y-axis gyroscope standby enabled status.
-         * @param New Y-axis standby enabled status
-         * @see getStandbyYGyroEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_YG_BIT
-         */
+     * @param New Y-axis standby enabled status
+     * @see getStandbyYGyroEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_YG_BIT
+     */
 	void setStandbyYGyroEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, 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).
-         * @return Current Z-axis standby enabled status
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_ZG_BIT
-         */
+     * If enabled, the Z-axis will not gather or report data (or use power).
+     * @return Current Z-axis standby enabled status
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_ZG_BIT
+     */
 	bool getStandbyZGyroEnabled()
 	{
 		return readBit(MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT);
 	}
 	/** Set Z-axis gyroscope standby enabled status.
-         * @param New Z-axis standby enabled status
-         * @see getStandbyZGyroEnabled()
-         * @see MPU6050_RA_PWR_MGMT_2
-         * @see MPU6050_PWR2_STBY_ZG_BIT
-         */
+     * @param New Z-axis standby enabled status
+     * @see getStandbyZGyroEnabled()
+     * @see MPU6050_RA_PWR_MGMT_2
+     * @see MPU6050_PWR2_STBY_ZG_BIT
+     */
 	void setStandbyZGyroEnabled(bool enabled)
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
@@ -2933,12 +3086,12 @@ class MPU6050
 	// FIFO_COUNT* registers
 
 	/** Get current FIFO buffer size.
-         * This value indicates the number of bytes stored in the FIFO buffer. This
-         * number is in turn the number of bytes that can be read from the FIFO buffer
-         * and it is directly proportional to the number of samples available given the
-         * set of sensor data bound to be stored in the FIFO (register 35 and 36).
-         * @return Current FIFO buffer size
-         */
+     * This value indicates the number of bytes stored in the FIFO buffer. This
+     * number is in turn the number of bytes that can be read from the FIFO buffer
+     * and it is directly proportional to the number of samples available given the
+     * set of sensor data bound to be stored in the FIFO (register 35 and 36).
+     * @return Current FIFO buffer size
+     */
 	uint16_t getFIFOCount()
 	{
 		uint8_t buffer[2] = {0};
@@ -2949,39 +3102,39 @@ class MPU6050
 	// FIFO_R_W register
 
 	/** Get byte from FIFO buffer.
-         * This register is used to read and write data from the FIFO buffer. Data is
-         * written to the FIFO in order of register number (from lowest to highest). If
-         * all the FIFO enable flags (see below) are enabled and all External Sensor
-         * Data registers (Registers 73 to 96) are associated with a Slave device, the
-         * contents of registers 59 through 96 will be written in order at the Sample
-         * Rate.
-         *
-         * The contents of the sensor data registers (Registers 59 to 96) are written
-         * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
-         * in FIFO_EN (Register 35). An additional flag for the sensor data registers
-         * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
-         *
-         * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
-         * automatically set to 1. This bit is located in INT_STATUS (Register 58).
-         * When the FIFO buffer has overflowed, the oldest data will be lost and new
-         * data will be written to the FIFO.
-         *
-         * If the FIFO buffer is empty, reading this register will return the last byte
-         * that was previously read from the FIFO until new data is available. The user
-         * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
-         * empty.
-         *
-         * @return Byte from FIFO buffer
-         */
+     * This register is used to read and write data from the FIFO buffer. Data is
+     * written to the FIFO in order of register number (from lowest to highest). If
+     * all the FIFO enable flags (see below) are enabled and all External Sensor
+     * Data registers (Registers 73 to 96) are associated with a Slave device, the
+     * contents of registers 59 through 96 will be written in order at the Sample
+     * Rate.
+     *
+     * The contents of the sensor data registers (Registers 59 to 96) are written
+     * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
+     * in FIFO_EN (Register 35). An additional flag for the sensor data registers
+     * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
+     *
+     * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
+     * automatically set to 1. This bit is located in INT_STATUS (Register 58).
+     * When the FIFO buffer has overflowed, the oldest data will be lost and new
+     * data will be written to the FIFO.
+     *
+     * If the FIFO buffer is empty, reading this register will return the last byte
+     * that was previously read from the FIFO until new data is available. The user
+     * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
+     * empty.
+     *
+     * @return Byte from FIFO buffer
+     */
 	uint8_t getFIFOByte()
 	{
 		return readByte(MPU6050_RA_FIFO_R_W);
 	}
 
 	/** Write byte to FIFO buffer.
-         * @see getFIFOByte()
-         * @see MPU6050_RA_FIFO_R_W
-         */
+     * @see getFIFOByte()
+     * @see MPU6050_RA_FIFO_R_W
+     */
 	void setFIFOByte(uint8_t data)
 	{
 		I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
@@ -2992,25 +3145,25 @@ class MPU6050
 	// WHO_AM_I register
 
 	/** Get Device ID.
-         * This register is used to verify the identity of the device (0b110100, 0x34).
-         * @return Device ID (6 bits only! should be 0x34)
-         * @see MPU6050_RA_WHO_AM_I
-         * @see MPU6050_WHO_AM_I_BIT
-         * @see MPU6050_WHO_AM_I_LENGTH
-         */
+     * This register is used to verify the identity of the device (0b110100, 0x34).
+     * @return Device ID (6 bits only! should be 0x34)
+     * @see MPU6050_RA_WHO_AM_I
+     * @see MPU6050_WHO_AM_I_BIT
+     * @see MPU6050_WHO_AM_I_LENGTH
+     */
 	uint8_t getDeviceID()
 	{
 		return readBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH);
 	}
 	/** Set Device ID.
-         * Write a new ID into the WHO_AM_I register (no idea why this should ever be
-         * necessary though).
-         * @param id New device ID to set.
-         * @see getDeviceID()
-         * @see MPU6050_RA_WHO_AM_I
-         * @see MPU6050_WHO_AM_I_BIT
-         * @see MPU6050_WHO_AM_I_LENGTH
-         */
+     * Write a new ID into the WHO_AM_I register (no idea why this should ever be
+     * necessary though).
+     * @param id New device ID to set.
+     * @see getDeviceID()
+     * @see MPU6050_RA_WHO_AM_I
+     * @see MPU6050_WHO_AM_I_BIT
+     * @see MPU6050_WHO_AM_I_LENGTH
+     */
 	void setDeviceID(uint8_t id)
 	{
 		I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id);
@@ -3213,30 +3366,28 @@ class MPU6050
 	{
 		I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
 	}
+
 	// BANK_SEL register
 	void setMemoryBank(uint8_t bank, bool prefetchEnabled = false, bool userBank = false);
 
 	// MEM_START_ADDR register
-	void setMemoryStartAddress(uint8_t address);
+	void setMemoryStartAddress(uint8_t address)
+	{
+		I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
+	}
 
 	// MEM_R_W register
-	uint8_t readMemoryByte();
-	void writeMemoryByte(uint8_t data);
-	void readMemoryBlock(uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0);
-	bool writeMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0,
-						  bool verify = true, bool useProgMem = false);
-	bool writeProgMemoryBlock(const uint8_t* data, uint16_t dataSize, uint8_t bank = 0, uint8_t address = 0,
-							  bool verify = true);
-
-	bool writeDMPConfigurationSet(const uint8_t* data, uint16_t dataSize, bool useProgMem = false);
+	uint8_t readMemoryByte()
+	{
+		return readByte(MPU6050_RA_MEM_R_W);
+	}
 
-	bool writeProgDMPConfigurationSet(const uint8_t* data, uint16_t dataSize)
+	void writeMemoryByte(uint8_t data)
 	{
-		return writeDMPConfigurationSet(data, dataSize, true);
+		I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
 	}
 
 	// DMP_CFG_1 register
-
 	uint8_t getDMPConfig1()
 	{
 		return readByte(MPU6050_RA_DMP_CFG_1);
@@ -3258,10 +3409,6 @@ class MPU6050
 		I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
 	}
 
-	void CalibrateGyro(uint8_t Loops = 15);	 // Fine tune after setting offsets with less Loops.
-	void CalibrateAccel(uint8_t Loops = 15); // Fine tune after setting offsets with less Loops.
-	void PID(uint8_t ReadAddress, float kP, float kI, uint8_t Loops); // Does the
-
 private:
 	// I2C helpers
 	uint8_t readBit(uint8_t regAddr, uint8_t bitNum);

From 5fd6c614a3e2be97f40a8cc9df0a5921d17bbd2f Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Sat, 4 Nov 2023 10:53:27 +0100
Subject: [PATCH 17/21] Update readme

---
 Sming/Libraries/MPU6050/MPU6050.h  | 8 ++------
 Sming/Libraries/MPU6050/README.rst | 1 +
 2 files changed, 3 insertions(+), 6 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index 571fb2d4b8..a775395ae8 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -1,10 +1,6 @@
-
 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011
-// (RM-MPU-6000A-00) 10/3/2011 by Jeff Rowberg <jeff@rowberg.net> Updates should
-// (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-//     ... - ongoing debug release
+// (RM-MPU-6000A-00)
+// Based On https://github.com/jrowberg/i2cdevlib
 
 // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
 // UNDERGOING ACTIVE DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES.
diff --git a/Sming/Libraries/MPU6050/README.rst b/Sming/Libraries/MPU6050/README.rst
index cd6dd4bb30..ee201c2f9a 100644
--- a/Sming/Libraries/MPU6050/README.rst
+++ b/Sming/Libraries/MPU6050/README.rst
@@ -11,3 +11,4 @@ Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tr
 -   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.
 -   Removed map function in favor of the Sming built-in one.
 -   Adapted include path, coding style and applied clangformat
+-   Deleted Calibration and Memory Block related code for code quality reason

From 0327384be6d1063d893066f895c45788da5f6b3f Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Tue, 7 Nov 2023 15:06:27 +0100
Subject: [PATCH 18/21] Refactor to use ASSERT_SLAVE_ID_VALID

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 154 ++++++++++++----------------
 Sming/Libraries/MPU6050/MPU6050.h   |  77 +++++++-------
 2 files changed, 105 insertions(+), 126 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 2ae84f87b9..6174f909e8 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -37,10 +37,19 @@ THE SOFTWARE.
 
 #include "MPU6050.h"
 #include <string.h>
+#include <cassert>
 
 #define I2C_NUM I2C_NUM_0
 using detail::concat;
 
+namespace
+{
+//Slave 4’s characteristics differ greatly from those of Slaves 0-3.
+//Hence our API support only up to slave 3
+constexpr uint8_t MAX_SLAVE_ID{3};
+#define ASSERT_SLAVE_ID_VALID(slaveId) assert((slaveId <= MAX_SLAVE_ID))
+} // namespace
+
 size_t MPU6050::Motion3::printTo(Print& p) const
 {
 	size_t n{0};
@@ -109,117 +118,95 @@ uint8_t MPU6050::getGyroZSelfTestFactoryTrim()
 	return (z & 0x1F);
 }
 
-uint8_t MPU6050::getSlaveAddress(uint8_t num)
+uint8_t MPU6050::getSlaveAddress(SlaveId slaveId)
 {
-	if(num > 3) {
-		return 0;
-	}
-	return readByte(MPU6050_RA_I2C_SLV0_ADDR + num * 3);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	return readByte(MPU6050_RA_I2C_SLV0_ADDR + slaveId * 3);
 }
 
-void MPU6050::setSlaveAddress(uint8_t num, uint8_t address)
+void MPU6050::setSlaveAddress(SlaveId slaveId, uint8_t address)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3, address);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + slaveId * 3, address);
 }
 
-uint8_t MPU6050::getSlaveRegister(uint8_t num)
+uint8_t MPU6050::getSlaveRegister(SlaveId slaveId)
 {
-	if(num > 3) {
-		return 0;
-	}
-	return readByte(MPU6050_RA_I2C_SLV0_REG + num * 3);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	return readByte(MPU6050_RA_I2C_SLV0_REG + slaveId * 3);
 }
 
-void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg)
+void MPU6050::setSlaveRegister(SlaveId slaveId, uint8_t reg)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3, reg);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + slaveId * 3, reg);
 }
 
-bool MPU6050::getSlaveEnabled(uint8_t num)
+bool MPU6050::getSlaveEnabled(SlaveId slaveId)
 {
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_EN_BIT);
 }
 
-void MPU6050::setSlaveEnabled(uint8_t num, bool enabled)
+void MPU6050::setSlaveEnabled(SlaveId slaveId, bool enabled)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_EN_BIT, enabled);
 }
 
-bool MPU6050::getSlaveWordByteSwap(uint8_t num)
+bool MPU6050::getSlaveWordByteSwap(SlaveId slaveId)
 {
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_BYTE_SW_BIT);
 }
 
-void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled)
+void MPU6050::setSlaveWordByteSwap(SlaveId slaveId, bool enabled)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
 }
 
-bool MPU6050::getSlaveWriteMode(uint8_t num)
+bool MPU6050::getSlaveWriteMode(SlaveId slaveId)
 {
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_REG_DIS_BIT);
 }
 
-void MPU6050::setSlaveWriteMode(uint8_t num, bool mode)
+void MPU6050::setSlaveWriteMode(SlaveId slaveId, bool mode)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
 }
 
-bool MPU6050::getSlaveWordGroupOffset(uint8_t num)
+bool MPU6050::getSlaveWordGroupOffset(SlaveId slaveId)
 {
-	if(num > 3) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	return readBit(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_GRP_BIT);
 }
 
-void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled)
+void MPU6050::setSlaveWordGroupOffset(SlaveId slaveId, bool enabled)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_GRP_BIT, enabled);
 }
 
-uint8_t MPU6050::getSlaveDataLength(uint8_t num)
+uint8_t MPU6050::getSlaveDataLength(SlaveId slaveId)
 {
-	if(num > 3) {
-		return false;
-	}
-	return readBits(MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	return readBits(MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH);
 }
 
-void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length)
+void MPU6050::setSlaveDataLength(SlaveId slaveId, uint8_t length)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH,
-					  length);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+
+	I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + slaveId * 3, MPU6050_I2C_SLV_LEN_BIT,
+					  MPU6050_I2C_SLV_LEN_LENGTH, length);
 }
 
 MPU6050::Motion6 MPU6050::getMotion6()
@@ -258,30 +245,17 @@ MPU6050::Motion3 MPU6050::getAngularRate()
 	return angularRate;
 }
 
-void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data)
+void MPU6050::setSlaveOutputByte(SlaveId slaveId, uint8_t data)
 {
-	if(num > 3) {
-		return;
-	}
-	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data);
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + slaveId, data);
 }
 
-bool MPU6050::getSlaveDelayEnabled(uint8_t num)
+bool MPU6050::getSlaveDelayEnabled(SlaveId slaveId)
 {
 	// MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
-	if(num > 4) {
-		return false;
-	}
-	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, num);
-}
-
-void MPU6050::getFIFOBytes(uint8_t* data, uint8_t length)
-{
-	if(length > 0) {
-		I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
-	} else {
-		*data = 0;
-	}
+	ASSERT_SLAVE_ID_VALID(slaveId);
+	return readBit(MPU6050_RA_I2C_MST_DELAY_CTRL, slaveId);
 }
 
 // XA_OFFS_* registers
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index a775395ae8..cd6931d947 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -416,6 +416,8 @@
 class MPU6050
 {
 public:
+	using SlaveId = uint8_t; // (0 - 3)
+
 	struct Motion3 {
 		int16_t x{};
 		int16_t y{};
@@ -1355,18 +1357,18 @@ class MPU6050
      * Sample Rate or at the reduced rate is determined by the Delay Enable bits in
      * Register 103.
      *
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current address for specified slave
      * @see MPU6050_RA_I2C_SLV0_ADDR
      */
-	uint8_t getSlaveAddress(uint8_t num);
+	uint8_t getSlaveAddress(SlaveId slaveId);
 	/** Set the I2C address of the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param address New address for specified slave
      * @see getSlaveAddress()
      * @see MPU6050_RA_I2C_SLV0_ADDR
      */
-	void setSlaveAddress(uint8_t num, uint8_t address);
+	void setSlaveAddress(SlaveId slaveId, uint8_t address);
 
 	/** Get the active internal register for the specified slave (0-3).
      * Read/write operations for this slave will be done to whatever internal
@@ -1375,35 +1377,35 @@ class MPU6050
      * The MPU-6050 supports a total of five slaves, but Slave 4 has unique
      * characteristics, and so it has its own functions.
      *
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current active register for specified slave
      * @see MPU6050_RA_I2C_SLV0_REG
      */
-	uint8_t getSlaveRegister(uint8_t num);
+	uint8_t getSlaveRegister(SlaveId slaveId);
 
 	/** Set the active internal register for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param reg New active register for specified slave
      * @see getSlaveRegister()
      * @see MPU6050_RA_I2C_SLV0_REG
      */
-	void setSlaveRegister(uint8_t num, uint8_t reg);
+	void setSlaveRegister(SlaveId slaveId, uint8_t 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
      * cleared to 0, this bit disables Slave 0 from data transfer operations.
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current enabled value for specified slave
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	bool getSlaveEnabled(uint8_t num);
+	bool getSlaveEnabled(SlaveId slaveId);
 	/** Set the enabled value for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param enabled New enabled value for specified slave
      * @see getSlaveEnabled()
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	void setSlaveEnabled(uint8_t num, bool enabled);
+	void setSlaveEnabled(SlaveId slaveId, bool 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,
@@ -1412,19 +1414,19 @@ class MPU6050
      * bytes transferred to and from Slave 0 will be written to EXT_SENS_DATA
      * registers in the order they were transferred.
      *
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current word pair byte-swapping enabled value for specified slave
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	bool getSlaveWordByteSwap(uint8_t num);
+	bool getSlaveWordByteSwap(SlaveId slaveId);
 
 	/** Set word pair byte-swapping enabled for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param enabled New word pair byte-swapping enabled value for specified slave
      * @see getSlaveWordByteSwap()
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	void setSlaveWordByteSwap(uint8_t num, bool enabled);
+	void setSlaveWordByteSwap(SlaveId slaveId, bool 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
@@ -1432,20 +1434,20 @@ class MPU6050
      * data. This should equal 0 when specifying the register address within the
      * Slave device to/from which the ensuing data transaction will take place.
      *
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current write mode for specified slave (0 = register address + data,
      * 1 = data only)
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	bool getSlaveWriteMode(uint8_t num);
+	bool getSlaveWriteMode(SlaveId slaveId);
 	/** Set write mode for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param mode New write mode for specified slave (0 = register address + data,
      * 1 = data only)
      * @see getSlaveWriteMode()
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	void setSlaveWriteMode(uint8_t num, bool mode);
+	void setSlaveWriteMode(SlaveId slaveId, bool 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.
@@ -1454,36 +1456,36 @@ class MPU6050
      * from register addresses are paired 1 and 2, 3 and 4, etc. (odd, then even
      * register addresses) are paired to form a word.
      *
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Current word pair grouping order offset for specified slave
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	bool getSlaveWordGroupOffset(uint8_t num);
+	bool getSlaveWordGroupOffset(SlaveId slaveId);
 
 	/** Set word pair grouping order offset for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param enabled New word pair grouping order offset for specified slave
      * @see getSlaveWordGroupOffset()
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	void setSlaveWordGroupOffset(uint8_t num, bool enabled);
+	void setSlaveWordGroupOffset(SlaveId slaveId, bool 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
      * bit to 0 is equivalent to disabling the register by writing 0 to I2C_SLV0_EN.
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @return Number of bytes to read for specified slave
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	uint8_t getSlaveDataLength(uint8_t num);
+	uint8_t getSlaveDataLength(SlaveId slaveId);
 
 	/** Set number of bytes to read for the specified slave (0-3).
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param length Number of bytes to read for specified slave
      * @see getSlaveDataLength()
      * @see MPU6050_RA_I2C_SLV0_CTRL
      */
-	void setSlaveDataLength(uint8_t num, uint8_t length);
+	void setSlaveDataLength(SlaveId slaveId, uint8_t length);
 
 	// I2C_SLV* registers (Slave 4)
 
@@ -2484,11 +2486,11 @@ class MPU6050
      * This register holds the output data written into Slave when Slave is set to
      * write mode. For further information regarding Slave control, please
      * refer to Registers 37 to 39 and immediately following.
-     * @param num Slave number (0-3)
+     * @param slaveId Slave ID (0-3)
      * @param data Byte to write
      * @see MPU6050_RA_I2C_SLV0_DO
      */
-	void setSlaveOutputByte(uint8_t num, uint8_t data);
+	void setSlaveOutputByte(SlaveId slaveId, uint8_t data);
 
 	// I2C_MST_DELAY_CTRL register
 	/** Get external data shadow delay enabled status.
@@ -2528,22 +2530,22 @@ class MPU6050
      * For further information regarding the Sample Rate, please refer to
      * register 25.
      *
-     * @param num Slave number (0-4)
+     * @param slaveId Slave ID (0-4)
      * @return Current slave delay enabled status.
      * @see MPU6050_RA_I2C_MST_DELAY_CTRL
      * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
      */
-	bool getSlaveDelayEnabled(uint8_t num);
+	bool getSlaveDelayEnabled(SlaveId slaveId);
 
 	/** Set slave delay enabled status.
-     * @param num Slave number (0-4)
+     * @param slaveId Slave ID (0-4)
      * @param enabled New slave delay enabled status.
      * @see MPU6050_RA_I2C_MST_DELAY_CTRL
      * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
      */
-	void setSlaveDelayEnabled(uint8_t num, bool enabled)
+	void setSlaveDelayEnabled(SlaveId slaveId, bool enabled)
 	{
-		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled);
+		I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, slaveId, enabled);
 	}
 
 	// SIGNAL_PATH_RESET register
@@ -3136,7 +3138,10 @@ class MPU6050
 		I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data);
 	}
 
-	void getFIFOBytes(uint8_t* data, uint8_t length);
+	void getFIFOBytes(uint8_t* data, uint8_t length)
+	{
+		I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data);
+	}
 
 	// WHO_AM_I register
 

From 8c4ee1e4e8d9211a1a533040478cef4bb4b77742 Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Tue, 7 Nov 2023 15:24:35 +0100
Subject: [PATCH 19/21] Update readme format

---
 Sming/Libraries/MPU6050/README.rst | 14 ++++++++++----
 1 file changed, 10 insertions(+), 4 deletions(-)

diff --git a/Sming/Libraries/MPU6050/README.rst b/Sming/Libraries/MPU6050/README.rst
index ee201c2f9a..0f4282007d 100644
--- a/Sming/Libraries/MPU6050/README.rst
+++ b/Sming/Libraries/MPU6050/README.rst
@@ -1,14 +1,20 @@
-API Documentation
------------------
+MPU6050 Gyro / Accelerometer
+=============================
 
-.. doxygenclass:: MPU6050
    :members:
 
 # MPU6050 Six-Axis (Gyro + Accelerometer)
-Based on code from [jrowberg/i2cdevlib](https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050) @ 605a740. Most of the code is the same, except:
+Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>` @ 605a740. Most of the code is the same, except:
 
 -   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
 -   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.
 -   Removed map function in favor of the Sming built-in one.
 -   Adapted include path, coding style and applied clangformat
 -   Deleted Calibration and Memory Block related code for code quality reason
+
+
+API Documentation
+-----------------
+
+.. doxygenclass:: MPU6050
+   :members:

From 4aac518959d15f61ee0b88f849b85a7fbf0f49fa Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Wed, 8 Nov 2023 08:10:05 +0100
Subject: [PATCH 20/21] README format

---
 Sming/Libraries/MPU6050/README.rst | 5 ++---
 1 file changed, 2 insertions(+), 3 deletions(-)

diff --git a/Sming/Libraries/MPU6050/README.rst b/Sming/Libraries/MPU6050/README.rst
index 0f4282007d..8018fe0acd 100644
--- a/Sming/Libraries/MPU6050/README.rst
+++ b/Sming/Libraries/MPU6050/README.rst
@@ -1,10 +1,9 @@
 MPU6050 Gyro / Accelerometer
 =============================
 
-   :members:
 
-# MPU6050 Six-Axis (Gyro + Accelerometer)
-Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>` @ 605a740. Most of the code is the same, except:
+MPU6050 Six-Axis (Gyro + Accelerometer)
+Based on code from `jrowberg/i2cdevlib <https://github.com/jrowberg/i2cdevlib/tree/master/ESP32_ESP-IDF/components/MPU6050>`__ @ 605a740. Most of the code is the same, except:
 
 -   Removed MPU6050::ReadRegister function due to incompatibility. It is also not used anywhere in the original code.
 -   MPU6050_6Axis_MotionApps20.h and MPU6050_9Axis_MotionApps41.h are not included due to deps to freeRTOS. helper_3dmath.h is also not included since it is only used in the above mentioned files.

From 850afe0ce493ac4d1276af123d4f4ee4fa84a223 Mon Sep 17 00:00:00 2001
From: Qian Qian <xeonqq@gmail.com>
Date: Wed, 8 Nov 2023 15:42:55 +0100
Subject: [PATCH 21/21] Fix typos

---
 Sming/Libraries/MPU6050/MPU6050.cpp | 2 +-
 Sming/Libraries/MPU6050/MPU6050.h   | 4 ++--
 2 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/Sming/Libraries/MPU6050/MPU6050.cpp b/Sming/Libraries/MPU6050/MPU6050.cpp
index 6174f909e8..cc639c8eb6 100644
--- a/Sming/Libraries/MPU6050/MPU6050.cpp
+++ b/Sming/Libraries/MPU6050/MPU6050.cpp
@@ -6,7 +6,7 @@
 // Changelog:
 //     ... - ongoing debug release
 
-// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
+// NOTE: THIS IS ONLY A PARTIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
 // UNDERGOING ACTIVE DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES.
 // PLEASE KEEP THIS IN MIND IF YOU DECIDE TO USE THIS PARTICULAR CODE FOR
 // ANYTHING.
diff --git a/Sming/Libraries/MPU6050/MPU6050.h b/Sming/Libraries/MPU6050/MPU6050.h
index cd6931d947..ef277d1e35 100644
--- a/Sming/Libraries/MPU6050/MPU6050.h
+++ b/Sming/Libraries/MPU6050/MPU6050.h
@@ -2,7 +2,7 @@
 // (RM-MPU-6000A-00)
 // Based On https://github.com/jrowberg/i2cdevlib
 
-// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
+// NOTE: THIS IS ONLY A PARTIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY
 // UNDERGOING ACTIVE DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES.
 // PLEASE KEEP THIS IN MIND IF YOU DECIDE TO USE THIS PARTICULAR CODE FOR
 // ANYTHING.
@@ -3427,7 +3427,7 @@ template <typename T> T MPU6050::readReg(uint8_t regAddr)
 
 	const auto sz = sizeof(T);
 	uint8_t buffer[sz] = {0};
-	//data follow big endien convention
+	//data follow big endian convention
 	I2Cdev::readBytes(devAddr, regAddr, sz, buffer);
 
 	T result{};