diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..599cc1a3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,14 @@ +PIC18/MPU6050/Examples/MPU6050_raw.X/dist/ +PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/private/ +PIC18/MPU6050/Examples/MPU6050_raw.X/build/ +*~ +/dsPIC30F/I2Cdev/I2CdevdsPic30F.X/nbproject/private/ +/dsPIC30F/I2Cdev/I2CdevdsPic30F.X/dist/default/ +/dsPIC30F/I2Cdev/testMCC.X/nbproject/private/ +/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/private/ +/dsPIC30F/MPU6050/Examples/MPU6050_example.X/build/default/ +/dsPIC30F/MPU6050/Examples/MPU6050_example.X/dist/default/ +ESP32_ESP-IDF/build/ +ESP32_ESP-IDF/sdkconfig +/RP2040/MPU6050/examples/mpu6050_calibration +/RP2040/MPU6050/examples/mpu6050_DMP_V6.12 diff --git a/Arduino/AD7746/AD7746.cpp b/Arduino/AD7746/AD7746.cpp index bf56e68f..53efa828 100755 --- a/Arduino/AD7746/AD7746.cpp +++ b/Arduino/AD7746/AD7746.cpp @@ -132,3 +132,11 @@ void AD7746::writeCapDacARegister(uint8_t data) { void AD7746::writeCapDacBRegister(uint8_t data) { I2Cdev::writeByte(devAddr, AD7746_RA_CAP_DAC_B, data); } + +void AD7746::write_register(uint8_t addr,uint8_t data) { + I2Cdev::writeByte(devAddr, addr, data); +} + +void AD7746::read_register(uint8_t addr, uint8_t *data) { + I2Cdev::readByte(devAddr, addr, data); +} diff --git a/Arduino/AD7746/AD7746.h b/Arduino/AD7746/AD7746.h index 98c4160f..5ca3f112 100755 --- a/Arduino/AD7746/AD7746.h +++ b/Arduino/AD7746/AD7746.h @@ -176,6 +176,8 @@ class AD7746 { void writeConfigurationRegister(uint8_t data); void writeCapDacARegister(uint8_t data); void writeCapDacBRegister(uint8_t data); + void write_register(uint8_t addr,uint8_t data); + void read_register(uint8_t addr, uint8_t *data); private: diff --git a/Arduino/AD7746/library.json b/Arduino/AD7746/library.json new file mode 100644 index 00000000..7050cfed --- /dev/null +++ b/Arduino/AD7746/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-AD7746", + "version": "1.0.0", + "keywords": "capacitance, converter, sensor, i2cdevlib, i2c", + "description": "The AD7745/AD7746 are 24-Bit Capacitance-to-Digital Converter with Temperature Sensor", + "include": "Arduino/AD7746", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/ADS1115/ADS1115.cpp b/Arduino/ADS1115/ADS1115.cpp index 23c867e8..c3d7e873 100644 --- a/Arduino/ADS1115/ADS1115.cpp +++ b/Arduino/ADS1115/ADS1115.cpp @@ -79,18 +79,19 @@ bool ADS1115::testConnection() { return I2Cdev::readWord(devAddr, ADS1115_RA_CONVERSION, buffer) == 1; } -/** Wait until the single-shot conversion is finished +/** Poll the operational status bit until the conversion is finished * Retry at most 'max_retries' times - * conversion is finished, then return; - * @see ADS1115_OS_INACTIVE + * conversion is finished, then return true; + * @see ADS1115_CFG_OS_BIT + * @return True if data is available, false otherwise */ -void ADS1115::waitBusy(uint16_t max_retries) { +bool ADS1115::pollConversion(uint16_t max_retries) { for(uint16_t i = 0; i < max_retries; i++) { - if (getOpStatus()==ADS1115_OS_INACTIVE) break; + if (isConversionReady()) return true; } + return false; } - /** Read differential value based on current MUX configuration. * The default MUX setting sets the device to get the differential between the * AIN0 and AIN1 pins. There are 8 possible MUX settings, but if you are using @@ -106,6 +107,8 @@ void ADS1115::waitBusy(uint16_t max_retries) { * effortless, but it has enormous potential to save power by only running the * comparison circuitry when needed. * + * @param triggerAndPoll If true (and only in singleshot mode) the conversion trigger + * will be executed and the conversion results will be polled. * @return 16-bit signed differential value * @see getConversionP0N1(); * @see getConversionPON3(); @@ -126,12 +129,10 @@ void ADS1115::waitBusy(uint16_t max_retries) { * @see ADS1115_MUX_P2_NG * @see ADS1115_MUX_P3_NG */ -int16_t ADS1115::getConversion() { - if (devMode == ADS1115_MODE_SINGLESHOT) - { - setOpStatus(ADS1115_OS_ACTIVE); - ADS1115::waitBusy(I2CDEV_DEFAULT_READ_TIMEOUT); - +int16_t ADS1115::getConversion(bool triggerAndPoll) { + if (triggerAndPoll && devMode == ADS1115_MODE_SINGLESHOT) { + triggerConversion(); + pollConversion(I2CDEV_DEFAULT_READ_TIMEOUT); } I2Cdev::readWord(devAddr, ADS1115_RA_CONVERSION, buffer); return buffer[0]; @@ -233,29 +234,30 @@ int16_t ADS1115::getConversionP3GND() { * Read the current differential and return it multiplied * by the constant for the current gain. mV is returned to * increase the precision of the voltage - * + * @param triggerAndPoll If true (and only in singleshot mode) the conversion trigger + * will be executed and the conversion results will be polled. */ -float ADS1115::getMilliVolts() { +float ADS1115::getMilliVolts(bool triggerAndPoll) { switch (pgaMode) { case ADS1115_PGA_6P144: - return (getConversion() * ADS1115_MV_6P144); + return (getConversion(triggerAndPoll) * ADS1115_MV_6P144); break; case ADS1115_PGA_4P096: - return (getConversion() * ADS1115_MV_4P096); + return (getConversion(triggerAndPoll) * ADS1115_MV_4P096); break; case ADS1115_PGA_2P048: - return (getConversion() * ADS1115_MV_2P048); + return (getConversion(triggerAndPoll) * ADS1115_MV_2P048); break; case ADS1115_PGA_1P024: - return (getConversion() * ADS1115_MV_1P024); + return (getConversion(triggerAndPoll) * ADS1115_MV_1P024); break; case ADS1115_PGA_0P512: - return (getConversion() * ADS1115_MV_0P512); + return (getConversion(triggerAndPoll) * ADS1115_MV_0P512); break; case ADS1115_PGA_0P256: case ADS1115_PGA_0P256B: case ADS1115_PGA_0P256C: - return (getConversion() * ADS1115_MV_0P256); + return (getConversion(triggerAndPoll) * ADS1115_MV_0P256); break; } } @@ -299,24 +301,21 @@ float ADS1115::getMvPerCount() { // CONFIG register /** Get operational status. - * @return Current operational status (0 for active conversion, 1 for inactive) - * @see ADS1115_OS_ACTIVE - * @see ADS1115_OS_INACTIVE + * @return Current operational status (false for active conversion, true for inactive) * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_OS_BIT */ -uint8_t ADS1115::getOpStatus() { +bool ADS1115::isConversionReady() { I2Cdev::readBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_OS_BIT, buffer); - return (uint8_t)buffer[0]; + return buffer[0]; } -/** Set operational status. - * This bit can only be written while in power-down mode (no conversions active). - * @param status New operational status (0 does nothing, 1 to trigger conversion) +/** Trigger a new conversion. + * Writing to this bit will only have effect while in power-down mode (no conversions active). * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_OS_BIT */ -void ADS1115::setOpStatus(uint8_t status) { - I2Cdev::writeBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_OS_BIT, status); +void ADS1115::triggerConversion() { + I2Cdev::writeBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_OS_BIT, 1); } /** Get multiplexer connection. * @return Current multiplexer connection setting @@ -401,9 +400,9 @@ void ADS1115::setGain(uint8_t gain) { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_MODE_BIT */ -uint8_t ADS1115::getMode() { +bool ADS1115::getMode() { I2Cdev::readBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_MODE_BIT, buffer); - devMode = (uint8_t)buffer[0]; + devMode = buffer[0]; return devMode; } /** Set device mode. @@ -413,7 +412,7 @@ uint8_t ADS1115::getMode() { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_MODE_BIT */ -void ADS1115::setMode(uint8_t mode) { +void ADS1115::setMode(bool mode) { if (I2Cdev::writeBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_MODE_BIT, mode)) { devMode = mode; } @@ -452,9 +451,9 @@ void ADS1115::setRate(uint8_t rate) { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_COMP_MODE_BIT */ -uint8_t ADS1115::getComparatorMode() { +bool ADS1115::getComparatorMode() { I2Cdev::readBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_COMP_MODE_BIT, buffer); - return (uint8_t)buffer[0]; + return buffer[0]; } /** Set comparator mode. * @param mode New comparator mode @@ -463,7 +462,7 @@ uint8_t ADS1115::getComparatorMode() { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_COMP_MODE_BIT */ -void ADS1115::setComparatorMode(uint8_t mode) { +void ADS1115::setComparatorMode(bool mode) { I2Cdev::writeBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_COMP_MODE_BIT, mode); } /** Get comparator polarity setting. @@ -473,9 +472,9 @@ void ADS1115::setComparatorMode(uint8_t mode) { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_COMP_POL_BIT */ -uint8_t ADS1115::getComparatorPolarity() { +bool ADS1115::getComparatorPolarity() { I2Cdev::readBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_COMP_POL_BIT, buffer); - return (uint8_t)buffer[0]; + return buffer[0]; } /** Set comparator polarity setting. * @param polarity New comparator polarity setting @@ -484,7 +483,7 @@ uint8_t ADS1115::getComparatorPolarity() { * @see ADS1115_RA_CONFIG * @see ADS1115_CFG_COMP_POL_BIT */ -void ADS1115::setComparatorPolarity(uint8_t polarity) { +void ADS1115::setComparatorPolarity(bool polarity) { I2Cdev::writeBitW(devAddr, ADS1115_RA_CONFIG, ADS1115_CFG_COMP_POL_BIT, polarity); } /** Get comparator latch enabled value. @@ -569,23 +568,32 @@ void ADS1115::setHighThreshold(int16_t threshold) { I2Cdev::writeWord(devAddr, ADS1115_RA_HI_THRESH, threshold); } +/** Configures ALERT/RDY pin as a conversion ready pin. + * It does this by setting the MSB of the high threshold register to '1' and the MSB + * of the low threshold register to '0'. COMP_POL and COMP_QUE bits will be set to '0'. + * Note: ALERT/RDY pin requires a pull up resistor. + */ +void ADS1115::setConversionReadyPinMode() { + I2Cdev::writeBitW(devAddr, ADS1115_RA_HI_THRESH, 15, 1); + I2Cdev::writeBitW(devAddr, ADS1115_RA_LO_THRESH, 15, 0); + setComparatorPolarity(0); + setComparatorQueueMode(0); +} + // Create a mask between two bits -unsigned createMask(unsigned a, unsigned b) -{ +unsigned createMask(unsigned a, unsigned b) { unsigned mask = 0; for (unsigned i=a; i<=b; i++) mask |= 1 << i; return mask; } -uint16_t shiftDown(uint16_t extractFrom, int places) -{ +uint16_t shiftDown(uint16_t extractFrom, int places) { return (extractFrom >> places); } -uint16_t getValueFromBits(uint16_t extractFrom, int high, int length) -{ +uint16_t getValueFromBits(uint16_t extractFrom, int high, int length) { int low= high-length +1; uint16_t mask = createMask(low ,high); return shiftDown(extractFrom & mask, low); @@ -593,8 +601,7 @@ uint16_t getValueFromBits(uint16_t extractFrom, int high, int length) /** Show all the config register settings */ -void ADS1115::showConfigRegister() -{ +void ADS1115::showConfigRegister() { I2Cdev::readWord(devAddr, ADS1115_RA_CONFIG, buffer); uint16_t configRegister =buffer[0]; @@ -638,6 +645,5 @@ void ADS1115::showConfigRegister() Serial.println(getValueFromBits(configRegister, ADS1115_CFG_COMP_QUE_BIT,ADS1115_CFG_COMP_QUE_LENGTH), BIN); #endif - }; diff --git a/Arduino/ADS1115/ADS1115.h b/Arduino/ADS1115/ADS1115.h index f8bdb7ff..2ac68eea 100644 --- a/Arduino/ADS1115/ADS1115.h +++ b/Arduino/ADS1115/ADS1115.h @@ -69,8 +69,6 @@ THE SOFTWARE. #define ADS1115_CFG_COMP_QUE_BIT 1 #define ADS1115_CFG_COMP_QUE_LENGTH 2 -#define ADS1115_OS_INACTIVE 0x00 -#define ADS1115_OS_ACTIVE 0x01 #define ADS1115_MUX_P0_N1 0x00 // default #define ADS1115_MUX_P0_N3 0x01 @@ -135,22 +133,23 @@ class ADS1115 { public: ADS1115(); ADS1115(uint8_t address); - + void initialize(); bool testConnection(); - + // SINGLE SHOT utilities - void waitBusy(uint16_t max_retries); + bool pollConversion(uint16_t max_retries); + void triggerConversion(); // Read the current CONVERSION register - int16_t getConversion(); - + int16_t getConversion(bool triggerAndPoll=true); + // Differential int16_t getConversionP0N1(); int16_t getConversionP0N3(); int16_t getConversionP1N3(); int16_t getConversionP2N3(); - + // Single-ended int16_t getConversionP0GND(); int16_t getConversionP1GND(); @@ -158,42 +157,42 @@ class ADS1115 { int16_t getConversionP3GND(); // Utility - float getMilliVolts(); + float getMilliVolts(bool triggerAndPoll=true); float getMvPerCount(); // CONFIG register - uint8_t getOpStatus(); - void setOpStatus(uint8_t op); + bool isConversionReady(); uint8_t getMultiplexer(); void setMultiplexer(uint8_t mux); uint8_t getGain(); void setGain(uint8_t gain); - uint8_t getMode(); - void setMode(uint8_t mode); + bool getMode(); + void setMode(bool mode); uint8_t getRate(); void setRate(uint8_t rate); - uint8_t getComparatorMode(); - void setComparatorMode(uint8_t mode); - uint8_t getComparatorPolarity(); - void setComparatorPolarity(uint8_t polarity); + bool getComparatorMode(); + void setComparatorMode(bool mode); + bool getComparatorPolarity(); + void setComparatorPolarity(bool polarity); bool getComparatorLatchEnabled(); void setComparatorLatchEnabled(bool enabled); uint8_t getComparatorQueueMode(); void setComparatorQueueMode(uint8_t mode); + void setConversionReadyPinMode(); // *_THRESH registers int16_t getLowThreshold(); void setLowThreshold(int16_t threshold); int16_t getHighThreshold(); void setHighThreshold(int16_t threshold); - + // DEBUG void showConfigRegister(); private: uint8_t devAddr; uint16_t buffer[2]; - uint8_t devMode; + bool devMode; uint8_t muxMode; uint8_t pgaMode; }; diff --git a/Arduino/ADS1115/Examples/ADS1115_differential/ADS1115_differential.ino b/Arduino/ADS1115/examples/ADS1115_differential/ADS1115_differential.ino similarity index 95% rename from Arduino/ADS1115/Examples/ADS1115_differential/ADS1115_differential.ino rename to Arduino/ADS1115/examples/ADS1115_differential/ADS1115_differential.ino index ef884d3e..a67b77bf 100644 --- a/Arduino/ADS1115/Examples/ADS1115_differential/ADS1115_differential.ino +++ b/Arduino/ADS1115/examples/ADS1115_differential/ADS1115_differential.ino @@ -29,7 +29,6 @@ OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. =============================================== */ -#include #include "ADS1115.h" ADS1115 adc0(ADS1115_DEFAULT_ADDRESS); @@ -49,16 +48,14 @@ void setup() { // We're going to do continuous sampling adc0.setMode(ADS1115_MODE_CONTINUOUS); - } -void loop() - { +void loop() { // Sensor is on P0/N1 (pins 4/5) Serial.println("Sensor 1 ************************"); // Set the gain (PGA) +/- 1.024v - adc0.setGain(ADS1115_PGA_1P024 ); + adc0.setGain(ADS1115_PGA_1P024); // Get the number of counts of the accumulator Serial.print("Counts for sensor 1 is:"); @@ -69,7 +66,7 @@ void loop() // To turn the counts into a voltage, we can use Serial.print("Voltage for sensor 1 is:"); - Serial.println(sensorOneCounts*adc0.getMvPerCount() ); + Serial.println(sensorOneCounts*adc0.getMvPerCount()); Serial.println(); @@ -90,9 +87,5 @@ void loop() Serial.println(); delay(500); - } - - - - +} diff --git a/Arduino/ADS1115/examples/ADS1115_single/ADS1115_single.ino b/Arduino/ADS1115/examples/ADS1115_single/ADS1115_single.ino new file mode 100644 index 00000000..4059ac1e --- /dev/null +++ b/Arduino/ADS1115/examples/ADS1115_single/ADS1115_single.ino @@ -0,0 +1,110 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for ADS1115 class +// Example of reading two differential inputs of the ADS1115 and showing the value in mV +// 2016-03-22 by Eadf (https://github.com/eadf) +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-03-22 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 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 "ADS1115.h" + +ADS1115 adc0(ADS1115_DEFAULT_ADDRESS); + +// Wire ADS1115 ALERT/RDY pin to Arduino pin 2 +const int alertReadyPin = 2; + +void setup() { + //I2Cdev::begin(); // join I2C bus + Wire.begin(); + Serial.begin(115200); // initialize serial communication + + Serial.println("Testing device connections..."); + Serial.println(adc0.testConnection() ? "ADS1115 connection successful" : "ADS1115 connection failed"); + + adc0.initialize(); // initialize ADS1115 16 bit A/D chip + + // We're going to do single shot sampling + adc0.setMode(ADS1115_MODE_SINGLESHOT); + + // Slow things down so that we can see that the "poll for conversion" code works + adc0.setRate(ADS1115_RATE_8); + + // Set the gain (PGA) +/- 6.144V + // Note that any analog input must be higher than –0.3V and less than VDD +0.3 + adc0.setGain(ADS1115_PGA_6P144); + // ALERT/RDY pin will indicate when conversion is ready + + pinMode(alertReadyPin,INPUT_PULLUP); + adc0.setConversionReadyPinMode(); + + // To get output from this method, you'll need to turn on the + //#define ADS1115_SERIAL_DEBUG // in the ADS1115.h file + #ifdef ADS1115_SERIAL_DEBUG + adc0.showConfigRegister(); + Serial.print("HighThreshold="); Serial.println(adc0.getHighThreshold(),BIN); + Serial.print("LowThreshold="); Serial.println(adc0.getLowThreshold(),BIN); + #endif +} + +/** Poll the assigned pin for conversion status + */ +void pollAlertReadyPin() { + for (uint32_t i = 0; i<100000; i++) + if (!digitalRead(alertReadyPin)) return; + Serial.println("Failed to wait for AlertReadyPin, it's stuck high!"); +} + +void loop() { + + // The below method sets the mux and gets a reading. + adc0.setMultiplexer(ADS1115_MUX_P0_NG); + adc0.triggerConversion(); + pollAlertReadyPin(); + Serial.print("A0: "); Serial.print(adc0.getMilliVolts(false)); Serial.print("mV\t"); + + adc0.setMultiplexer(ADS1115_MUX_P1_NG); + adc0.triggerConversion(); + pollAlertReadyPin(); + Serial.print("A1: "); Serial.print(adc0.getMilliVolts(false)); Serial.print("mV\t"); + + adc0.setMultiplexer(ADS1115_MUX_P2_NG); + adc0.triggerConversion(); + pollAlertReadyPin(); + Serial.print("A2: "); Serial.print(adc0.getMilliVolts(false)); Serial.print("mV\t"); + + adc0.setMultiplexer(ADS1115_MUX_P3_NG); + // Do conversion polling via I2C on this last reading: + Serial.print("A3: "); Serial.print(adc0.getMilliVolts(true)); Serial.print("mV"); + + Serial.println(digitalRead(alertReadyPin)); + delay(500); +} + + + + + diff --git a/Arduino/ADS1115/library.json b/Arduino/ADS1115/library.json new file mode 100644 index 00000000..50dad5c9 --- /dev/null +++ b/Arduino/ADS1115/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-ADS1115", + "version": "1.0.0", + "keywords": "MUX, PGA, comparator, oscillator, reference, i2cdevlib, i2c", + "description": "ADS1115 is 16-Bit ADC with Integrated MUX, PGA, Comparator, Oscillator, and Reference", + "include": "Arduino/ADS1115", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/ADXL345/Examples/ADXL345_raw/ADXL345_raw.ino b/Arduino/ADXL345/examples/ADXL345_raw/ADXL345_raw.ino similarity index 100% rename from Arduino/ADXL345/Examples/ADXL345_raw/ADXL345_raw.ino rename to Arduino/ADXL345/examples/ADXL345_raw/ADXL345_raw.ino diff --git a/Arduino/ADXL345/library.json b/Arduino/ADXL345/library.json new file mode 100644 index 00000000..90ef2108 --- /dev/null +++ b/Arduino/ADXL345/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-ADXL345", + "version": "1.0.0", + "keywords": "accelerometer, sensor, i2cdevlib, i2c", + "description": "The ADXL345 is a small, thin, ultralow power, 3-axis accelerometer with high resolution (13-bit) measurement", + "include": "Arduino/ADXL345", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/AK8963/AK8963.cpp b/Arduino/AK8963/AK8963.cpp new file mode 100644 index 00000000..7643d76c --- /dev/null +++ b/Arduino/AK8963/AK8963.cpp @@ -0,0 +1,189 @@ +// I2Cdev library collection - AK8963 I2C device class header file +// Based on AKM AK8963 datasheet, 10/2013 +// 8/27/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-01-02 - initial release based on AK8975 code +// + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 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 "AK8963.h" + +/** Default constructor, uses default I2C address. + * @see AK8963_DEFAULT_ADDRESS + */ +AK8963::AK8963() { + devAddr = AK8963_DEFAULT_ADDRESS; +} + +/** Specific address constructor. + * @param address I2C address + * @see AK8963_DEFAULT_ADDRESS + * @see AK8963_ADDRESS_00 + */ +AK8963::AK8963(uint8_t address) { + devAddr = address; +} + +/** Power on and prepare for general usage. + * No specific pre-configuration is necessary for this device. + */ +void AK8963::initialize() { +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool AK8963::testConnection() { + if (I2Cdev::readByte(devAddr, AK8963_RA_WIA, buffer) == 1) { + return (buffer[0] == 0x48); + } + return false; +} + +// WIA register + +uint8_t AK8963::getDeviceID() { + I2Cdev::readByte(devAddr, AK8963_RA_WIA, buffer); + return buffer[0]; +} + +// INFO register + +uint8_t AK8963::getInfo() { + I2Cdev::readByte(devAddr, AK8963_RA_INFO, buffer); + return buffer[0]; +} + +// ST1 register + +bool AK8963::getDataReady() { + I2Cdev::readBit(devAddr, AK8963_RA_ST1, AK8963_ST1_DRDY_BIT, buffer); + return buffer[0]; +} + +bool AK8963::getDataOverrun() { + I2Cdev::readBit(devAddr, AK8963_RA_ST1, AK8963_ST1_DOR_BIT, buffer); + return buffer[0]; +} + +// H* registers +void AK8963::getHeading(int16_t *x, int16_t *y, int16_t *z) { + I2Cdev::readBytes(devAddr, AK8963_RA_HXL, 6, buffer); + *x = (((int16_t)buffer[1]) << 8) | buffer[0]; + *y = (((int16_t)buffer[3]) << 8) | buffer[2]; + *z = (((int16_t)buffer[5]) << 8) | buffer[4]; +} +int16_t AK8963::getHeadingX() { + I2Cdev::readBytes(devAddr, AK8963_RA_HXL, 2, buffer); + return (((int16_t)buffer[1]) << 8) | buffer[0]; +} +int16_t AK8963::getHeadingY() { + I2Cdev::readBytes(devAddr, AK8963_RA_HYL, 2, buffer); + return (((int16_t)buffer[1]) << 8) | buffer[0]; +} +int16_t AK8963::getHeadingZ() { + I2Cdev::readBytes(devAddr, AK8963_RA_HZL, 2, buffer); + return (((int16_t)buffer[1]) << 8) | buffer[0]; +} + +// ST2 register +bool AK8963::getOverflowStatus() { + I2Cdev::readBit(devAddr, AK8963_RA_ST2, AK8963_ST2_HOFL_BIT, buffer); + return buffer[0]; +} +bool AK8963::getOutputBit() { + I2Cdev::readBit(devAddr, AK8963_RA_ST2, AK8963_ST2_BITM_BIT, buffer); + return buffer[0]; +} + +// CNTL1 register +uint8_t AK8963::getMode() { + I2Cdev::readBits(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_MODE_BIT, AK8963_CNTL1_MODE_LENGTH, buffer); + return buffer[0]; +} +void AK8963::setMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_MODE_BIT, AK8963_CNTL1_MODE_LENGTH, mode); +} +uint8_t AK8963::getResolution() { + I2Cdev::readBit(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_RES_BIT, buffer); + return buffer[0]; +} +void AK8963::setResolution(uint8_t res) { + I2Cdev::writeBit(devAddr, AK8963_RA_CNTL1, AK8963_CNTL1_RES_BIT, res); +} + +// CNTL2 register +void AK8963::reset() { + I2Cdev::writeByte(devAddr, AK8963_RA_CNTL2, AK8963_CNTL2_RESET); +} + +// ASTC register +void AK8963::setSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, AK8963_RA_ASTC, AK8963_ASTC_SELF_BIT, enabled); +} + +// I2CDIS +void AK8963::disableI2C() { + I2Cdev::writeByte(devAddr, AK8963_RA_I2CDIS, AK8963_I2CDIS_DISABLE); +} + +// ASA* registers +void AK8963::getAdjustment(int8_t *x, int8_t *y, int8_t *z) { + I2Cdev::readBytes(devAddr, AK8963_RA_ASAX, 3, buffer); + *x = buffer[0]; + *y = buffer[1]; + *z = buffer[2]; +} +void AK8963::setAdjustment(int8_t x, int8_t y, int8_t z) { + buffer[0] = x; + buffer[1] = y; + buffer[2] = z; + I2Cdev::writeBytes(devAddr, AK8963_RA_ASAX, 3, buffer); +} +uint8_t AK8963::getAdjustmentX() { + I2Cdev::readByte(devAddr, AK8963_RA_ASAX, buffer); + return buffer[0]; +} +void AK8963::setAdjustmentX(uint8_t x) { + I2Cdev::writeByte(devAddr, AK8963_RA_ASAX, x); +} +uint8_t AK8963::getAdjustmentY() { + I2Cdev::readByte(devAddr, AK8963_RA_ASAY, buffer); + return buffer[0]; +} +void AK8963::setAdjustmentY(uint8_t y) { + I2Cdev::writeByte(devAddr, AK8963_RA_ASAY, y); +} +uint8_t AK8963::getAdjustmentZ() { + I2Cdev::readByte(devAddr, AK8963_RA_ASAZ, buffer); + return buffer[0]; +} +void AK8963::setAdjustmentZ(uint8_t z) { + I2Cdev::writeByte(devAddr, AK8963_RA_ASAZ, z); +} diff --git a/Arduino/AK8963/AK8963.h b/Arduino/AK8963/AK8963.h new file mode 100644 index 00000000..c19f13b5 --- /dev/null +++ b/Arduino/AK8963/AK8963.h @@ -0,0 +1,151 @@ +// I2Cdev library collection - AK8963 I2C device class header file +// Based on AKM AK8963 datasheet, 10/2013 +// 8/27/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-01-02 - initial release based on AK8975 code +// + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 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 _AK8963_H_ +#define _AK8963_H_ + +#include "I2Cdev.h" + +#define AK8963_ADDRESS_00 0x0C +#define AK8963_ADDRESS_01 0x0D +#define AK8963_ADDRESS_10 0x0E +#define AK8963_ADDRESS_11 0x0F +#define AK8963_DEFAULT_ADDRESS AK8963_ADDRESS_00 + +#define AK8963_RA_WIA 0x00 +#define AK8963_RA_INFO 0x01 +#define AK8963_RA_ST1 0x02 +#define AK8963_RA_HXL 0x03 +#define AK8963_RA_HXH 0x04 +#define AK8963_RA_HYL 0x05 +#define AK8963_RA_HYH 0x06 +#define AK8963_RA_HZL 0x07 +#define AK8963_RA_HZH 0x08 +#define AK8963_RA_ST2 0x09 +#define AK8963_RA_CNTL1 0x0A +#define AK8963_RA_CNTL2 0x0B +#define AK8963_RA_ASTC 0x0C +#define AK8963_RA_TS1 0x0D // SHIPMENT TEST, DO NOT USE +#define AK8963_RA_TS2 0x0E // SHIPMENT TEST, DO NOT USE +#define AK8963_RA_I2CDIS 0x0F +#define AK8963_RA_ASAX 0x10 +#define AK8963_RA_ASAY 0x11 +#define AK8963_RA_ASAZ 0x12 + +#define AK8963_ST1_DRDY_BIT 0 +#define AK8963_ST1_DOR_BIT 1 + +#define AK8963_ST2_HOFL_BIT 3 +#define AK8963_ST2_BITM_BIT 4 + +#define AK8963_CNTL1_MODE_BIT 3 +#define AK8963_CNTL1_MODE_LENGTH 4 +#define AK8963_CNTL1_RES_BIT 4 + +#define AK8963_CNTL2_RESET 0x01 + +#define AK8963_MODE_POWERDOWN 0x0 +#define AK8963_MODE_SINGLE 0x1 +#define AK8963_MODE_CONTINUOUS_8HZ 0x2 +#define AK8963_MODE_EXTERNAL 0x4 +#define AK8963_MODE_CONTINUOUS_100HZ 0x6 +#define AK8963_MODE_SELFTEST 0x8 +#define AK8963_MODE_FUSEROM 0xF + +#define AK8963_RES_14_BIT 0 +#define AK8963_RES_16_BIT 1 + +#define AK8963_CNTL2_SRST_BIT 0 + +#define AK8963_ASTC_SELF_BIT 6 + +#define AK8963_I2CDIS_DISABLE 0x1B + +class AK8963 { + public: + AK8963(); + AK8963(uint8_t address); + + void initialize(); + bool testConnection(); + + // WIA register + uint8_t getDeviceID(); + + // INFO register + uint8_t getInfo(); + + // ST1 register + bool getDataReady(); + bool getDataOverrun(); + + // H* registers + void getHeading(int16_t *x, int16_t *y, int16_t *z); + int16_t getHeadingX(); + int16_t getHeadingY(); + int16_t getHeadingZ(); + + // ST2 register + bool getOverflowStatus(); + bool getOutputBit(); + + // CNTL1 register + uint8_t getMode(); + void setMode(uint8_t mode); + uint8_t getResolution(); + void setResolution(uint8_t resolution); + void reset(); + + // ASTC register + void setSelfTest(bool enabled); + + // I2CDIS + void disableI2C(); // um, why...? + + // ASA* registers + void getAdjustment(int8_t *x, int8_t *y, int8_t *z); + void setAdjustment(int8_t x, int8_t y, int8_t z); + uint8_t getAdjustmentX(); + void setAdjustmentX(uint8_t x); + uint8_t getAdjustmentY(); + void setAdjustmentY(uint8_t y); + uint8_t getAdjustmentZ(); + void setAdjustmentZ(uint8_t z); + + private: + uint8_t devAddr; + uint8_t buffer[6]; + uint8_t mode; +}; + +#endif /* _AK8963_H_ */ diff --git a/Arduino/AK8963/library.json b/Arduino/AK8963/library.json new file mode 100644 index 00000000..2d1e96e1 --- /dev/null +++ b/Arduino/AK8963/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-AK8963", + "version": "1.0.0", + "keywords": "compass, sensor, i2cdevlib, i2c", + "description": "AK8963 is 3-axis electronic compass IC with high sensitive Hall sensor technology", + "include": "Arduino/AK8963", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/AK8975/AK8975.cpp b/Arduino/AK8975/AK8975.cpp index c68b5cd9..1fedf829 100644 --- a/Arduino/AK8975/AK8975.cpp +++ b/Arduino/AK8975/AK8975.cpp @@ -148,35 +148,51 @@ void AK8975::disableI2C() { // ASA* registers void AK8975::getAdjustment(int8_t *x, int8_t *y, int8_t *z) { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::readBytes(devAddr, AK8975_RA_ASAX, 3, buffer); *x = buffer[0]; *y = buffer[1]; *z = buffer[2]; } void AK8975::setAdjustment(int8_t x, int8_t y, int8_t z) { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); buffer[0] = x; buffer[1] = y; buffer[2] = z; I2Cdev::writeBytes(devAddr, AK8975_RA_ASAX, 3, buffer); } uint8_t AK8975::getAdjustmentX() { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::readByte(devAddr, AK8975_RA_ASAX, buffer); return buffer[0]; } void AK8975::setAdjustmentX(uint8_t x) { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::writeByte(devAddr, AK8975_RA_ASAX, x); } uint8_t AK8975::getAdjustmentY() { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::readByte(devAddr, AK8975_RA_ASAY, buffer); return buffer[0]; } void AK8975::setAdjustmentY(uint8_t y) { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::writeByte(devAddr, AK8975_RA_ASAY, y); } uint8_t AK8975::getAdjustmentZ() { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::readByte(devAddr, AK8975_RA_ASAZ, buffer); return buffer[0]; } void AK8975::setAdjustmentZ(uint8_t z) { + I2Cdev::writeByte(devAddr, AK8975_RA_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); I2Cdev::writeByte(devAddr, AK8975_RA_ASAZ, z); -} \ No newline at end of file +} diff --git a/Arduino/AK8975/Examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino b/Arduino/AK8975/examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino similarity index 99% rename from Arduino/AK8975/Examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino rename to Arduino/AK8975/examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino index aef0e807..10a41ad4 100644 --- a/Arduino/AK8975/Examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino +++ b/Arduino/AK8975/examples/AK8975_MPUEVB_heading/AK8975_MPUEVB_heading.ino @@ -90,7 +90,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(mag.testConnection() ? "AK8975 connection successful" : "AK8975 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/AK8975/Examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino b/Arduino/AK8975/examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino similarity index 98% rename from Arduino/AK8975/Examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino rename to Arduino/AK8975/examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino index 630833ea..47f415bc 100644 --- a/Arduino/AK8975/Examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino +++ b/Arduino/AK8975/examples/AK8975_MPUEVB_raw/AK8975_MPUEVB_raw.ino @@ -89,7 +89,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(mag.testConnection() ? "AK8975 connection successful" : "AK8975 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/AK8975/Examples/AK8975_raw/AK8975_raw.ino b/Arduino/AK8975/examples/AK8975_raw/AK8975_raw.ino similarity index 98% rename from Arduino/AK8975/Examples/AK8975_raw/AK8975_raw.ino rename to Arduino/AK8975/examples/AK8975_raw/AK8975_raw.ino index 49a56af0..55a4d411 100644 --- a/Arduino/AK8975/Examples/AK8975_raw/AK8975_raw.ino +++ b/Arduino/AK8975/examples/AK8975_raw/AK8975_raw.ino @@ -68,7 +68,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(mag.testConnection() ? "AK8975 connection successful" : "AK8975 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/AK8975/library.json b/Arduino/AK8975/library.json new file mode 100644 index 00000000..ff39cdae --- /dev/null +++ b/Arduino/AK8975/library.json @@ -0,0 +1,19 @@ +{ + "name": "I2Cdevlib-AK8975", + "version": "1.0.0", + "keywords": "compass, sensor, i2cdevlib, i2c", + "description": "AK8975 is 3-axis electronic compass IC with high sensitive Hall sensor technology", + "include": "Arduino/AK8975", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*", + "jrowberg/I2Cdevlib-MPU6050": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/AT30TSE75x/AT30TSE75x.cpp b/Arduino/AT30TSE75x/AT30TSE75x.cpp new file mode 100644 index 00000000..acdfd4aa --- /dev/null +++ b/Arduino/AT30TSE75x/AT30TSE75x.cpp @@ -0,0 +1,630 @@ +// I2Cdev library collection - AT30TSE75x I2C device class +// Based on ATMEL AT30TSE75x datasheet, 2013 Rev. Atmel-8751F-DTS-AT30TSE752-754-758-Datasheet_092013 +// 2014-02-16 by Bartosz Kryza +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2014-02-16 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2014 Bartosz Kryza, 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 "AT30TSE75x.h" + +/** Default constructor, uses default I2C address. Assumes the device is + * AT30TSE752 and A2-A0 pins are connected to ground. + */ +AT30TSE75x::AT30TSE75x() { + devAddr = 0x00; + devType = AT30TSE75x_752; +} + +/** Specific address constructor. + * @param address 3 least significat bits of the address should be provided here + * depending on the A2-A0 connection + * @deviceType The type of device (AT30TSE75x_752, AT30TSE75x_754, AT30TSE75x_758) + */ +AT30TSE75x::AT30TSE75x(uint8_t address, uint8_t deviceType) { + // Here we only need the least significant 3 bits + devAddr = 0x07 & address; + devType = deviceType; +} + +/** Power on and prepare for general usage. + */ +void AT30TSE75x::initialize() { + // Nothing to do here +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool AT30TSE75x::testConnection() { + + if (I2Cdev::readWord(devAddr, AT30TSE75x_RA_TEMPERATURE, buffer) == 1) { + return true; + } + return false; + +} + +/** Get the current reading of temperature in Celcius degrees + * @return Current temperature measurement in Celcius degrees + */ +float AT30TSE75x::getTemperatureCelcius() { + + uint16_t rawTemperature = getTemperatureRaw(); + + float celciusTemperature = 0.0; + + if((rawTemperature & 0x8000) == 0) { + celciusTemperature = (rawTemperature>>8) + ((rawTemperature&0x00F0)>>4)*AT30TSE75x_STEP_12BIT; + } + else { + uint16_t twosComplement = (~rawTemperature) + 1; + celciusTemperature = - (twosComplement>>8) - ((twosComplement & 0x00F0)>>4)*AT30TSE75x_STEP_12BIT; + } + + return celciusTemperature; + +} + +/** Get the current reading of temperature in Fahrenheit degrees + * @return Current temperature measurement in Fahrenheit degrees + */ +float AT30TSE75x::getTemperatureFahrenheit() { + + return (getTemperatureCelcius()*9.0)/5.0 + 32.0; + +} + +/** Get the current contents of devices temperature register + * @return Current temperature register content + */ +uint16_t AT30TSE75x::getTemperatureRaw() { + + uint16_t temperatureValue = 0x0000; + I2Cdev::readWord(AT30TSE75x_ADDRESS_TEMP_SENSOR | devAddr, + AT30TSE75x_RA_TEMPERATURE, + &temperatureValue); + return temperatureValue; + +} + +/** Get the current configuration register content + * @return Current configuration register content + */ +uint16_t AT30TSE75x::getConfiguration() { + uint16_t config = 0x0000; + I2Cdev::readWord(AT30TSE75x_ADDRESS_TEMP_SENSOR | devAddr, AT30TSE75x_RA_CONFIGURATION, &config); + return config; +} + +/** Get the status of Non-volatile Busy flag (Read-only) + * @return Status of Non-volatile Busy flag + */ +bool AT30TSE75x::getNVRBusy() { + uint16_t config = getConfiguration(); + return (config>>AT30TSE75x_CONFIG_NVRBSY_BIT)&(0x0001); +} + +/** Get the status of shutdown mode flag + * @return Status of shutdown mode + */ +bool AT30TSE75x::getShutdownMode() { + uint16_t config = getConfiguration(); + return (config>>AT30TSE75x_CONFIG_SD_BIT)&(0x0001); +} + +/** Set the device shutdown mode + * @param true - shutdown active, false - normal operation + */ +void AT30TSE75x::setShutdownMode(bool shutdownMode) { + uint16_t config = getConfiguration()&(~(0x01<>AT30TSE75x_CONFIG_CMPINT_BIT)&(0x0001); +} + +/** Set the device shutdown mode + * @param false - comparator mode, true - interrupt mode + */ +void AT30TSE75x::setComparatorInterruptMode(bool comparatorOrInterrup) { + uint16_t config = getConfiguration()&(~(0x01<>AT30TSE75x_CONFIG_POL_BIT)&(0x0001); +} + +/** Set the alert polarity + * @param false - alert polarity low, true - alert polarity high + */ +void AT30TSE75x::setAlertPolarity(bool alertPolarity) { + uint16_t config = getConfiguration()&(~(0x01<>AT30TSE75x_CONFIG_FT_BIT)&(0x0003); +} + +/** Set the value of alert fault count + * @return Value of alert fault count + * @see AT30TSE75x_FAULT_COUNT_1 + * @see AT30TSE75x_FAULT_COUNT_2 + * @see AT30TSE75x_FAULT_COUNT_4 + * @see AT30TSE75x_FAULT_COUNT_6 + */ +void AT30TSE75x::setFaultToleranceQueue(uint8_t faultCount) { + uint16_t config = getConfiguration()&(~(0x03<>AT30TSE75x_CONFIG_RES_BIT)&(0x0003); +} + +/** Set the conversion resolution + * @param conversionResolution Value of the resolution + * @see AT30TSE75x_RES_9BIT + * @see AT30TSE75x_RES_10BIT + * @see AT30TSE75x_RES_11BIT + * @see AT30TSE75x_RES_12BIT + */ +void AT30TSE75x::setConversionResolution(uint8_t conversionResolution) { + uint16_t config = getConfiguration()&(~(0x03<>AT30TSE75x_CONFIG_FT_BIT)&(0x0001); +} + +/** Set the status of device one shot mode + * @param oneShotMode false - one shot mode disabled, true - one shot mode enabled + */ +void AT30TSE75x::setOneShotMode(bool oneShotMode) { + uint16_t config = getConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_NVCMPINT_BIT)&(0x0001); +} + +/** Set the status of comparator/interrupt mode in non-volatile configuration register + * @param comparatorOrInterrupt false - comparator mode, true - interrupt mode + */ +void AT30TSE75x::setNVComparatorInterruptMode(bool comparatorOrInterrupt) { + uint16_t config = getNVConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_RLCK_BIT)&(0x0001); +} + +/** Set the status of comparator/interrupt mode in non-volatile configuration register + * @param comparatorOrInterrupt false - comparator mode, true - interrupt mode + */ +void AT30TSE75x::setNVRegisterLock(bool registerLock) { + uint16_t config = getNVConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_RLCKDWN_BIT)&(0x0001); +} + +/** Set the status of comparator/interrupt mode in non-volatile configuration register + * Warning! Calling this method with true param disables any further modifications of any registers. + * @param registerLockdown false - registers lockdown reset, true - registers lockdown set + */ +void AT30TSE75x::setNVRegisterLockdown(bool registerLockdown) { + uint16_t config = getNVConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_NVSD_BIT)&(0x0001); +} + +/** Set the status of device shutdown mode from non-volatile configuration register. + * @param shutdownMode false - device shutdown unset, true - device shutdown set + */ +void AT30TSE75x::setNVShutdownMode(bool shutdownMode) { + uint16_t config = getNVConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_NVPOL_BIT)&(0x0001); +} + +/** Set the alert polarity from non-volatile configuration register. + * @param alertPolarity false - alert polarity low, true - alert polarity high + */ +void AT30TSE75x::setNVAlertPolarity(bool alertPolarity) { + uint16_t config = getNVConfiguration()&(~(0x01<>AT30TSE75x_NVCONFIG_NVFT_BIT)&(0x0003); +} + + +/** Set the value of fault tolerance from non-volatile configuration register. + * @param comparatorOrInterrupt false - comparator mode, true - interrupt mode + * @see AT30TSE75x_FAULT_COUNT_1 + * @see AT30TSE75x_FAULT_COUNT_2 + * @see AT30TSE75x_FAULT_COUNT_4 + * @see AT30TSE75x_FAULT_COUNT_6 + */ +void AT30TSE75x::setNVFaultToleranceQueue(uint8_t faultCount) { + uint16_t config = getNVConfiguration()&(~(0x03<>AT30TSE75x_NVCONFIG_NVR_BIT)&(0x0003); +} + +/** Set the conversion resolution from non-volatile configuration register + * @param conversionResolution Conversion resolution setting + * @see AT30TSE75x_RES_9BIT + * @see AT30TSE75x_RES_10BIT + * @see AT30TSE75x_RES_11BIT + * @see AT30TSE75x_RES_12BIT + */ +void AT30TSE75x::setNVConversionResolution(uint8_t conversionResolution) { + uint16_t config = getNVConfiguration()&(~(0x03<>8)&0x01)); + } + else { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ((devAddr&(~0x03))|((address>>8)&0x03)); + } + + byteAddress = 0x00FF&address; + + uint8_t result = 0x00; + + I2Cdev::readByte(eepromDeviceAddress, byteAddress, &result); + + return result; + +} + +/** Write single byte to EEPROM + * @param address Address of byte in EEPROM + * @param value Value to be stored at address + */ +void AT30TSE75x::writeEEPROMByte(uint16_t address, uint8_t value) { + + uint8_t eepromDeviceAddress = 0x00; + uint8_t byteAddress = 0x00; + + if(devType == AT30TSE75x_752) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | devAddr; + } + else if(devType == AT30TSE75x_754) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ( (devAddr&(~0x01))|((address>>8)&0x01) ); + } + else { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ( (devAddr&(~0x03))|((address>>8)&0x03) ); + } + + byteAddress = 0x00FF&address; + + I2Cdev::writeByte(eepromDeviceAddress, byteAddress, value); + +} + +/** Read entire page from EEPROM + * @param address Address of page in EEPROM (must be aligned to 0 byte in the requested page) + * @param value Pointer to where the data should be stored (must be at least 16 byte array) + */ +void AT30TSE75x::readEEPROMPage(uint16_t address, uint8_t* page) { + + uint8_t eepromDeviceAddress = 0x00; + uint8_t byteAddress = 0x00; + + if(devType == AT30TSE75x_752) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | devAddr; + } + else if(devType == AT30TSE75x_754) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ((devAddr&(~0x01))|((address>>8)&0x01)); + } + else { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ((devAddr&(~0x03))|((address>>8)&0x03)); + } + + byteAddress = 0x00FF&address; + + bool status = I2Cdev::readBytes(eepromDeviceAddress, byteAddress, 16, page); + +} + +/** Write entire page (16 bytes) to EEPROM + * After writing it is good to sleep for 5-10 ms as the bytes are buffered + * by the device before writing them to EEPROM. + * @param address Address of page in EEPROM (must be aligned to required pages' 0 byte) + * @param page Pointer to array of 16 bytes of data + */ +void AT30TSE75x::writeEEPROMPage(uint16_t address, uint8_t* page) { + + uint8_t eepromDeviceAddress = 0x00; + uint8_t byteAddress = 0x00; + + if(devType == AT30TSE75x_752) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | devAddr; + } + else if(devType == AT30TSE75x_754) { + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ((devAddr&(~0x01))|((address>>8)&0x01)); + } + else { + Serial.println("Writing EEPROM page"); + eepromDeviceAddress = AT30TSE75x_ADDRESS_SERIAL_EEPROM | ((devAddr&(~0x03))|((address>>8)&0x03)); + } + + byteAddress = 0x00FF&address; + + I2Cdev::writeBytes(eepromDeviceAddress, byteAddress, 16, page); + +} + + +/** Sets the reversible software write protection + * Please note that the A2-A0 pins must be set to A2=GND, A1=GND, A0=HV(7V~10V) + */ +void AT30TSE75x::setReversibleSoftwareWriteProtect() { + I2Cdev::writeByte(AT30TSE75x_ADDRESS_WRITE_PROTECT|0x1, 0x00, 0x00); +} + +/** Clears the reversible software write protection + * Please note that the A2-A0 pins must be set to A2=GND, A1=VCC, A0=HV(7V~10V) + */ +void AT30TSE75x::clearReversibleSoftwareWriteProtect() { + I2Cdev::writeByte(AT30TSE75x_ADDRESS_WRITE_PROTECT|0x3, 0x00, 0x00); +} + +/** Reset the configuration register and temperature alert limits to default values. + * It won't change the non-volatile register. + */ +void AT30TSE75x::reset() { + setShutdownMode(false); + setComparatorInterruptMode(false); + setAlertPolarity(false); + setFaultToleranceQueue(AT30TSE75x_FAULT_COUNT_1); + setConversionResolution(AT30TSE75x_RES_9BIT); + setOneShotMode(false); + setTempLowLimit(0x4B00); + setTempHighLimit(0x5000); +} + diff --git a/Arduino/AT30TSE75x/AT30TSE75x.h b/Arduino/AT30TSE75x/AT30TSE75x.h new file mode 100644 index 00000000..db0f1a79 --- /dev/null +++ b/Arduino/AT30TSE75x/AT30TSE75x.h @@ -0,0 +1,272 @@ +// I2Cdev library collection - AT30TSE752/754/758 I2C device class header file +// Based on ATMEL AT30TSE75 datasheet, 2013 Rev. Atmel-8751F-DTS-AT30TSE752-754-758-Datasheet_092013 +// 2014-02-16 by Bartosz Kryza +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2014-02-16 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2014 Bartosz Kryza, 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 _AT30TSE75x_H_ +#define _AT30TSE75x_H_ + +#include "I2Cdev.h" + + +/** @defgroup devices Defines to distinguish between different AT30TSE752/754/758 variants + * @{ + */ +#define AT30TSE75x_752 1 /*!< ID of x752 variant (2Kb EEPROM) */ +#define AT30TSE75x_754 2 /*!< ID of x754 variant (4Kb EEPROM) */ +#define AT30TSE75x_758 3 /*!< ID of x758 variant (8Kb EEPROM) */ +/** @} */ + +/** @defgroup address Device addresses + * AT30TSE75 has 3 distinct subdevices with different I2C addresses: + * AT30TSE75x consist of 3 devices: + * - Temperature sensor + * - Serial EEPROM + * - Software Write Protection + * each with a separate I2C address. The 3 least significant bits + * of the 7-bit address are specified by wiring pins A2,A1,A0 to GND or VCC. + * The 4 most significant bits are predefined for each subdevice as below: + * @{ + */ +#define AT30TSE75x_ADDRESS_TEMP_SENSOR (0x9<<3) /*!< The 4 MSB of the 7-bit Temperature Sensor subdevice address */ +#define AT30TSE75x_ADDRESS_SERIAL_EEPROM (0xA<<3) /*!< The 4 MSB of the 7-bit Serial EEPROM subdevice address */ +#define AT30TSE75x_ADDRESS_WRITE_PROTECT (0x6<<3) /*!< The 4 MSB of the 7-bit Software Write Protect subdevice address */ + +#define AT30TSE75x_DEFAULT_ADDRESS 0x00 /*!< Assumes that the A2-A0 pins are grounded. */ +/** @} */ + + +/** @defgroup register Register addresses + * @{ + */ +#define AT30TSE75x_RA_TEMPERATURE 0x00 /*!< Address of Temperature register */ +#define AT30TSE75x_RA_CONFIGURATION 0x01 /*!< Address of Configuration register */ +#define AT30TSE75x_RA_TLOW_LIMIT 0x02 /*!< Address of alert temperature low limit */ +#define AT30TSE75x_RA_THIGH_LIMIT 0x03 /*!< Address of alert temperature high limit */ +#define AT30TSE75x_RA_NV_CONFIGURATION 0x11 /*!< Address of Non-volatile Configuration register */ +#define AT30TSE75x_RA_NV_TLOW_LIMIT 0x12 /*!< Address of Non-volatile alert temperature low limit */ +#define AT30TSE75x_RA_NV_THIGH_LIMIT 0x13 /*!< Address of Non-volatile alert temperature high limit */ +/** @} */ + + +/** @defgroup cfgstruct Configuration Register Structure + * @{ + */ +#define AT30TSE75x_CONFIG_NVRBSY_BIT 0 /*!< Configuration register Non-volatile Register Busy (NVRBSY) bit offset */ + +#define AT30TSE75x_CONFIG_RFU_BIT 1 /*!< Configuration register reserved bits segment offset (7 bits) */ +#define AT30TSE75x_CONFIG_RFU_LENGTH 7 + +#define AT30TSE75x_CONFIG_SD_BIT 8 /*!< Configuration register Shutdown mode (SD) bit offset */ + +#define AT30TSE75x_CONFIG_CMPINT_BIT 9 /*!< Configuration register ~Comparator/Interrupt mode bit offset */ + +#define AT30TSE75x_CONFIG_POL_BIT 10 /*!< Configuration register Alert polarity (POL) bit offset */ + +#define AT30TSE75x_CONFIG_FT_BIT 11 /*!< Configuration register alert fault count (FT) value (2 bits) offset */ +#define AT30TSE75x_CONFIG_FT_LENGTH 2 + +#define AT30TSE75x_CONFIG_RES_BIT 13 /*!< Configuration register conversion resolution (RES) value (2 bits) offset */ +#define AT30TSE75x_CONFIG_RES_LENGTH 2 + +#define AT30TSE75x_CONFIG_OS_BIT 15 /*!< Configuration register one shot mode bit offset */ +/** @} */ + + + +/** @defgroup nvcfgstruct Non-volatile Configuration Register Structure + * @{ + */ +#define AT30TSE75x_NVCONFIG_RFU_BIT 0 /*!< Non-volatile configuration register reserved bit offset */ + +#define AT30TSE75x_NVCONFIG_RLCK_BIT 1 /*!< Non-volatile configuration register register lock (RLCK) it offset */ + +#define AT30TSE75x_NVCONFIG_RLCKDWN_BIT 2 /*!< Non-volatile configuration register register lockdown (RLCKDWN) bit offset */ + +#define AT30TSE75x_NVCONFIG_RFU2_BIT 3 /*!< Non-volatile configuration register reserved bit segment offset (4 bits) */ +#define AT30TSE75x_NVCONFIG_RFU2_LENGTH 4 + +#define AT30TSE75x_NVCONFIG_NVSD_BIT 8 /*!< Non-volatile configuration register shutdown (NVSD) bit offset */ + +#define AT30TSE75x_NVCONFIG_NVCMPINT_BIT 9 /*!< Non-volatile configuration register ~comparator/interrupt (NVCMPINT) bit offset */ + +#define AT30TSE75x_NVCONFIG_NVPOL_BIT 10 /*!< Non-volatile configuration register alert polarity (NVPOL) bit offset */ + +#define AT30TSE75x_NVCONFIG_NVFT_BIT 11 /*!< Non-volatile configuration register alert fault count (NVFT) value offset (2 bits) */ +#define AT30TSE75x_NVCONFIG_NVFT_LENGTH 2 + +#define AT30TSE75x_NVCONFIG_NVR_BIT 13 /*!< Non-volatile configuration register resolution (NVR) value offset (2 bits) */ +#define AT30TSE75x_NVCONFIG_NVR_LENGTH 2 +/** @} */ + + + +/** @defgroup resolution Conversion resolution parameters + * Conversion resolution (9, 10, 11, or 12-bits) impacts both the resolution + * step (from 0.0625C to 0.5C) and conversion time (25ms, 50ms, 100ms, 200ms). + * @{ + */ +#define AT30TSE75x_RES_9BIT 0x00 /*!< 9-bit resolution configuration register value */ +#define AT30TSE75x_RES_10BIT 0x01 /*!< 10-bit resolution configuration register value */ +#define AT30TSE75x_RES_11BIT 0x02 /*!< 11-bit resolution configuration register value */ +#define AT30TSE75x_RES_12BIT 0x03 /*!< 12-bit resolution configuration register value */ + +#define AT30TSE75x_STEP_9BIT 0.5000 /*!< 9-bit resolution step */ +#define AT30TSE75x_STEP_10BIT 0.2500 /*!< 10-bit resolution step */ +#define AT30TSE75x_STEP_11BIT 0.1250 /*!< 11-bit resolution step */ +#define AT30TSE75x_STEP_12BIT 0.0625 /*!< 12-bit resolution step */ + +#define AT30TSE75x_CONVTIME_MS_9BIT 25 /*!< 9-bit resolution conversion time (ms) */ +#define AT30TSE75x_CONVTIME_MS_10BIT 50 /*!< 10-bit resolution conversion time (ms) */ +#define AT30TSE75x_CONVTIME_MS_11BIT 100 /*!< 11-bit resolution conversion time (ms) */ +#define AT30TSE75x_CONVTIME_MS_12BIT 200 /*!< 12-bit resolution conversion time (ms) */ +/** @} */ + +/** @defgroup resolution Conversion resolution parameters + * Conversion resolution (9, 10, 11, or 12-bits) impacts both the resolution + * step (from 0.0625C to 0.5C) and conversion time (25ms, 50ms, 100ms, 200ms). + * @{ + */ +#define AT30TSE75x_FAULT_COUNT_1 0x00 /*!< Alert fault count value - 1 fault */ +#define AT30TSE75x_FAULT_COUNT_2 0x01 /*!< Alert fault count value - 2 consecutive faults */ +#define AT30TSE75x_FAULT_COUNT_4 0x02 /*!< Alert fault count value - 4 consecutive faults */ +#define AT30TSE75x_FAULT_COUNT_6 0x03 /*!< Alert fault count value - 6 consecutive faults */ +/** @} */ + + + +class AT30TSE75x { + public: + + AT30TSE75x(); + + AT30TSE75x(uint8_t address, uint8_t deviceType); + + void initialize(); + bool testConnection(); + + + float getTemperatureCelcius(); + float getTemperatureFahrenheit(); + uint16_t getTemperatureRaw(); + + + // CONFIGURATION register + uint16_t getConfiguration(); + + bool getNVRBusy(); + + bool getShutdownMode(); + void setShutdownMode(bool shutdownMode); + + bool getComparatorInterruptMode(); + void setComparatorInterruptMode(bool comparatorOrInterrup); + + bool getAlertPolarity(); + void setAlertPolarity(bool alertPolarity); + + uint8_t getFaultToleranceQueue(); + void setFaultToleranceQueue(uint8_t faultCount); + + uint8_t getConversionResolution(); + void setConversionResolution(uint8_t conversionResolution); + + bool getOneShotMode(); + void setOneShotMode(bool oneShotMode); + + + // Temperature alert low limit register + void setTempLowLimit(uint16_t lowLimit); + uint16_t getTempLowLimit(); + + + // Temperature alert high limit register + void setTempHighLimit(uint16_t highLimit); + uint16_t getTempHighLimit(); + + + // Non-volatile Configuration register + uint16_t getNVConfiguration(); + + bool getNVComparatorInterruptMode(); + void setNVComparatorInterruptMode(bool comparatorOrInterrup); + + bool getNVRegisterLock(); + void setNVRegisterLock(bool registerLock); + + bool getNVRegisterLockdown(); + void setNVRegisterLockdown(bool registerLockdown); + + bool getNVShutdownMode(); + void setNVShutdownMode(bool shutdownMode); + + bool getNVAlertPolarity(); + void setNVAlertPolarity(bool alertPolarity); + + uint8_t getNVFaultToleranceQueue(); + void setNVFaultToleranceQueue(uint8_t faultCount); + + uint8_t geNVConversionResolution(); + void setNVConversionResolution(uint8_t conversionResolution); + + + // Temperature alert low limit non-volatile register + void setNVTempLowLimit(uint16_t lowLimit); + uint16_t getNVTempLowLimit(); + + + // Temperature alert high limit non-volatile register + void setNVTempHighLimit(uint16_t highLimit); + uint16_t getNVTempHighLimit(); + + + // EEPROM + uint8_t readEEPROMByte(uint16_t address); + void writeEEPROMByte(uint16_t address, uint8_t value); + + void readEEPROMPage(uint16_t address, uint8_t* page); + void writeEEPROMPage(uint16_t address, uint8_t* page); + + + // Software write protect + void setReversibleSoftwareWriteProtect(); + void clearReversibleSoftwareWriteProtect(); + + // Reset Non-volatile config to default values + void reset(); + + + private: + uint8_t devAddr; + uint8_t devType; + uint16_t buffer[6]; +}; + +#endif /* _AT30TSE75x_H_ */ diff --git a/Arduino/AT30TSE75x/Examples/AT30TSE75x_basic/AT30TSE75x_basic.ino b/Arduino/AT30TSE75x/Examples/AT30TSE75x_basic/AT30TSE75x_basic.ino new file mode 100644 index 00000000..d9480606 --- /dev/null +++ b/Arduino/AT30TSE75x/Examples/AT30TSE75x_basic/AT30TSE75x_basic.ino @@ -0,0 +1,113 @@ +// I2Cdev library collection - AT30TSE752/754/758 I2C device class header file +// Based on ATMEL AT30TSE75 datasheet, 2013 Rev. Atmel-8751F-DTS-AT30TSE752-754-758-Datasheet_092013 +// 2014-02-16 by Bartosz Kryza +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2014-02-16 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2014 Bartosz Kryza, 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 +#include "AT30TSE75x.h" + +// +// Initialize the sensor with A2-A0 pins grounded +// and variant with 8Kb of EEPROM memory. +// +AT30TSE75x tempSensor(0x00, AT30TSE75x_758); + +void setup() { + + // + // Initialize Wire and Serial + // + Wire.begin(); + Serial.begin(9600); + Serial.println("Initializing temp sensor"); + + // + // Initialize the sensor with 12 bit conversion resolution + // + tempSensor.initialize(); + tempSensor.setConversionResolution(AT30TSE75x_RES_12BIT); + +} + + + +void loop() { + + // + // Read Celcius temperature + // + float tempValue = tempSensor.getTemperatureCelcius(); + Serial.print("Current temperature: "); + Serial.print(tempValue); + Serial.println("C"); + + // + // Read Fahrenheit temperature + // + tempValue = tempSensor.getTemperatureFahrenheit(); + Serial.print("Current temperature: "); + Serial.print(tempValue); + Serial.println("F"); + + // + // Write/read EEPROM byte + // + uint8_t page = 16; + uint8_t byteInPage = 5; + tempSensor.writeEEPROMByte(16*page+byteInPage, 13); + uint8_t eepromValue = tempSensor.readEEPROMByte(16*page+byteInPage); + Serial.print("Read EEPROM byte: "); + Serial.println(eepromValue); + + // + // Write/read EEPROM page + // + uint8_t pageBytes[16] = {1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16}; + page = 3; + tempSensor.writeEEPROMPage(16*page, pageBytes); + + // AT30TSE buffers pages before writing them to the EEPROM thus + // delay is necessary between writing and reading a page from EEPROM + // to ensure we get new values + delay(10); + + uint8_t pageBytesRead[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; + + tempSensor.readEEPROMPage(16*page, pageBytesRead); + + Serial.print("Read EEPROM page: "); + for(int i=0; i<16; i++) { + Serial.print(pageBytesRead[i]); Serial.print(" "); + } + Serial.println(""); + + delay(2000); + +} diff --git a/Arduino/BMA150/examples/BMA150_raw/BMA150_raw.ino b/Arduino/BMA150/examples/BMA150_raw/BMA150_raw.ino index 23fb931d..c5151c91 100644 --- a/Arduino/BMA150/examples/BMA150_raw/BMA150_raw.ino +++ b/Arduino/BMA150/examples/BMA150_raw/BMA150_raw.ino @@ -64,7 +64,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(accel.testConnection() ? "BMA150 connection successful" : "BMA150 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/BMA150/library.json b/Arduino/BMA150/library.json new file mode 100644 index 00000000..693e1a53 --- /dev/null +++ b/Arduino/BMA150/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-BMA150", + "version": "1.0.0", + "keywords": "accelerometer, sensor, i2cdevlib, i2c", + "description": "The BMA150 is a triaxial, low-g acceleration sensor IC with digital output for consumer market applications", + "include": "Arduino/BMA150", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/BMP085/BMP085.cpp b/Arduino/BMP085/BMP085.cpp index 2119ae1a..f186d74f 100644 --- a/Arduino/BMP085/BMP085.cpp +++ b/Arduino/BMP085/BMP085.cpp @@ -31,6 +31,7 @@ THE SOFTWARE. */ #include "BMP085.h" +#include /** * Default constructor, uses default I2C device address. @@ -166,10 +167,14 @@ void BMP085::setControl(uint8_t value) { /* measurement register methods */ uint16_t BMP085::getMeasurement2() { + // wait for end of conversion + while(getControl() & 0x20); I2Cdev::readBytes(devAddr, BMP085_RA_MSB, 2, buffer); return ((uint16_t)buffer[0] << 8) + buffer[1]; } uint32_t BMP085::getMeasurement3() { + // wait for end of conversion + while(getControl() & 0x20); I2Cdev::readBytes(devAddr, BMP085_RA_MSB, 3, buffer); return ((uint32_t)buffer[0] << 16) + ((uint16_t)buffer[1] << 8) + buffer[2]; } @@ -207,6 +212,7 @@ float BMP085::getTemperatureC() { T = (B5 + 8) / 2^4 */ int32_t ut = getRawTemperature(); + if(ut == 0) return NAN; int32_t x1 = ((ut - (int32_t)ac6) * (int32_t)ac5) >> 15; int32_t x2 = ((int32_t)mc << 11) / (x1 + md); b5 = x1 + x2; @@ -222,7 +228,7 @@ uint32_t BMP085::getRawPressure() { return 0; // wrong measurement mode for pressure request } -float BMP085::getPressure() { +int32_t BMP085::getPressure() { /* Datasheet forumla UP = raw pressure @@ -244,6 +250,7 @@ float BMP085::getPressure() { p = p + (X1 + X2 + 3791) / 2^4 */ uint32_t up = getRawPressure(); + if(up == 0) return 0; uint8_t oss = (measureMode & 0xC0) >> 6; int32_t p; int32_t b6 = b5 - 4000; @@ -269,4 +276,4 @@ float BMP085::getPressure() { float BMP085::getAltitude(float pressure, float seaLevelPressure) { return 44330 * (1.0 - pow(pressure / seaLevelPressure, 0.1903)); -} \ No newline at end of file +} diff --git a/Arduino/BMP085/BMP085.h b/Arduino/BMP085/BMP085.h index 6b5e09a7..b5d5a6a5 100644 --- a/Arduino/BMP085/BMP085.h +++ b/Arduino/BMP085/BMP085.h @@ -34,6 +34,7 @@ THE SOFTWARE. #define _BMP085_H_ #include "I2Cdev.h" +#include #define BMP085_ADDRESS 0x77 #define BMP085_DEFAULT_ADDRESS BMP085_ADDRESS @@ -75,10 +76,11 @@ class BMP085 { public: BMP085(); BMP085(uint8_t address); - + void initialize(); bool testConnection(); +#ifdef BMP085_INCLUDE_INDIVIDUAL_CALIBRATION_ACCESS /* calibration register methods */ int16_t getAC1(); int16_t getAC2(); @@ -91,6 +93,7 @@ class BMP085 { int16_t getMB(); int16_t getMC(); int16_t getMD(); +#endif /* CONTROL register methods */ uint8_t getControl(); @@ -108,12 +111,12 @@ class BMP085 { float getTemperatureC(); float getTemperatureF(); uint32_t getRawPressure(); - float getPressure(); + int32_t getPressure(); float getAltitude(float pressure, float seaLevelPressure=101325); private: uint8_t devAddr; - uint8_t buffer[2]; + uint8_t buffer[3]; bool calibrationLoaded; int16_t ac1, ac2, ac3, b1, b2, mb, mc, md; diff --git a/Arduino/BMP085/Examples/BMP085_basic/BMP085_basic.ino b/Arduino/BMP085/examples/BMP085_basic/BMP085_basic.ino similarity index 93% rename from Arduino/BMP085/Examples/BMP085_basic/BMP085_basic.ino rename to Arduino/BMP085/examples/BMP085_basic/BMP085_basic.ino index 0e9633eb..c5f8ce8c 100644 --- a/Arduino/BMP085/Examples/BMP085_basic/BMP085_basic.ino +++ b/Arduino/BMP085/examples/BMP085_basic/BMP085_basic.ino @@ -46,8 +46,7 @@ BMP085 barometer; float temperature; float pressure; -float altitude; -int32_t lastMicros; +int32_t altitude; #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) bool blinkState = false; @@ -77,16 +76,11 @@ void loop() { // request temperature barometer.setControl(BMP085_MODE_TEMPERATURE); - // wait appropriate time for conversion (4.5ms delay) - lastMicros = micros(); - while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds()); - // read calibrated temperature value in degrees Celsius temperature = barometer.getTemperatureC(); // request pressure (3x oversampling mode, high detail, 23.5ms delay) barometer.setControl(BMP085_MODE_PRESSURE_3); - while (micros() - lastMicros < barometer.getMeasureDelayMicroseconds()); // read calibrated pressure value in Pascals (Pa) pressure = barometer.getPressure(); diff --git a/Arduino/BMP085/library.json b/Arduino/BMP085/library.json new file mode 100644 index 00000000..9d7d4901 --- /dev/null +++ b/Arduino/BMP085/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-BMP085", + "version": "1.0.0", + "keywords": "altimeter, altitude, barometer, pressure, temperature, sensor, i2cdevlib, i2c", + "description": "The BMP085 is barometric pressure, temperature and altitude sensor", + "include": "Arduino/BMP085", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/DS1307/DS1307.cpp b/Arduino/DS1307/DS1307.cpp index 7e125c58..c1cfc88a 100644 --- a/Arduino/DS1307/DS1307.cpp +++ b/Arduino/DS1307/DS1307.cpp @@ -360,7 +360,7 @@ void DS1307::setDateTime24(uint16_t year, uint8_t month, uint8_t day, uint8_t ho //////////////////////////////////////////////////////////////////////////////// // utility code, some of this could be exposed in the DateTime API if needed - static uint8_t daysInMonth [] PROGMEM = { 31,28,31,30,31,30,31,31,30,31,30,31 }; + static const uint8_t daysInMonth [] PROGMEM = { 31,28,31,30,31,30,31,31,30,31,30,31 }; // number of days since 2000/01/01, valid for 2001..2099 static uint16_t date2days(uint16_t y, uint8_t m, uint8_t d) { @@ -435,7 +435,7 @@ void DS1307::setDateTime24(uint16_t year, uint8_t month, uint8_t day, uint8_t ho yOff = conv2d(date + 9); // Jan Feb Mar Apr May Jun Jul Aug Sep Oct Nov Dec switch (date[0]) { - case 'J': m = date[1] == 'a' ? 1 : m = date[2] == 'n' ? 6 : 7; break; + case 'J': m = (date[1] == 'a' ? 1 : (date[2] == 'n' ? 6 : 7)); break; case 'F': m = 2; break; case 'A': m = date[2] == 'r' ? 4 : 8; break; case 'M': m = date[2] == 'r' ? 3 : 5; break; @@ -456,11 +456,13 @@ void DS1307::setDateTime24(uint16_t year, uint8_t month, uint8_t day, uint8_t ho } uint32_t DateTime::unixtime(void) const { + return secondstime() + SECONDS_FROM_1970_TO_2000; + } + + long DateTime::secondstime(void) const { uint32_t t; uint16_t days = date2days(yOff, m, d); - t = time2long(days, hh, mm, ss); - t += SECONDS_FROM_1970_TO_2000; // seconds from 1970 to 2000 - + t = time2long(days, hh, mm, ss); return t; } #endif diff --git a/Arduino/DS1307/examples/DS1307_settime/DS1307_settime.ino b/Arduino/DS1307/examples/DS1307_settime/DS1307_settime.ino new file mode 100644 index 00000000..7655a90c --- /dev/null +++ b/Arduino/DS1307/examples/DS1307_settime/DS1307_settime.ino @@ -0,0 +1,96 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for DS1307 class +// +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// I2C Device Library hosted at http://www.i2cdevlib.com +// +// Changelog: +// 2016-04-15 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2016 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 and DS1307 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "DS1307.h" + + +DS1307 rtc; + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) + // Note that the DS1307 doesn't support 400Khz i2c + Wire.begin(); + + // initialize serial communication + // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but + // it's really up to you depending on your project) + Serial.begin(38400); + + // initialize device + Serial.println("Initializing I2C devices..."); + rtc.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(rtc.testConnection() ? "DS1307 connection successful" : "DS1307 connection failed"); + + + // Use "the compiler's time" to set time. Make sure the sketch is recompiled before uploading. + DateTime compilerNow (__DATE__, __TIME__); + rtc.setDateTime(compilerNow); + rtc.setClockRunning(true); + Serial.print(" Clock is set "); + Serial.println(rtc.getClockRunning()?"and running":"but it is NOT running"); +} + +void printDigits(int digits, const char* prefix="") { + // utility function: prints prefix and leading 0 before the number + Serial.print(prefix); + if (digits < 10) + Serial.print('0'); + Serial.print(digits); +} + +void loop() { + // ISO 8601 FTW! + + uint16_t year = 0; + uint8_t month, day, dow, hours, minutes, seconds = 0; + static uint32_t tick = 0; + + rtc.getDateTime24(&year, &month, &day, &hours, &minutes, &seconds); + Serial.print(tick++); + printDigits(year, ": "); + printDigits(month,"-"); + printDigits(day,"-"); + printDigits(hours," "); + printDigits(minutes,":"); + printDigits(seconds, ":"); + + Serial.println(); + delay(1000); +} + + + diff --git a/Arduino/DS1307/Examples/DS1307_tick/DS1307_tick.ino b/Arduino/DS1307/examples/DS1307_tick/DS1307_tick.ino similarity index 100% rename from Arduino/DS1307/Examples/DS1307_tick/DS1307_tick.ino rename to Arduino/DS1307/examples/DS1307_tick/DS1307_tick.ino diff --git a/Arduino/DS1307/library.json b/Arduino/DS1307/library.json new file mode 100644 index 00000000..4a37b19c --- /dev/null +++ b/Arduino/DS1307/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-DS1307", + "version": "1.0.0", + "keywords": "rtc, time, clock, i2cdevlib, i2c", + "description": "The DS1307 serial real-time clock (RTC) is a low-power, full binary-coded decimal (BCD) clock/calendar plus 56 bytes of NV SRAM", + "include": "Arduino/DS1307", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/HMC5843/HMC5843.cpp b/Arduino/HMC5843/HMC5843.cpp index f1ff3a8d..a31ea5aa 100644 --- a/Arduino/HMC5843/HMC5843.cpp +++ b/Arduino/HMC5843/HMC5843.cpp @@ -227,7 +227,7 @@ void HMC5843::setMode(uint8_t newMode) { // use this method to guarantee that bits 7-2 are set to zero, which is a // requirement specified in the datasheet; it's actually more efficient than // using the I2Cdev.writeBits method - I2Cdev::writeByte(devAddr, HMC5843_RA_MODE, mode << (HMC5843_MODEREG_BIT - HMC5843_MODEREG_LENGTH + 1)); + I2Cdev::writeByte(devAddr, HMC5843_RA_MODE, newMode << (HMC5843_MODEREG_BIT - HMC5843_MODEREG_LENGTH + 1)); mode = newMode; // track to tell if we have to clear bit 7 after a read } diff --git a/Arduino/HMC5843/Examples/HMC5843_raw/HMC5843_raw.ino b/Arduino/HMC5843/examples/HMC5843_raw/HMC5843_raw.ino similarity index 98% rename from Arduino/HMC5843/Examples/HMC5843_raw/HMC5843_raw.ino rename to Arduino/HMC5843/examples/HMC5843_raw/HMC5843_raw.ino index 7c45f108..e222c822 100644 --- a/Arduino/HMC5843/Examples/HMC5843_raw/HMC5843_raw.ino +++ b/Arduino/HMC5843/examples/HMC5843_raw/HMC5843_raw.ino @@ -65,7 +65,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(mag.testConnection() ? "HMC5843 connection successful" : "HMC5843 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/HMC5843/library.json b/Arduino/HMC5843/library.json new file mode 100644 index 00000000..04feb72d --- /dev/null +++ b/Arduino/HMC5843/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-HMC5843", + "version": "1.0.0", + "keywords": "magnetometer, compass, sensor, i2cdevlib, i2c", + "description": "The HMC5843 is 3-Axis digital compass/magnetometer", + "include": "Arduino/HMC5843", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/HMC5883L/HMC5883L.cpp b/Arduino/HMC5883L/HMC5883L.cpp index 7fad0462..29d8fb56 100644 --- a/Arduino/HMC5883L/HMC5883L.cpp +++ b/Arduino/HMC5883L/HMC5883L.cpp @@ -250,7 +250,7 @@ void HMC5883L::setMode(uint8_t newMode) { // use this method to guarantee that bits 7-2 are set to zero, which is a // requirement specified in the datasheet; it's actually more efficient than // using the I2Cdev.writeBits method - I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, mode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); + I2Cdev::writeByte(devAddr, HMC5883L_RA_MODE, newMode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1)); mode = newMode; // track to tell if we have to clear bit 7 after a read } diff --git a/Arduino/HMC5883L/Examples/HMC5883L_raw/HMC5883L_raw.ino b/Arduino/HMC5883L/examples/HMC5883L_raw/HMC5883L_raw.ino similarity index 98% rename from Arduino/HMC5883L/Examples/HMC5883L_raw/HMC5883L_raw.ino rename to Arduino/HMC5883L/examples/HMC5883L_raw/HMC5883L_raw.ino index 0d346067..0ab1b152 100644 --- a/Arduino/HMC5883L/Examples/HMC5883L_raw/HMC5883L_raw.ino +++ b/Arduino/HMC5883L/examples/HMC5883L_raw/HMC5883L_raw.ino @@ -66,7 +66,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(mag.testConnection() ? "HMC5883L connection successful" : "HMC5883L connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/HMC5883L/library.json b/Arduino/HMC5883L/library.json new file mode 100644 index 00000000..abe9cd9a --- /dev/null +++ b/Arduino/HMC5883L/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-HMC5883L", + "version": "1.0.0", + "keywords": "magnetometer, compass, sensor, i2cdevlib, i2c", + "description": "The HMC5883L is 3-Axis digital compass/magnetometer", + "include": "Arduino/HMC5883L", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/HTU21D/HTU21D.cpp b/Arduino/HTU21D/HTU21D.cpp new file mode 100644 index 00000000..838c7fdd --- /dev/null +++ b/Arduino/HTU21D/HTU21D.cpp @@ -0,0 +1,97 @@ +// I2Cdev library collection - HTU21D I2C device class header file +// Based on MEAS HTU21D HPC199_2 HTU321(F) datasheet, October 2013 +// 2016-03-24 by https://github.com/eadf +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-03-24 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2016 Eadf, 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 "HTU21D.h" + + +/** Default constructor, uses default I2C address. + * @see HTU21D_DEFAULT_ADDRESS + */ +HTU21D::HTU21D() { + devAddr = HTU21D_DEFAULT_ADDRESS; +} + +/** Power on and prepare for general usage. + * This operation calls reset() on the HTU21D device and it takes at least 15milliseconds. + */ +void HTU21D::initialize() { + reset(); +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * This operation calls reset() on the HTU21D device and it takes at least 15milliseconds. + * @return True if connection is valid, false otherwise + */ +bool HTU21D::testConnection() { + reset(); + buffer[0] = 0; + I2Cdev::readByte(devAddr, HTU21D_READ_USER_REGISTER, buffer); + return buffer[0] == 0x2; +} + +/** Reads and returns the temperature, ignores the CRC field. + * @return The measured temperature, or NaN if the operation failed. + */ +float HTU21D::getTemperature() { + // Ignore the CRC byte + uint16_t t = 0; + if (1!=I2Cdev::readWord(devAddr, HTU21D_RA_TEMPERATURE, &t)){ + return NAN; + } + // clear the status bits (bit0 & bit1) and calculate the temperature + // as per the formula in the datasheet + return ((float)(t&0xFFFC))*175.72/65536.0-46.85; +} + +/** Reads and returns the humidity, ignores the CRC field + * @return The measured humidity, or NaN if the operation failed. + */ +float HTU21D::getHumidity() { + // Ignore the CRC byte + uint16_t t = 0; + if (1!=I2Cdev::readWord(devAddr, HTU21D_RA_HUMIDITY, &t)){ + return NAN; + } + // clear the status bits (bit0 & bit1) and calculate the humidity + // as per the formula in the datasheet + return ((float)(t&0xFFFC))*125.0/65536.0-6.0; +} + +/** Does a soft reset of the HTU21D + * This operation takes at least 15milliseconds. + */ +void HTU21D::reset() { + I2Cdev::writeByte(devAddr, HTU21D_RESET, 0); + delay(15); +} + diff --git a/Arduino/HTU21D/HTU21D.h b/Arduino/HTU21D/HTU21D.h new file mode 100644 index 00000000..a5e277eb --- /dev/null +++ b/Arduino/HTU21D/HTU21D.h @@ -0,0 +1,63 @@ +// I2Cdev library collection - HTU21D I2C device class header file +// Based on MEAS HTU21D HPC199_2 HTU321(F) datasheet, October 2013 +// 2016-03-24 by https://github.com/eadf +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-03-24 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2016 Eadf, 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 _HTU21D_H_ +#define _HTU21D_H_ + +#include "I2Cdev.h" + +#define HTU21D_DEFAULT_ADDRESS 0x40 + +#define HTU21D_RA_TEMPERATURE 0xE3 +#define HTU21D_RA_HUMIDITY 0xE5 +#define HTU21D_RESET 0xFE +#define HTU21D_WRITE_USER_REGISTER 0xE6 +#define HTU21D_READ_USER_REGISTER 0xE7 + +class HTU21D { + public: + HTU21D(); + + void initialize(); + bool testConnection(); + + float getTemperature(); + float getHumidity(); + + void reset(); + + private: + uint8_t devAddr; + uint8_t buffer[2]; +}; + +#endif /* _HTU21D_H_ */ diff --git a/Arduino/HTU21D/examples/HTU21D_simple/HTU21D_simple.ino b/Arduino/HTU21D/examples/HTU21D_simple/HTU21D_simple.ino new file mode 100644 index 00000000..0b198fb4 --- /dev/null +++ b/Arduino/HTU21D/examples/HTU21D_simple/HTU21D_simple.ino @@ -0,0 +1,51 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for HTU21D class +// Example of reading temperature and humidity from the HTU21D sensor +// 2016-03-24 by Eadf (https://github.com/eadf) +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-03-24 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2016 Eadf, 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 "HTU21D.h" + +HTU21D htu21d; + +void setup() { + //I2Cdev::begin(); // join I2C bus + Wire.begin(); + Serial.begin(38400); + htu21d.initialize(); + Serial.println("Testing device connections..."); + Serial.println(htu21d.testConnection() ? "HTU21D connection successful" : "HTU21D connection failed"); +} + +void loop() { + Serial.print("Temperature: "); Serial.print(htu21d.getTemperature()); + Serial.print("\t\tHumidity: "); Serial.println(htu21d.getHumidity()); + delay(400); +} + diff --git a/Arduino/HTU21D/library.json b/Arduino/HTU21D/library.json new file mode 100644 index 00000000..f85f0d23 --- /dev/null +++ b/Arduino/HTU21D/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-HTU21D", + "version": "1.0.0", + "keywords": "temperature, humidity, i2cdevlib, i2c", + "description": "HTU21D is 12-Bit humidity and 14-bit temperature sensor", + "include": "Arduino/HTU21D", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/I2Cdev/I2Cdev.cpp b/Arduino/I2Cdev/I2Cdev.cpp index c576c088..40b5dfc4 100644 --- a/Arduino/I2Cdev/I2Cdev.cpp +++ b/Arduino/I2Cdev/I2Cdev.cpp @@ -1,8 +1,11 @@ // I2Cdev library collection - Main I2C device class // Abstracts bit and byte I2C R/W functions into a convenient class -// 6/9/2012 by Jeff Rowberg +// 2013-06-05 by Jeff Rowberg // // Changelog: +// 2021-09-28 - allow custom Wire object as transaction function argument +// 2020-01-20 - hardija : complete support for Teensy 3.x +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire @@ -45,26 +48,26 @@ THE SOFTWARE. #include "I2Cdev.h" -#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE #ifdef I2CDEV_IMPLEMENTATION_WARNINGS #if ARDUINO < 100 #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. #warning This I2Cdev implementation does not support: #warning - Repeated starts conditions #warning - Timeout detection (some Wire requests block forever) #elif ARDUINO == 100 #warning Using outdated Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2Cdev Fastwire implementation is recommended. + #warning Arduino IDE v1.6.5+ with I2Cdev Fastwire implementation is recommended. #warning This I2Cdev implementation does not support: #warning - Repeated starts conditions #warning - Timeout detection (some Wire requests block forever) #elif ARDUINO > 100 - #warning Using current Arduino IDE with Wire library is functionally limiting. - #warning Arduino IDE v1.0.1+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + /*#warning Using current Arduino IDE with Wire library is functionally limiting. + #warning Arduino IDE v1.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. #warning This I2Cdev implementation does not support: - #warning - Timeout detection (some Wire requests block forever) + #warning - Timeout detection (some Wire requests block forever)*/ #endif #endif @@ -100,9 +103,9 @@ I2Cdev::I2Cdev() { * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { +int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout, void *wireObj) { uint8_t b; - uint8_t count = readByte(devAddr, regAddr, &b, timeout); + uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj); *data = b & (1 << bitNum); return count; } @@ -115,9 +118,9 @@ int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { +int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout, void *wireObj) { uint16_t b; - uint8_t count = readWord(devAddr, regAddr, &b, timeout); + uint8_t count = readWord(devAddr, regAddr, &b, timeout, wireObj); *data = b & (1 << bitNum); return count; } @@ -131,14 +134,14 @@ int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { +int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) { // 01101001 read byte // 76543210 bit numbers // xxx args: bitStart=4, length=3 // 010 masked // -> 010 shifted uint8_t count, b; - if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { + if ((count = readByte(devAddr, regAddr, &b, timeout, wireObj)) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); b &= mask; b >>= (bitStart - length + 1); @@ -156,7 +159,7 @@ int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) */ -int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { +int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) { // 1101011001101001 read byte // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 @@ -164,7 +167,7 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin // -> 010 shifted uint8_t count; uint16_t w; - if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { + if ((count = readWord(devAddr, regAddr, &w, timeout, wireObj)) != 0) { uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); w &= mask; w >>= (bitStart - length + 1); @@ -180,8 +183,8 @@ int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uin * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { - return readBytes(devAddr, regAddr, 1, data, timeout); +int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout, void *wireObj) { + return readBytes(devAddr, regAddr, 1, data, timeout, wireObj); } /** Read single word from a 16-bit device register. @@ -191,8 +194,8 @@ int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Status of read operation (true = success) */ -int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { - return readWords(devAddr, regAddr, 1, data, timeout); +int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout, void *wireObj) { + return readWords(devAddr, regAddr, 1, data, timeout, wireObj); } /** Read multiple bytes from an 8-bit device register. @@ -203,7 +206,7 @@ int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Number of bytes read (-1 indicates failure) */ -int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { +int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -214,74 +217,66 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 Serial.print("..."); #endif - int8_t count = 0; + uint8_t count = 0; uint32_t t1 = millis(); - #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; #if (ARDUINO < 100) // Arduino v00xx (before v1.0), Wire library // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.receive(); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->receive(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); #endif } - - Wire.endTransmission(); } #elif (ARDUINO == 100) // Arduino v1.0.0, Wire library // Adds standardized write() and read() stream methods instead of send() and receive() // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); #endif } - - Wire.endTransmission(); } #elif (ARDUINO > 100) // Arduino v1.0.1+, Wire library // Adds official support for repeated start condition, yay! // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH)); - - for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) { - data[count] = Wire.read(); + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -323,7 +318,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8 * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) * @return Number of words read (-1 indicates failure) */ -int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { +int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -334,32 +329,33 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 Serial.print("..."); #endif - int8_t count = 0; + uint8_t count = 0; uint32_t t1 = millis(); - #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE) +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; #if (ARDUINO < 100) // Arduino v00xx (before v1.0), Wire library // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.send(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.receive() << 8; + data[count] = useWire->receive() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.receive(); + data[count] |= useWire->receive(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -368,31 +364,28 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 } msb = !msb; } - - Wire.endTransmission(); } #elif (ARDUINO == 100) // Arduino v1.0.0, Wire library // Adds standardized write() and read() stream methods instead of send() and receive() // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; + data[count] = useWire->read() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); + data[count] |= useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -401,31 +394,28 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 } msb = !msb; } - - Wire.endTransmission(); } #elif (ARDUINO > 100) // Arduino v1.0.1+, Wire library // Adds official support for repeated start condition, yay! // I2C/TWI subsystem uses internal buffer that breaks with large data requests - // so if user requests more than BUFFER_LENGTH bytes, we have to do it in + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in // smaller chunks instead of all at once - for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) { - Wire.beginTransmission(devAddr); - Wire.write(regAddr); - Wire.endTransmission(); - Wire.beginTransmission(devAddr); - Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes bool msb = true; // starts with MSB, then LSB - for (; Wire.available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { if (msb) { // first byte is bits 15-8 (MSb=15) - data[count] = Wire.read() << 8; + data[count] = useWire->read() << 8; } else { // second byte is bits 7-0 (LSb=0) - data[count] |= Wire.read(); + data[count] |= useWire->read(); #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[count], HEX); if (count + 1 < length) Serial.print(" "); @@ -434,8 +424,6 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 } msb = !msb; } - - Wire.endTransmission(); } #endif @@ -443,8 +431,8 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 // Fastwire library // no loop required for fastwire - uint16_t intermediate[(uint8_t)length]; - uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, (uint8_t *)intermediate, (uint8_t)(length * 2)); + uint8_t intermediate[(uint8_t)length*2]; + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, intermediate, (uint8_t)(length * 2)); if (status == 0) { count = length; // success for (uint8_t i = 0; i < length; i++) { @@ -474,11 +462,11 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1 * @param value New bit value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { +bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj) { uint8_t b; - readByte(devAddr, regAddr, &b); + readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj); b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); - return writeByte(devAddr, regAddr, b); + return writeByte(devAddr, regAddr, b, wireObj); } /** write a single bit in a 16-bit device register. @@ -488,11 +476,11 @@ bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t * @param value New bit value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { +bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj) { uint16_t w; - readWord(devAddr, regAddr, &w); + readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj); w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); - return writeWord(devAddr, regAddr, w); + return writeWord(devAddr, regAddr, w, wireObj); } /** Write multiple bits in an 8-bit device register. @@ -503,7 +491,7 @@ bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_ * @param data Right-aligned value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { +bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj) { // 010 value to write // 76543210 bit numbers // xxx args: bitStart=4, length=3 @@ -512,13 +500,13 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8 // 10100011 original & ~mask // 10101011 masked | value uint8_t b; - if (readByte(devAddr, regAddr, &b) != 0) { + if (readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj) != 0) { uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data b &= ~(mask); // zero all important bits in existing byte b |= data; // combine data with existing byte - return writeByte(devAddr, regAddr, b); + return writeByte(devAddr, regAddr, b, wireObj); } else { return false; } @@ -532,7 +520,7 @@ bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8 * @param data Right-aligned value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { +bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj) { // 010 value to write // fedcba9876543210 bit numbers // xxx args: bitStart=12, length=3 @@ -541,13 +529,13 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint // 1010001110010110 original & ~mask // 1010101110010110 masked | value uint16_t w; - if (readWord(devAddr, regAddr, &w) != 0) { + if (readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj) != 0) { uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); data <<= (bitStart - length + 1); // shift data into correct position data &= mask; // zero all non-important bits in data w &= ~(mask); // zero all important bits in existing word w |= data; // combine data with existing word - return writeWord(devAddr, regAddr, w); + return writeWord(devAddr, regAddr, w, wireObj); } else { return false; } @@ -559,8 +547,8 @@ bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint * @param data New byte value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { - return writeBytes(devAddr, regAddr, 1, &data); +bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj) { + return writeBytes(devAddr, regAddr, 1, &data, wireObj); } /** Write single word to a 16-bit device register. @@ -569,8 +557,8 @@ bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { * @param data New word value to write * @return Status of operation (true = success) */ -bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { - return writeWords(devAddr, regAddr, 1, &data); +bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj) { + return writeWords(devAddr, regAddr, 1, &data, wireObj); } /** Write multiple bytes to an 8-bit device register. @@ -580,7 +568,7 @@ bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { * @param data Buffer to copy new data from * @return Status of operation (true = success) */ -bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { +bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -591,12 +579,20 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_ Serial.print("..."); #endif uint8_t status = 0; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; +#endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send((uint8_t) regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write((uint8_t) regAddr); // send address + useWire->beginTransmission(devAddr); + useWire->send((uint8_t) regAddr); // send address + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->beginTransmission(devAddr); + useWire->write((uint8_t) regAddr); // send address #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::beginTransmission(devAddr); Fastwire::write(regAddr); @@ -607,17 +603,21 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_ if (i + 1 < length) Serial.print(" "); #endif #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t) data[i]); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.write((uint8_t) data[i]); + useWire->send((uint8_t) data[i]); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t) data[i]); #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::write((uint8_t) data[i]); #endif } #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - status = Wire.endTransmission(); + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::stop(); //status = Fastwire::endTransmission(); @@ -635,7 +635,7 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_ * @param data Buffer to copy new data from * @return Status of operation (true = success) */ -bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { +bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data, void *wireObj) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print("I2C (0x"); Serial.print(devAddr, HEX); @@ -646,37 +646,49 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16 Serial.print("..."); #endif uint8_t status = 0; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; +#endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.beginTransmission(devAddr); - Wire.send(regAddr); // send address - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.beginTransmission(devAddr); - Wire.write(regAddr); // send address + useWire->beginTransmission(devAddr); + useWire->send(regAddr); // send address + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->beginTransmission(devAddr); + useWire->write(regAddr); // send address #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::beginTransmission(devAddr); Fastwire::write(regAddr); #endif - for (uint8_t i = 0; i < length * 2; i++) { + for (uint8_t i = 0; i < length; i++) { #ifdef I2CDEV_SERIAL_DEBUG Serial.print(data[i], HEX); if (i + 1 < length) Serial.print(" "); #endif #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.send((uint8_t)(data[i] >> 8)); // send MSB - Wire.send((uint8_t)data[i++]); // send LSB - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - Wire.write((uint8_t)(data[i] >> 8)); // send MSB - Wire.write((uint8_t)data[i++]); // send LSB + useWire->send((uint8_t)(data[i] >> 8)); // send MSB + useWire->send((uint8_t)data[i]); // send LSB + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t)(data[i] >> 8)); // send MSB + useWire->write((uint8_t)data[i]); // send LSB #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB - status = Fastwire::write((uint8_t)data[i++]); // send LSB + status = Fastwire::write((uint8_t)data[i]); // send LSB if (status != 0) break; #endif } #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) - Wire.endTransmission(); - #elif (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) - status = Wire.endTransmission(); + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) Fastwire::stop(); //status = Fastwire::endTransmission(); @@ -736,7 +748,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; #endif TWSR = 0; // no prescaler => prescaler = 1 - TWBR = ((16000L / khz) - 16) / 2; // change the I2C clock rate + TWBR = F_CPU / 2000 / khz - 8; // change the I2C clock rate TWCR = 1 << TWEN; // enable twi module, no interrupt } @@ -976,7 +988,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register // enable twi module, acks, and twi interrupt - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA); /* TWEN - TWI Enable Bit TWIE - TWI Interrupt Enable @@ -1045,7 +1057,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; } void twii_SetStart() { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTA); } void twi_write01() { @@ -1128,19 +1140,19 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; void twi_reply(uint8_t ack) { // transmit master read ready signal, with or without ack if (ack){ - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT) | (1 << TWEA); } else { - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT); } } void twi_stop(void) { // send stop condition - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTO); // wait for stop condition to be exectued on bus // TWINT is not set after a stop condition! - while (TWCR & _BV(TWSTO)) { + while (TWCR & (1 << TWSTO)) { continue; } @@ -1150,7 +1162,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; void twi_releaseBus(void) { // release bus - TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT); + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT); // update twi state twi_state = TWI_READY; diff --git a/Arduino/I2Cdev/I2Cdev.h b/Arduino/I2Cdev/I2Cdev.h index f6f87970..5b59c56f 100644 --- a/Arduino/I2Cdev/I2Cdev.h +++ b/Arduino/I2Cdev/I2Cdev.h @@ -1,8 +1,11 @@ // I2Cdev library collection - Main I2C device class header file // Abstracts bit and byte I2C R/W functions into a convenient class -// 6/9/2012 by Jeff Rowberg +// 2013-06-05 by Jeff Rowberg // // Changelog: +// 2021-09-28 - allow custom Wire object as transaction function argument +// 2020-01-20 - hardija : complete support for Teensy 3.x +// 2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1 // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire @@ -46,11 +49,20 @@ THE SOFTWARE. #ifndef _I2CDEV_H_ #define _I2CDEV_H_ +// ----------------------------------------------------------------------------- +// Enable deprecated pgmspace typedefs in avr-libc +// ----------------------------------------------------------------------------- +#define __PROG_TYPES_COMPAT__ + // ----------------------------------------------------------------------------- // I2C interface implementation setting // ----------------------------------------------------------------------------- +#ifndef I2CDEV_IMPLEMENTATION #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_TEENSY_3X_WIRE +//#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_SBWIRE //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE +#endif // I2CDEV_IMPLEMENTATION // comment this out if you are using a non-optimal IDE/implementation setting // but want the compiler to shut up about it @@ -64,6 +76,8 @@ THE SOFTWARE. // ^^^ NBWire implementation is still buggy w/some interrupts! #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library +#define I2CDEV_BUILTIN_SBWIRE 5 // I2C object from Shuning (Steve) Bian's SBWire Library at https://github.com/freespace/SBWire +#define I2CDEV_TEENSY_3X_WIRE 6 // Teensy 3.x support using i2c_t3 library // ----------------------------------------------------------------------------- // Arduino-style "Serial.print" debug constant (uncomment to enable) @@ -79,9 +93,37 @@ THE SOFTWARE. #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE #include #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + #include + #endif #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY #include #endif + #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE + #include "SBWire.h" + #endif +#endif + +#ifdef SPARK + #include "application.h" + #define ARDUINO 101 + #define BUFFER_LENGTH 32 +#endif + +#ifndef I2CDEVLIB_WIRE_BUFFER_LENGTH + #if defined(I2C_BUFFER_LENGTH) + // Arduino ESP32 core Wire uses this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH + #elif defined(BUFFER_LENGTH) + // Arduino AVR core Wire and many others use this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH BUFFER_LENGTH + #elif defined(SERIAL_BUFFER_SIZE) + // Arduino SAMD core Wire uses this + #define I2CDEVLIB_WIRE_BUFFER_LENGTH SERIAL_BUFFER_SIZE + #else + // should be a safe fallback, though possibly inefficient + #define I2CDEVLIB_WIRE_BUFFER_LENGTH 32 + #endif #endif // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") @@ -90,24 +132,24 @@ THE SOFTWARE. class I2Cdev { public: I2Cdev(); - - static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); - static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); - - static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); - static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); - static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); - static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); - static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); - static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); - static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); - static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); + + static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + + static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0); + static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0); + static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0); + static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0); + static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0); + static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0); + static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0); + static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0); static uint16_t readTimeout; }; @@ -119,7 +161,7 @@ class I2Cdev { // Copyright(C) 2012 // Francesco Ferrara ////////////////////// - + /* Master */ #define TW_START 0x08 #define TW_REP_START 0x10 @@ -162,24 +204,24 @@ class I2Cdev { // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html #define NBWIRE_BUFFER_LENGTH 32 - + class TwoWire { private: static uint8_t rxBuffer[]; static uint8_t rxBufferIndex; static uint8_t rxBufferLength; - + static uint8_t txAddress; static uint8_t txBuffer[]; static uint8_t txBufferIndex; static uint8_t txBufferLength; - + // static uint8_t transmitting; static void (*user_onRequest)(void); static void (*user_onReceive)(int); static void onRequestService(void); static void onReceiveService(uint8_t*, int); - + public: TwoWire(); void begin(); @@ -201,26 +243,26 @@ class I2Cdev { void onReceive(void (*)(int)); void onRequest(void (*)(void)); }; - + #define TWI_READY 0 #define TWI_MRX 1 #define TWI_MTX 2 #define TWI_SRX 3 #define TWI_STX 4 - + #define TW_WRITE 0 #define TW_READ 1 - + #define TW_MT_SLA_NACK 0x20 #define TW_MT_DATA_NACK 0x30 - + #define CPU_FREQ 16000000L #define TWI_FREQ 100000L #define TWI_BUFFER_LENGTH 32 - + /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ - - #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) + + #define TW_STATUS_MASK ((1 << TWS7)|(1 << TWS6)|(1 << TWS5)|(1 << TWS4)|(1 << TWS3)) #define TW_STATUS (TWSR & TW_STATUS_MASK) #define TW_START 0x08 #define TW_REP_START 0x10 @@ -250,18 +292,18 @@ class I2Cdev { #define TW_SR_STOP 0xA0 #define TW_NO_INFO 0xF8 #define TW_BUS_ERROR 0x00 - + //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) - + #ifndef sbi // set bit - #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) + #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= (1 << bit)) #endif // sbi - + #ifndef cbi // clear bit - #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit)) #endif // cbi - + extern TwoWire Wire; #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE diff --git a/Arduino/I2Cdev/library.json b/Arduino/I2Cdev/library.json new file mode 100644 index 00000000..336e9d06 --- /dev/null +++ b/Arduino/I2Cdev/library.json @@ -0,0 +1,17 @@ +{ + "name": "I2Cdevlib-Core", + "version": "1.0.1", + "keywords": "i2cdevlib, i2c", + "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.", + "include": "Arduino/I2Cdev", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "frameworks": "arduino", + "platforms": "*", + "dependencies": { + "Wire": "*" + } +} diff --git a/Arduino/IAQ2000/IAQ2000.cpp b/Arduino/IAQ2000/IAQ2000.cpp index 6a538599..8e44b9cb 100755 --- a/Arduino/IAQ2000/IAQ2000.cpp +++ b/Arduino/IAQ2000/IAQ2000.cpp @@ -7,6 +7,7 @@ // // Changelog: // 2012-04-01 - initial release +// 2012-11-08 - added TVoc and Status /* ============================================ I2Cdev device library code is placed under the MIT license @@ -62,7 +63,7 @@ void IAQ2000::initialize() { * @return True if connection is valid, false otherwise */ bool IAQ2000::testConnection() { - if (getIaq() >= 450) { + if (getIaqpred() >= 450) { return true; } else { @@ -73,12 +74,26 @@ bool IAQ2000::testConnection() { /** Read iAQ-2000 indoor air quality sensor. * @return Predicted CO2 concentration based on human induced volatile organic compounds (VOC) detection (in ppm VOC + CO2 equivalents) */ -uint16_t IAQ2000::getIaq() { - // read bytes from the DATA1 AND DATA2 registers and bit-shifting them into a 16-bit value +uint16_t IAQ2000::getIaqpred() { + // read bytes from the DATA0 AND DATA1 registers and bit-shifting them into a 16-bit value readAllBytes(devAddr, 2, buffer); return ((buffer[0] << 8) | buffer[1]); } +uint8_t IAQ2000::getIaqstatus() { + // read bytes from the DATA2 register + readAllBytes(devAddr, 3, buffer); + return (buffer[2]); +} + +uint16_t IAQ2000::getIaqtvoc() { + // read bytes from the DATA7 AND DATA8 registers and bit-shifting them into a 16-bit value + readAllBytes(devAddr, 9, buffer); + return ((buffer[7] << 8) | buffer[8]); +} + + + /** Read bytes from a slave device. * This is a "stripped-down" version of the standard Jeff Rowberg's I2Cdev::readBytes method * intended to provide compatibility with iAQ-2000, @@ -98,7 +113,7 @@ int8_t IAQ2000::readAllBytes(uint8_t devAddr, uint8_t length, uint8_t *data, uin Serial.print(" bytes..."); #endif - int8_t count = 0; + uint8_t count = 0; Wire.requestFrom(devAddr, length); diff --git a/Arduino/IAQ2000/IAQ2000.h b/Arduino/IAQ2000/IAQ2000.h index 432b3573..86839b22 100755 --- a/Arduino/IAQ2000/IAQ2000.h +++ b/Arduino/IAQ2000/IAQ2000.h @@ -7,6 +7,7 @@ // // Changelog: // 2012-04-01 - initial release +// 2015-11-08 - added TVoc and Status /* ============================================ I2Cdev device library code is placed under the MIT license @@ -48,12 +49,14 @@ class IAQ2000 { IAQ2000(); IAQ2000(uint8_t address); void initialize(); - bool testConnection(); - uint16_t getIaq(); + bool testConnection(); + uint16_t getIaqtvoc(); + uint16_t getIaqpred(); + uint8_t getIaqstatus(); private: uint8_t devAddr; - uint8_t buffer[2]; + uint8_t buffer[8]; int8_t readAllBytes(uint8_t devAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); }; diff --git a/Arduino/IAQ2000/examples/IAQ2000/IAQ2000.ino b/Arduino/IAQ2000/examples/IAQ2000/IAQ2000.ino index bcae91a7..96dcf4f3 100644 --- a/Arduino/IAQ2000/examples/IAQ2000/IAQ2000.ino +++ b/Arduino/IAQ2000/examples/IAQ2000/IAQ2000.ino @@ -5,29 +5,30 @@ // // Changelog: // 2012-04-01 - initial release +// 2015-11-08 - added TVoc and Status /* ============================================ -IAQ2000 device library code is placed under the MIT license -Copyright (c) 2012 Peteris Skorovs, 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. -=============================================== + IAQ2000 device library code is placed under the MIT license + Copyright (c) 2012 Peteris Skorovs, 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. + =============================================== */ // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation @@ -39,54 +40,123 @@ THE SOFTWARE. #include "I2Cdev.h" #include "IAQ2000.h" + // class default I2C address is 0x5A // specific I2C addresses may be passed as a parameter here // but this device only supports one I2C address (0x5A) IAQ2000 iaq; uint16_t airQuality; +uint8_t iAQstatus; +uint16_t airTvoc; + +unsigned long startTime; +uint16_t oldairQuality = 0; +uint8_t oldiAQstatus = 0; +uint16_t oldairTvoc = 0; +uint8_t result = 0; + #define LED_PIN 13 bool blinkState = false; + void setup() { - // join I2C bus (I2Cdev library doesn't do this automatically) - Wire.begin(); - - // initialize serial communication - // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but - // it's really up to you depending on your project) - Serial.begin(38400); - - // initialize device - Serial.println("Initializing I2C devices..."); - iaq.initialize(); - - // verify connection - Serial.println("Testing device connections..."); - Serial.println(iaq.testConnection() ? "iAQ-2000 connection successful" : "iAQ-2000 connection failed"); - - // configure LED pin for output - pinMode(LED_PIN, OUTPUT); - -} + // configure LED pin for output + pinMode(LED_PIN, OUTPUT); + // join I2C bus (I2Cdev library doesn't do this automatically) + Wire.begin(); -void loop() { - // read seansor - airQuality = iaq.getIaq(); - - // display predicted CO2 concentration based on human induced volatile organic compounds (VOC) detection (in ppm VOC + CO2 equivalents) - // during the first 6 hours of continuous operation (burn-in) the module will display 450ppm - // the successful burn-in is saved to the EEPROM, the run-in time after restart is 15min - Serial.print("CO2 = "); - Serial.print(airQuality); - Serial.print(" "); - Serial.println("[ppm]"); - - // blink LED to indicate activity + // initialize serial communication + // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but + // it's really up to you depending on your project) + Serial.begin(38400); + delay(1000); + Serial.println("Initializing Serial..."); + delay(1000); + + while (!Serial) { + ; // wait for serial port to connect + } + + startTime = millis(); + + // initialize device + // Serial.println("Initializing I2C devices..."); + + + iaq.initialize(); + if (!iaq.testConnection()) { blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); + delay(500); + } +} - // wait five seconds - delay(5000); -} \ No newline at end of file +void loop() { + + // display predicted CO2 concentration based on human induced volatile organic compounds (VOC) detection (in ppm VOC + CO2 equivalents) + // during the first 6 hours of continuous operation (burn-in) the module will display 450ppm + // the successful burn-in is saved to the EEPROM, the run-in time after restart is 15min + Serial.print("CO2 = "); + Serial.print(airQuality); + Serial.print(" "); + Serial.print("[ppm] 450-2000"); + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + + // wait five seconds + delay(5000); + + // read seansor + iAQstatus = iaq.getIaqstatus(); + + // display predicted CO2 concentration based on human induced volatile organic compounds (VOC) detection (in ppm VOC + CO2 equivalents) + // during the first 6 hours of continuous operation (burn-in) the module will display 450ppm + // the successful burn-in is saved to the EEPROM, the run-in time after restart is 15min + Serial.print(" -- Status = "); + if (iAQstatus == 0) + { + Serial.print("OK"); + } + if (iAQstatus == 1) + { + Serial.print("BUSY"); + } + if (iAQstatus == 16) + { + Serial.print("WARUMUP"); + } + if (iAQstatus == 128) + { + Serial.print("ERROR"); + } + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + + // wait five seconds + delay(5000); + + // read seansor + airTvoc = iaq.getIaqtvoc(); + + // display predicted CO2 concentration based on human induced volatile organic compounds (VOC) detection (in ppm VOC + CO2 equivalents) + // during the first 6 hours of continuous operation (burn-in) the module will display 450ppm + // the successful burn-in is saved to the EEPROM, the run-in time after restart is 15min + Serial.print(" -- TVoc = "); + Serial.print(airTvoc); + Serial.print(" "); + Serial.println("[ppb] 125-600"); + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + + // wait five seconds + delay(5000); + +} diff --git a/Arduino/IAQ2000/keywords.txt b/Arduino/IAQ2000/keywords.txt index 775b8571..2188577c 100644 --- a/Arduino/IAQ2000/keywords.txt +++ b/Arduino/IAQ2000/keywords.txt @@ -11,9 +11,9 @@ IAQ2000 KEYWORD1 # Methods and Functions (KEYWORD2) ####################################### -initialize KEYWORD2 +initialize KEYWORD2 testConnection KEYWORD2 -getIaq KEYWORD2 +getIaq KEYWORD2 ####################################### # Instances (KEYWORD2) diff --git a/Arduino/IAQ2000/library.json b/Arduino/IAQ2000/library.json new file mode 100644 index 00000000..dfcd93c6 --- /dev/null +++ b/Arduino/IAQ2000/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-IAQ2000", + "version": "1.0.0", + "keywords": "co2, carbon, dioxide, sensor, i2cdevlib, i2c", + "description": "The iAQ-2000 Indoor Air Quality Module is a sensitive, low-cost solution for detecting poor air quality", + "include": "Arduino/IAQ2000", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/ITG3200/Examples/ITG3200_raw/ITG3200_raw.ino b/Arduino/ITG3200/examples/ITG3200_raw/ITG3200_raw.ino similarity index 98% rename from Arduino/ITG3200/Examples/ITG3200_raw/ITG3200_raw.ino rename to Arduino/ITG3200/examples/ITG3200_raw/ITG3200_raw.ino index f4bd7292..02a8476b 100644 --- a/Arduino/ITG3200/Examples/ITG3200_raw/ITG3200_raw.ino +++ b/Arduino/ITG3200/examples/ITG3200_raw/ITG3200_raw.ino @@ -66,7 +66,7 @@ void setup() { Serial.println("Testing device connections..."); Serial.println(gyro.testConnection() ? "ITG3200 connection successful" : "ITG3200 connection failed"); - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } diff --git a/Arduino/ITG3200/library.json b/Arduino/ITG3200/library.json new file mode 100644 index 00000000..23b4f1b7 --- /dev/null +++ b/Arduino/ITG3200/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-ITG3200", + "version": "1.0.0", + "keywords": "gyroscope, sensor, i2cdevlib, i2c", + "description": "The ITG-3200 is groundbreaking 3-axis, digital output MEMS gyroscope", + "include": "Arduino/ITG3200", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/L3G4200D/L3G4200D.cpp b/Arduino/L3G4200D/L3G4200D.cpp index dd7b0326..04e10c5a 100644 --- a/Arduino/L3G4200D/L3G4200D.cpp +++ b/Arduino/L3G4200D/L3G4200D.cpp @@ -950,11 +950,14 @@ void L3G4200D::getAngularVelocity(int16_t* x, int16_t* y, int16_t* z) { * @see L3G4200D_RA_OUT_X_H */ int16_t L3G4200D::getAngularVelocityX() { - I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_X_L, 2, buffer); + uint8_t bufferL[6]; + uint8_t bufferH[6]; + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_X_L, 1, bufferL); + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_X_H, 1, bufferH); if (getEndianMode() == L3G4200D_BIG_ENDIAN) { - return (((int16_t)buffer[1]) << 8) | buffer[0]; + return (((int16_t) bufferL[0]) << 8) | bufferH[0]; } else { - return (((int16_t)buffer[0]) << 8) | buffer[1]; + return (((int16_t) bufferH[0]) << 8) | bufferL[0]; } } @@ -964,11 +967,14 @@ int16_t L3G4200D::getAngularVelocityX() { * @see L3G4200D_RA_OUT_Y_H */ int16_t L3G4200D::getAngularVelocityY() { - I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Y_L, 2, buffer); + uint8_t bufferL[6]; + uint8_t bufferH[6]; + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Y_L, 1, bufferL); + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Y_H, 1, bufferH); if (getEndianMode() == L3G4200D_BIG_ENDIAN) { - return (((int16_t)buffer[1]) << 8) | buffer[0]; + return (((int16_t) bufferL[0]) << 8) | bufferH[0]; } else { - return (((int16_t)buffer[0]) << 8) | buffer[1]; + return (((int16_t) bufferH[0]) << 8) | bufferL[0]; } } @@ -978,11 +984,14 @@ int16_t L3G4200D::getAngularVelocityY() { * @see L3G4200D_RA_OUT_Z_H */ int16_t L3G4200D::getAngularVelocityZ() { - I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Z_L, 2, buffer); + uint8_t bufferL[6]; + uint8_t bufferH[6]; + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Z_L, 1, bufferL); + I2Cdev::readBytes(devAddr, L3G4200D_RA_OUT_Z_H, 1, bufferH); if (getEndianMode() == L3G4200D_BIG_ENDIAN) { - return (((int16_t)buffer[1]) << 8) | buffer[0]; + return (((int16_t) bufferL[0]) << 8) | bufferH[0]; } else { - return (((int16_t)buffer[0]) << 8) | buffer[1]; + return (((int16_t) bufferH[0]) << 8) | bufferL[0]; } } diff --git a/Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino b/Arduino/L3G4200D/examples/L3G4200D_raw/L3G4200D_raw.ino similarity index 100% rename from Arduino/L3G4200D/Examples/L3G4200D_raw/L3G4200D_raw.ino rename to Arduino/L3G4200D/examples/L3G4200D_raw/L3G4200D_raw.ino diff --git a/Arduino/L3G4200D/library.json b/Arduino/L3G4200D/library.json new file mode 100644 index 00000000..7ffe66eb --- /dev/null +++ b/Arduino/L3G4200D/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-L3G4200D", + "version": "1.0.0", + "keywords": "accelerometer, sensor, i2cdevlib, i2c", + "description": "The L3G4200D is a low-power three-axis angular rate sensor able to provide unprecedented stablility of zero rate level and sensitivity over temperature and time", + "include": "Arduino/L3G4200D", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/L3GD20H/L3GD20H.cpp b/Arduino/L3GD20H/L3GD20H.cpp new file mode 100644 index 00000000..bd5d8887 --- /dev/null +++ b/Arduino/L3GD20H/L3GD20H.cpp @@ -0,0 +1,1695 @@ +// I2Cdev library collection - L3GD20H I2C device class +// Based on STMicroelectronics L3GD20H datasheet rev. 2, 3/2013 +// 3/05/2015 by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-05 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jonathan Arnett, 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 "L3GD20H.h" + +/** Default constructor, uses default I2C address. + * @see L3GD20H_DEFAULT_ADDRESS + */ +L3GD20H::L3GD20H() { + devAddr = L3GD20H_DEFAULT_ADDRESS; + endianMode = 0; +} + +/** Specific address constructor. + * @param address I2C address + * @see L3GD20H_DEFAULT_ADDRESS + * @see L3GD20H_ADDRESS + */ +L3GD20H::L3GD20H(uint8_t address) { + devAddr = address; + endianMode = 0; +} + +/** Power on and prepare for general usage. + * All values are defaults except for the power on bit in CTRL_1 + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_RA_CTRL2 + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_RA_CTRL5 + */ +void L3GD20H::initialize() { + I2Cdev::writeByte(devAddr, L3GD20H_RA_CTRL1, 0b00001111); + I2Cdev::writeByte(devAddr, L3GD20H_RA_CTRL2, 0b00000000); + I2Cdev::writeByte(devAddr, L3GD20H_RA_CTRL3, 0b00000000); + I2Cdev::writeByte(devAddr, L3GD20H_RA_CTRL4, 0b00000000); + I2Cdev::writeByte(devAddr, L3GD20H_RA_CTRL5, 0b00000000); +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, false otherwise + */ +bool L3GD20H::testConnection() { + return getDeviceID() == 0b11010111; +} + +// WHO_AM_I register, read-only + +/** Get the Device ID. + * The WHO_AM_I register holds the device's id + * @return Device ID (should be 0b11010011, 109, 0x69) + * @see L3GD20H_RA_WHO_AM_I + */ +uint8_t L3GD20H::getDeviceID() { + I2Cdev::readByte(devAddr, L3GD20H_RA_WHO_AM_I, buffer); + return buffer[0]; +} + +// CTRL1 register, r/w + +/** Set the output data rate. Makes use of the setLowODREnabled function. + * @param rate The new data output rate (can be 12, 25, 50, 100, 200, 400, or 800) + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_ODR_BIT + * @see L3GD20H_ODR_LENGTH + * @see L3GD20H_RATE_100_12 + * @see L3GD20H_RATE_200_25 + * @see L3GD20H_RATE_400_50 + * @see L3GD20H_RATE_800_50 + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_LOW_ODR_BIT + */ +void L3GD20H::setOutputDataRate(uint16_t rate) { + uint8_t writeVal; + bool lowODRwriteVal; + + if (rate ==12) { + writeVal = L3GD20H_RATE_100_12; + setLowODREnabled(true); + } else if (rate == 25) { + writeVal = L3GD20H_RATE_200_25; + setLowODREnabled(true); + } else if (rate == 50) { + writeVal = L3GD20H_RATE_400_50; + setLowODREnabled(true); + } else if (rate == 100) { + writeVal = L3GD20H_RATE_100_12; + setLowODREnabled(false); + } else if (rate == 200) { + writeVal = L3GD20H_RATE_200_25; + setLowODREnabled(false); + } else if (rate == 400) { + writeVal = L3GD20H_RATE_400_50; + setLowODREnabled(false); + } else { + writeVal = L3GD20H_RATE_800_50; + setLowODREnabled(false); + } + + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL1, L3GD20H_ODR_BIT, + L3GD20H_ODR_LENGTH, writeVal); +} + +/** Get the current output data rate + * @return Current data output rate + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_ODR_BIT + * @see L3GD20H_ODR_LENGTH + * @see L3GD20H_RATE_100_12 + * @see L3GD20H_RATE_200_25 + * @see L3GD20H_RATE_400_50 + * @see L3GD20H_RATE_800_50 + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_LOW_ODR_BIT + */ +uint16_t L3GD20H::getOutputDataRate() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL1, L3GD20H_ODR_BIT, + L3GD20H_ODR_LENGTH, buffer); + uint8_t rate = buffer[0]; + + if (rate == L3GD20H_RATE_100_12) { + if (getLowODREnabled() == true) { + return 12; + } else { + return 100; + } + } else if (rate == L3GD20H_RATE_200_25) { + if (getLowODREnabled() == true) { + return 25; + } else { + return 200; + } + } else if (rate == L3GD20H_RATE_400_50) { + if (getLowODREnabled() == true) { + return 50; + } else { + return 400; + } + } else if (rate == L3GD20H_RATE_800_50) { + if (getLowODREnabled() == true) { + return 50; + } else { + return 800; + } + } + return 800; +} + +/** Set the bandwidth cut-off mode + * @param mode The new bandwidth cut-off mode + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_BW_BIT + * @see L3GD20H_BW_LENGTH + * @see L3GD20H_BW_LOW + * @see L3GD20H_BW_MED_LOW + * @see L3GD20H_BW_MED_HIGH + * @see L3GD20H_BW_HIGH + */ +void L3GD20H::setBandwidthCutOffMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL1, L3GD20H_BW_BIT, + L3GD20H_BW_LENGTH, mode); +} + +/** Get the current bandwidth cut-off mode + * @return Current bandwidth cut off mode + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_BW_BIT + * @see L3GD20H_BW_LENGTH + * @see L3GD20H_BW_LOW + * @see L3GD20H_BW_MED_LOW + * @see L3GD20H_BW_MED_HIGH + * @see L3GD20H_BW_HIGH + */ +uint8_t L3GD20H::getBandwidthCutOffMode() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL1, L3GD20H_BW_BIT, + L3GD20H_BW_LENGTH, buffer); + return buffer[0]; +} + +// /** Gets the current bandwidth cutoff based on ODR and BW +// * @return Float value of the bandwidth cut off +// * @see L3GD20H_RA_CTRL1 +// * @see L3GD20H_ODR_BIT +// * @see L3GD20H_ODR_LENGTH +// * @see L3GD20H_RATE_100_12 +// * @see L3GD20H_RATE_200_25 +// * @see L3GD20H_RATE_400_50 +// * @see L3GD20H_RATE_800_50 +// * @see L3GD20H_BW_BIT +// * @see L3GD20H_BW_LENGTH +// * @see L3GD20H_BW_LOW +// * @see L3GD20H_BW_MED_LOW +// * @see L3GD20H_BW_MED_HIGH +// * @see L3GD20H_BW_HIGH +// */ +// float L3GD20H::getBandwidthCutOff() { +// uint16_t dataRate = getOutputDataRate(); +// uint8_t bandwidthMode = getBandwidthCutOffMode(); + +// if (dataRate == 50) { +// return 16.6; +// } else if (dataRate == 100) { +// if (bandwidthMode == L3GD20H_BW_LOW) { +// return 12.5; +// } else { +// return 25.0; +// } +// } else if (dataRate == 200) { +// if (bandwidthMode == L3GD20H_BW_LOW) { +// return 12.5; +// } else if (bandwidthMode == L3GD20H_BW_MED_LOW) { +// return 0.0; +// } else if (bandwidthMode == L3GD20H_BW_MED_HIGH) { +// return 0.0; +// } else { +// return 70.0; +// } +// } else if (dataRate == 400) { +// if (bandwidthMode == L3GD20H_BW_LOW) { +// return 20.0; +// } else if (bandwidthMode == L3GD20H_BW_MED_LOW) { +// return 25.0; +// } else if (bandwidthMode == L3GD20H_BW_MED_HIGH) { +// return 50.0; +// } else { +// return 110.0; +// } +// } else if (dataRate == 800) { +// if (bandwidthMode == L3GD20H_BW_LOW) { +// return 30.0; +// } else if (bandwidthMode == L3GD20H_BW_MED_LOW) { +// return 35.0; +// } else if (bandwidthMode == L3GD20H_BW_MED_HIGH) { +// return 0.0; +// } else { +// return 110.0; +// } +// } else { +// return 0.0 +// } +// } + +/** Set power on or off + * @param enabled The new power setting (true for on, false for off) + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_PD_BIT + */ +void L3GD20H::setPowerOn(bool on) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_PD_BIT, on); +} + +/** Get the current power state + * @return Powered on state (true for on, false for off) + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_PD_BIT + */ +bool L3GD20H::getPowerOn() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_PD_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get Z data + * @param enabled The new enabled state of the Z axis + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_ZEN_BIT + */ +void L3GD20H::setZEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_ZEN_BIT, enabled); +} + +/** Get whether Z axis data is enabled + * @return True if the Z axis is enabled, false otherwise + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_ZEN_BIT + */ +bool L3GD20H::getZEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_ZEN_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get Y data + * @param enabled The new enabled state of the Y axis + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_YEN_BIT + */ +void L3GD20H::setYEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_YEN_BIT, enabled); +} + +/** Get whether Y axis data is enabled + * @return True if the Y axis is enabled, false otherwise + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_YEN_BIT + */ +bool L3GD20H::getYEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_YEN_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get X data + * @param enabled The new enabled state of the X axis + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_XEN_BIT + */ +void L3GD20H::setXEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_XEN_BIT, enabled); +} + +/** Get whether X axis data is enabled + * @return True if the X axis is enabled, false otherwise + * @see L3GD20H_RA_CTRL1 + * @see L3GD20H_XEN_BIT + */ +bool L3GD20H::getXEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL1, L3GD20H_XEN_BIT, buffer); + return buffer[0]; +} + +// CTRL2 register, r/w + +/** Set the high pass mode + * @param mode The new high pass mode + * @see L3GD20H_RA_CTRL2 + * @see L3GD20H_HPM_BIT + * @see L3GD20H_HPM_LENGTH + * @see L3GD20H_HPM_HRF + * @see L3GD20H_HPM_REFERENCE + * @see L3GD20H_HPM_NORMAL + * @see L3GD20H_HPM_AUTORESET + */ +void L3GD20H::setHighPassMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL2, L3GD20H_HPM_BIT, + L3GD20H_HPM_LENGTH, mode); +} + +/** Get the high pass mode + * @return High pass mode + * @see L3GD20H_RA_CTRL2 + * @see L3GD20H_HPM_BIT + * @see L3GD20H_HPM_LENGTH + * @see L3GD20H_HPM_HRF + * @see L3GD20H_HPM_REFERENCE + * @see L3GD20H_HPM_NORMAL + * @see L3GD20H_HPM_AUTORESET + */ +uint8_t L3GD20H::getHighPassMode() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL2, L3GD20H_HPM_BIT, + L3GD20H_HPM_LENGTH, buffer); + return buffer[0]; +} + +/** Set the high pass filter cut off frequency level (1 - 10) + * @param level The new level for the hpcf, using one of the defined levels + * @see L3GD20H_RA_CTRL2 + * @see L3GD20H_HPCF_BIT + * @see L3GD20H_HPCF_LENGTH + * @see L3GD20H_HPCF1 + * @see L3GD20H_HPCF2 + * @see L3GD20H_HPCF3 + * @see L3GD20H_HPCF4 + * @see L3GD20H_HPCF5 + * @see L3GD20H_HPCF6 + * @see L3GD20H_HPCF7 + * @see L3GD20H_HPCF8 + * @see L3GD20H_HPCF9 + * @see L3GD20H_HPCF10 + */ +void L3GD20H::setHighPassFilterCutOffFrequencyLevel(uint8_t level) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL2, L3GD20H_HPCF_BIT, + L3GD20H_HPCF_LENGTH, level); +} + +/** Get the high pass filter cut off frequency level (1 - 10) + * @return High pass filter cut off frequency level + * @see L3GD20H_RA_CTRL2 + * @see L3GD20H_HPCF_BIT + * @see L3GD20H_HPCF_LENGTH + * @see L3GD20H_HPCF1 + * @see L3GD20H_HPCF2 + * @see L3GD20H_HPCF3 + * @see L3GD20H_HPCF4 + * @see L3GD20H_HPCF5 + * @see L3GD20H_HPCF6 + * @see L3GD20H_HPCF7 + * @see L3GD20H_HPCF8 + * @see L3GD20H_HPCF9 + * @see L3GD20H_HPCF10 + */ +uint8_t L3GD20H::getHighPassFilterCutOffFrequencyLevel() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL2, L3GD20H_HPCF_BIT, + L3GD20H_HPCF_LENGTH, buffer); + return buffer[0]; +} + +// CTRL3 register, r/w + +/** Set the INT1 interrupt enabled state + * @param enabled New enabled state for the INT1 interrupt + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT1_IG_BIT + */ +void L3GD20H::setINT1InterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT1_IG_BIT, + enabled); +} + +/** Get the INT1 interrupt enabled state + * @return True if the INT1 interrupt is enabled, false otherwise + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT1_IG_BIT + */ +bool L3GD20H::getINT1InterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT1_IG_BIT, + buffer); + return buffer[0]; +} + +/** Set the INT1 boot status enabled state + * @param enabled New enabled state for the INT1 boot status + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT1_BOOT_BIT + */ +void L3GD20H::setINT1BootStatusEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT1_BOOT_BIT, + enabled); +} + +/** Get the INT1 boot status enabled state + * @return INT1 boot status status + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT1_BOOT_BIT + */ +bool L3GD20H::getINT1BootStatusEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT1_BOOT_BIT, + buffer); + return buffer[0]; +} + +/** Interrupts the active INT1 configuration + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_H_LACTIVE_BIT + */ +void L3GD20H::interruptActiveINT1Config() { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_H_LACTIVE_BIT, 1); +} + +/** Set output mode to push-pull or open-drain + * @param mode New output mode + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_PP_OD_BIT + * @see L3GD20H_PUSH_PULL + * @see L3GD20H_OPEN_DRAIN + */ +void L3GD20H::setOutputMode(bool mode) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_PP_OD_BIT, + mode); +} + +/** Get whether mode is push-pull or open drain + * @return Output mode (TRUE for open-drain, FALSE for push-pull) + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_PP_OD_BIT + * @see L3GD20H_PUSH_PULL + * @see L3GD20H_OPEN_DRAIN + */ +bool L3GD20H::getOutputMode() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_PP_OD_BIT, + buffer); + return buffer[0]; +} + +/** Set data ready interrupt enabled state on INT2 pin + * @param enabled New INT2 data ready interrupt enabled state + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_DRDY_BIT + */ +void L3GD20H::setINT2DataReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_DRDY_BIT, + enabled); +} + +/** Get whether the data ready interrupt is enabled on the INT2 pin + * @return True if the INT2 data ready interrupt is enabled, false otherwise + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_DRDY_BIT + */ +bool L3GD20H::getINT2DataReadyEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_DRDY_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the INT2 FIFO threshold (watermark) interrupt is enabled + * The sensor contains a 32-slot FIFO buffer for storing data so that it may be + * read later. If enabled, the sensor will generate an interrupt on the + * INT2/DRDY pin when the threshold has been reached. The threshold can be + * configured through the setFIFOWatermark function. + * @param enabled New enabled state of the INT2 FIFO threshold (watermark) + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_I2_WTM_BIT + */ +void L3GD20H::setINT2FIFOWatermarkInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_FTH_BIT, + enabled); +} + +/** Get the INT2 FIFO threshold (watermark) interrupt enabled state + * @return true if the FIFO watermark is enabled, false otherwise + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_FTH_BIT + */ +bool L3GD20H::getINT2FIFOWatermarkInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_FTH_BIT, + buffer); + return buffer[0]; +} + +/** Set whether an interrupt is triggered on INT2 when the FIFO is overrun + * @param enabled New FIFO overrun interrupt enabled state + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_ORUN_BIT + */ +void L3GD20H::setINT2FIFOOverrunInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_ORUN_BIT, + enabled); +} + +/** Get whether an interrupt is triggered on INT2 when the FIFO is overrun + * @return True if the INT2 FIFO overrun interrupt is enabled, false otherwise + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_ORUN_BIT + */ +bool L3GD20H::getINT2FIFOOverrunInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_ORUN_BIT, + buffer); + return buffer[0]; +} + +/** Set whether an interrupt is triggered on INT2 when the FIFO buffer is empty + * @param enabled New INT2 FIFO empty interrupt state + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_EMPTY_BIT + */ +void L3GD20H::setINT2FIFOEmptyInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_EMPTY_BIT, + enabled); +} + +/** Get whether the INT2 FIFO empty interrupt is enabled + * @returns True if the INT2 FIFO empty interrupt is enabled, false otherwise + * @see L3GD20H_RA_CTRL3 + * @see L3GD20H_INT2_EMPTY_BIT + */ +bool L3GD20H::getINT2FIFOEmptyInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL3, L3GD20H_INT2_EMPTY_BIT, + buffer); + return buffer[0]; +} + +// CTRL4 register, r/w + +/** Set the Block Data Update (BDU) enabled state + * @param enabled New BDU enabled state + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_BDU_BIT + */ +void L3GD20H::setBlockDataUpdateEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_BDU_BIT, enabled); +} + +/** Get the BDU enabled state + * @return True if Block Data Update is enabled, false otherwise + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_BDU_BIT + */ +bool L3GD20H::getBlockDataUpdateEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_BDU_BIT, buffer); + return buffer[0]; +} + +/** Set the data endian modes + * In Big Endian mode, the Most Significat Byte (MSB) is on the lower address, + * and the Least Significant Byte (LSB) is on the higher address. Little Endian + * mode reverses this order. Little Endian is the default mode. + * @param endianness New endian mode + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_BLE_BIT + * @see L3GD20H_BIG_ENDIAN + * @see L3GD20H_LITTLE_ENDIAN + */ +void L3GD20H::setEndianMode(bool endianness) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_BLE_BIT, + endianness); + endianMode = getEndianMode(); +} + +/** Get the data endian mode + * @return Current endian mode + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_BLE_BIT + * @see L3GD20H_BIG_ENDIAN + * @see L3GD20H_LITTLE_ENDIAN + */ +bool L3GD20H::getEndianMode() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_BLE_BIT, + buffer); + return buffer[0]; +} + +/** Set the full scale of the data output (in dps) + * @param scale The new scale of the data output (250 [actual 245], 500, 2000) + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_FS_BIT + * @see L3GD20H_FS_LENGTH + * @see L3GD20H_FS_250 + * @see L3GD20H_FS_500 + * @see L3GD20H_FS_2000 + */ +void L3GD20H::setFullScale(uint16_t scale) { + uint8_t writeBits; + + if (scale == 250) { + writeBits = L3GD20H_FS_250; + } else if (scale == 500) { + writeBits = L3GD20H_FS_500; + } else { + writeBits = L3GD20H_FS_2000; + } + + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL4, L3GD20H_FS_BIT, + L3GD20H_FS_LENGTH, writeBits); +} + +/** Get the current full scale of the output data (in dps) + * @return Current scale of the output data + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_FS_BIT + * @see L3GD20H_FS_LENGTH + * @see L3GD20H_FS_250 + * @see L3GD20H_FS_500 + * @see L3GD20H_FS_2000 + */ +uint16_t L3GD20H::getFullScale() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL4, + L3GD20H_FS_BIT, L3GD20H_FS_LENGTH, buffer); + uint8_t readBits = buffer[0]; + + if (readBits == L3GD20H_FS_250) { + return 250; + } else if (readBits == L3GD20H_FS_500) { + return 500; + } else { + return 2000; + } +} + +//TODO +//Implement +//L3GD20H::setLevelSensitiveLatchedEnabled() and +//L3GD20H::getLevelSensitiveLatchedEnabled() +// + +/** Set the self test mode + * @param mode New self test mode (Normal, 0, 1) + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_ST_BIT + * @see L3GD20H_ST_LENGTH + * @see L3GD20H_SELF_TEST_NORMAL + * @see L3GD20H_SELF_TEST_0 + * @see L3GD20H_SELF_TEST_1 + */ +void L3GD20H::setSelfTestMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL4, L3GD20H_ST_BIT, + L3GD20H_ST_LENGTH, mode); +} + +/** Get the current self test mode + * @return Current self test mode + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_ST_BIT + * @see L3GD20H_ST_LENGTH + * @see L3GD20H_SELF_TEST_NORMAL + * @see L3GD20H_SELF_TEST_0 + * @see L3GD20H_SELF_TEST_1 + */ +uint8_t L3GD20H::getSelfTestMode() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL4, L3GD20H_ST_BIT, + L3GD20H_ST_LENGTH, buffer); + return buffer[0]; +} + +/** Set the SPI mode + * @param mode New SPI mode + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_SIM_BIT + * @see L3GD20H_SPI_4_WIRE + * @see L3GD20H_SPI_3_WIRE + */ +void L3GD20H::setSPIMode(bool mode) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_SIM_BIT, mode); +} + +/** Get the SPI mode + * @return Current SPI mode + * @see L3GD20H_RA_CTRL4 + * @see L3GD20H_SIM_BIT + * @see L3GD20H_SPI_4_WIRE + * @see L3GD20H_SPI_3_WIRE + */ +bool L3GD20H::getSPIMode() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL4, L3GD20H_SIM_BIT, + buffer); + return buffer[0]; +} + +// CTRL5 register, r/w + +/** Reboots the FIFO memory content + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_BOOT_BIT + */ +void L3GD20H::rebootMemoryContent() { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_BOOT_BIT, true); +} + +/** Set whether the FIFO buffer is enabled + * @param enabled New enabled state of the FIFO buffer + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_FIFO_EN_BIT + */ +void L3GD20H::setFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_FIFO_EN_BIT, + enabled); +} + +/** Get whether the FIFO buffer is enabled + * @return True if the FIFO buffer is enabled, false otherwise + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_FIFO_EN_BIT + */ +bool L3GD20H::getFIFOEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_FIFO_EN_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the sensing chain FIFO stops writing new values once + * the FIFO Threshold (watermark) is reached + * @param enabled New state of the StopOnFTH bit + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_FIFO_STOPONFTH_BIT + */ +void L3GD20H::setStopOnFIFOThresholdEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_STOPONFTH_BIT, + enabled); +} + +/** Get whether the sensing chain FIFO stopping writing new values once + * the FIFO Threshold (watermark) is enabled + * @return True if the state of the StopOnFTH bit is high (enabled) + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_FIFO_STOPONFTH_BIT + */ +bool L3GD20H::getStopOnFIFOThresholdEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_STOPONFTH_BIT, + buffer); + return buffer[0]; +} + + +/** Set the high pass filter enabled state + * @param enabled New high pass filter enabled state + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_HPEN_BIT + */ +void L3GD20H::setHighPassFilterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_HPEN_BIT, + enabled); +} + +/** Get whether the high pass filter is enabled + * @return True if the high pass filter is enabled, false otherwise + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_HPEN_BIT + */ +bool L3GD20H::getHighPassFilterEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_CTRL5, L3GD20H_HPEN_BIT, + buffer); + return buffer[0]; +} + +/** Sets the filter mode to one of the four provided. + * This function also uses the setHighPassFilterEnabled function in order to set + * the mode. That function does not have to be called in addition to this one. + * In addition to setting the filter for the data in the FIFO buffer + * (controlled by the bits written to OUT_SEL), this function also sets the + * filter used for interrupt generation (the bits written to IG_SEL) to be the + * same as the filter used for the FIFO buffer. The filter used for interrupt + * generation can be set separately with the setInterruptFilter function. + * @param filter New method to be used when filtering data + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_IG_SEL_BIT + * @see L3GD20H_IG_SEL_LENGTH + * @see L3GD20H_OUT_SEL_BIT + * @see L3GD20H_OUT_SEL_LENGTH + * @see L3GD20H_NON_HIGH_PASS + * @see L3GD20H_HIGH_PASS + * @see L3GD20H_LOW_PASS + * @see L3GD20H_LOW_HIGH_PASS + */ +void L3GD20H::setDataFilter(uint8_t filter) { + if (filter == L3GD20H_HIGH_PASS || filter == L3GD20H_LOW_HIGH_PASS) { + setHighPassFilterEnabled(true); + } else { + setHighPassFilterEnabled(false); + } + + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL5, L3GD20H_OUT_SEL_BIT, + L3GD20H_OUT_SEL_LENGTH, filter); + I2Cdev::writeBits(devAddr, L3GD20H_RA_CTRL5, L3GD20H_IG_SEL_BIT, + L3GD20H_IG_SEL_LENGTH, filter); +} + +/** Gets the data filter currently in use + * @return Defined value that represents the filter in use + * @see L3GD20H_RA_CTRL5 + * @see L3GD20H_OUT_SEL_BIT + * @see L3GD20H_OUT_SEL_LENGTH + * @see L3GD20H_NON_HIGH_PASS + * @see L3GD20H_HIGH_PASS + * @see L3GD20H_LOW_PASS + * @see L3GD20H_LOW_HIGH_PASS + */ +uint8_t L3GD20H::getDataFilter() { + I2Cdev::readBits(devAddr, L3GD20H_RA_CTRL5, L3GD20H_OUT_SEL_BIT, + L3GD20H_OUT_SEL_LENGTH, buffer); + uint8_t outBits = buffer[0]; + + if (outBits == L3GD20H_NON_HIGH_PASS || outBits == L3GD20H_HIGH_PASS) { + return outBits; + } + + if (getHighPassFilterEnabled()) { + return L3GD20H_LOW_HIGH_PASS; + } else { + return L3GD20H_LOW_PASS; + } +} + +//ToDo: +//setInterruptFilter +//getInterruptFilter + +// REFERENCE register, r/w + +/** Set the reference value for the high pass filter + * @param reference New 8-bit digital high pass filter reference value + * @see L3GD20H_RA_REFERENCE + */ +void L3GD20H::setHighPassFilterReference(uint8_t reference) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_REFERENCE, reference); +} + +/** Get the 8-bit reference value for the high pass filter + * @return 8-bit reference value for the high pass filter + * @see L3GD20H_RA_REFERENCE + */ +uint8_t L3GD20H::getHighPassFilterReference() { + I2Cdev::readByte(devAddr, L3GD20H_RA_REFERENCE, buffer); + return buffer[0]; +} + +// void L3GD20H::setInterruptReference(uint8_t reference) { +// I2Cdev::writeByte(devAddr, L3GD20H_RA_REFERENCE, reference); +// } + +// uint8_t L3GD20H::getInterruptReference() { +// I2Cdev::readByte(devAddr, L3GD20H_RA_REFERENCE, buffer); +// return buffer[0]; +// } + +// OUT_TEMP register, read-only + +/** Gets the current temperature reading from the sensor + * @return Current temperature + * @see L3GD20H_RA_OUT_TEMP + */ +uint8_t L3GD20H::getTemperature() { + I2Cdev::readByte(devAddr, L3GD20H_RA_OUT_TEMP, buffer); + return buffer[0]; +} + +// STATUS register, read-only + +/** Get whether new data overwrote the last set of data before it was read + * @return True if the last set of data was overwritten before being read, false + * otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_ZYXOR_BIT + */ +bool L3GD20H::getXYZOverrun() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_ZYXOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new Z data overwrote the last set of data before it was read + * @return True if the last set of Z data was overwritten before being read, + * false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_ZOR_BIT + */ +bool L3GD20H::getZOverrun() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_ZOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new Y data overwrote the last set of data before it was read + * @return True if the last set of Y data was overwritten before being read, + * false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_YOR_BIT + */ +bool L3GD20H::getYOverrun() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_YOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new X data overwrote the last set of data before it was read + * @return True if the last set of X data was overwritten before being read, + * false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_XOR_BIT + */ +bool L3GD20H::getXOverrun() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_XOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new data avaialable + * @return True if there is new data available, false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_ZYXDA_BIT + */ +bool L3GD20H::getXYZDataAvailable() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_ZYXDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new Z data avaialable + * @return True if there is new Z data available, false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_ZDA_BIT + */ +bool L3GD20H::getZDataAvailable() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_ZDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new Y data avaialable + * @return True if there is new Y data available, false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_YDA_BIT + */ +bool L3GD20H::getYDataAvailable() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_YDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new X data avaialable + * @return True if there is new X data available, false otherwise + * @see L3GD20H_RA_STATUS + * @see L3GD20H_XDA_BIT + */ +bool L3GD20H::getXDataAvailable() { + I2Cdev::readBit(devAddr, L3GD20H_RA_STATUS, L3GD20H_XDA_BIT, + buffer); + return buffer[0]; +} + +// OUT_* registers, read-only + +/** Get the angular velocity for all 3 axes + * Due to the fact that this device supports two difference Endian modes, both + * must be accounted for when reading data. In Little Endian mode, the first + * byte (lowest address) is the least significant and in Big Endian mode the + * first byte is the most significant. + * @param x 16-bit integer container for the X-axis angular velocity + * @param y 16-bit integer container for the Y-axis angular velocity + * @param z 16-bit integer container for the Z-axis angular velocity + */ +void L3GD20H::getAngularVelocity(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, L3GD20H_RA_OUT_X_L | 0x80, 6, buffer); + if (endianMode == L3GD20H_LITTLE_ENDIAN) { + *x = (((int16_t)buffer[1]) << 8) | buffer[0]; + *y = (((int16_t)buffer[3]) << 8) | buffer[2]; + *z = (((int16_t)buffer[5]) << 8) | buffer[4]; + } else { + *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 the angular velocity about the X-axis + * @return Angular velocity about the X-axis + * @see L3GD20H_RA_OUT_X_L + * @see L3GD20H_RA_OUT_X_H + */ +int16_t L3GD20H::getAngularVelocityX() { + I2Cdev::readBytes(devAddr, L3GD20H_RA_OUT_X_L | 0x80, 2, buffer); + if (endianMode == L3GD20H_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +/** Get the angular velocity about the Y-axis + * @return Angular velocity about the Y-axis + * @see L3GD20H_RA_OUT_Y_L + * @see L3GD20H_RA_OUT_Y_H + */ +int16_t L3GD20H::getAngularVelocityY() { + I2Cdev::readBytes(devAddr, L3GD20H_RA_OUT_Y_L | 0x80, 2, buffer); + if (endianMode == L3GD20H_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +/** Get the angular velocity about the Z-axis + * @return Angular velocity about the Z-axis + * @see L3GD20H_RA_OUT_Z_L + * @see L3GD20H_RA_OUT_Z_H + */ +int16_t L3GD20H::getAngularVelocityZ() { + I2Cdev::readBytes(devAddr, L3GD20H_RA_OUT_Z_L | 0x80, 2, buffer); + if (endianMode == L3GD20H_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +// FIFO_CTRL register, r/w + +/** Set the FIFO mode to one of the defined modes + * @param mode New FIFO mode + * @see L3GD20H_RA_FIFO_CTRL + * @see L3GD20H_FIFO_MODE_BIT + * @see L3GD20H_FIFO_MODE_LENGTH + * @see L3GD20H_FM_BYPASS + * @see L3GD20H_FM_FIFO + * @see L3GD20H_FM_STREAM + * @see L3GD20H_FM_STREAM_FIFO + * @see L3GD20H_FM_BYPASS_STREAM + * @see L3GD20H_FM_DYNAMIC_STREAM + * @see L3GD20H_FM_BYPASS_FIFO + */ +void L3GD20H::setFIFOMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_FIFO_CTRL, L3GD20H_FIFO_MODE_BIT, + L3GD20H_FIFO_MODE_LENGTH, mode); +} + +/** Get the FIFO mode to one of the defined modes + * @return Current FIFO mode + * @see L3GD20H_RA_FIFO_CTRL + * @see L3GD20H_FIFO_MODE_BIT + * @see L3GD20H_FIFO_MODE_LENGTH + * @see L3GD20H_FM_BYPASS + * @see L3GD20H_FM_FIFO + * @see L3GD20H_FM_STREAM + * @see L3GD20H_FM_STREAM_FIFO + * @see L3GD20H_FM_BYPASS_STREAM + * @see L3GD20H_FM_DYNAMIC_STREAM + * @see L3GD20H_FM_BYPASS_FIFO + */ +uint8_t L3GD20H::getFIFOMode() { + I2Cdev::readBits(devAddr, L3GD20H_RA_FIFO_CTRL, + L3GD20H_FIFO_MODE_BIT, L3GD20H_FIFO_MODE_LENGTH, buffer); + return buffer[0]; +} + +/** Set the 5-bit FIFO (watermark) threshold + * @param fth New 5-bit FIFO (watermark) threshold + * @see L3GD20H_RA_FIFO_CTRL + * @see L3GD20H_FIFO_TH_BIT + * @see L3GD20H_FIFO_TH_LENGTH + */ +void L3GD20H::setFIFOThreshold(uint8_t fth) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_FIFO_CTRL, L3GD20H_FIFO_TH_BIT, + L3GD20H_FIFO_TH_LENGTH, fth); +} + +/** Get the FIFO watermark threshold + * @return FIFO watermark threshold + * @see L3GD20H_RA_FIFO_CTRL + * @see L3GD20H_FIFO_TH_BIT + * @see L3GD20H_FIFO_TH_LENGTH + */ +uint8_t L3GD20H::getFIFOThreshold() { + I2Cdev::readBits(devAddr, L3GD20H_RA_FIFO_CTRL, L3GD20H_FIFO_TH_BIT, + L3GD20H_FIFO_TH_LENGTH, buffer); + return buffer[0]; +} + +// FIFO_SRC register, read-only + +/** Get whether the number of data sets in the FIFO buffer is less than the + * watermark + * @return True if the number of data sets in the FIFO buffer is more than or + * equal to the watermark, false otherwise. + * @see L3GD20H_RA_FIFO_SRC + * @see L3GD20H_FIFO_TH_STATUS_BIT + */ +bool L3GD20H::getFIFOAtWatermark() { + I2Cdev::readBit(devAddr, L3GD20H_RA_FIFO_SRC, L3GD20H_FIFO_TH_STATUS_BIT, + buffer); + return buffer[0]; +} + +/** Get whether the FIFO buffer is full + * @return True if the FIFO buffer is full, false otherwise + * @see L3GD20H_RA_FIFO_SRC + * @see L3GD20H_FIFO_OVRN_BIT + */ +bool L3GD20H::getFIFOOverrun() { + I2Cdev::readBit(devAddr, L3GD20H_RA_FIFO_SRC, + L3GD20H_OVRN_BIT, buffer); + return buffer[0]; +} + +/** Get whether the FIFO buffer is empty + * @return True if the FIFO buffer is empty, false otherwise + * @see L3GD20H_RA_FIFO_SRC + * @see L3GD20H_FIFO_EMPTY_BIT + */ +bool L3GD20H::getFIFOEmpty() { + I2Cdev::readBit(devAddr, L3GD20H_RA_FIFO_SRC, + L3GD20H_EMPTY_BIT, buffer); + return buffer[0]; +} + +/** Get the number of filled FIFO buffer slots + * @return Number of filled slots in the FIFO buffer + * @see L3GD20H_RA_FIFO_SRC + * @see L3GD20H_FIFO_FSS_BIT + * @see L3GD20H_FIFO_FSS_LENGTH + */ +uint8_t L3GD20H::getFIFOStoredDataLevel() { + I2Cdev::readBits(devAddr, L3GD20H_RA_FIFO_SRC, + L3GD20H_FIFO_FSS_BIT, L3GD20H_FIFO_FSS_LENGTH, buffer); + return buffer[0]; +} + +// IG_CFG register, r/w + +/** Set the combination mode for interrupt events + * @param combination New combination mode for interrupt events. + * L3GD20H_AND_OR_OR for OR and L3GD20H_AND_OR_AND for AND + * @see L3GD20H_RA_IG_CFG + * @see L3GD20H_AND_OR_BIT + * @see L3GD20H_AND_OR_OR + * @see L3GD20H_AND_OR_AND + */ +void L3GD20H::setInterruptCombination(bool combination) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_AND_OR_BIT, + combination); +} + +/** Get the combination mode for interrupt events + * @return Combination mode for interrupt events. L3GD20H_AND_OR_OR for OR and + * L3GD20H_AND_OR_AND for AND + * @see L3GD20H_RA_IG_CFG + * @see L3GD20H_AND_OR_BIT + * @see L3GD20H_AND_OR_OR + * @see L3GD20H_AND_OR_AND + */ +bool L3GD20H::getInterruptCombination() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_AND_OR_BIT, + buffer); + return buffer[0]; +} + +/** Set whether an interrupt request is latched + * This bit is cleared when the IG_SRC register is read + * @param latched New status of the latched request + * @see L3GD20H_RA_IG_CFG + * @see L3GD20H_LIR_BIT + */ +void L3GD20H::setInterruptRequestLatched(bool latched) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_LIR_BIT, latched); +} + +/** Get whether an interrupt request is latched + * @return True if an interrupt request is latched, false otherwise + * @see L3GD20H_RA_IG_CFG + * @see L3GD20H_LIR_BIT + */ +bool L3GD20H::getInterruptRequestLatched() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_LIR_BIT, + buffer); + return buffer[0]; +}; + +/** Set whether the interrupt for Z high is enabled + * @param enabled New enabled state for Z high interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_ZHIE_BIT + */ +void L3GD20H::setZHighInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_ZHIE_BIT, enabled); +} + +/** Get whether the interrupt for Z high is enabled + * @return True if the interrupt for Z high is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_ZHIE_BIT + */ +bool L3GD20H::getZHighInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_ZHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt for Z low is enabled + * @param enabled New enabled state for Z low interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_ZLIE_BIT + */ +void L3GD20H::setZLowInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_ZLIE_BIT, enabled); +} + +/** Get whether the interrupt for Z low is enabled + * @return True if the interrupt for Z low is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_ZLIE_BIT + */ +bool L3GD20H::getZLowInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_ZLIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt for Y high is enabled + * @param enabled New enabled state for Y high interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_YHIE_BIT + */ +void L3GD20H::setYHighInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_YHIE_BIT, enabled); +} + +/** Get whether the interrupt for Y high is enabled + * @return True if the interrupt for Y high is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_YHIE_BIT + */ +bool L3GD20H::getYHighInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_YHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt for Y low is enabled + * @param enabled New enabled state for Y low interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_YLIE_BIT + */ +void L3GD20H::setYLowInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_YLIE_BIT, enabled); +} + +/** Get whether the interrupt for Y low is enabled + * @return True if the interrupt for Y low is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_YLIE_BIT + */ +bool L3GD20H::getYLowInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_YLIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt for X high is enabled + * @param enabled New enabled state for X high interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_XHIE_BIT + */ +void L3GD20H::setXHighInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_XHIE_BIT, enabled); +} + +/** Get whether the interrupt for X high is enabled + * @return True if the interrupt for X high is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_XHIE_BIT + */ +bool L3GD20H::getXHighInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_XHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt for X low is enabled + * @param enabled New enabled state for X low interrupt. + * @see L3GD20H_IG_CFG + * @see L3GD20H_XLIE_BIT + */ +void L3GD20H::setXLowInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_XLIE_BIT, enabled); +} + +/** Get whether the interrupt for X low is enabled + * @return True if the interrupt for X low is enabled, false otherwise + * @see L3GD20H_IG_CFG + * @see L3GD20H_XLIE_BIT + */ +bool L3GD20H::getXLowInterruptEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_CFG, L3GD20H_XLIE_BIT, + buffer); + return buffer[0]; +} + +// IG_SRC register, read-only + +/** Get whether an interrupt has been generated + * @return True if one or more interrupts has been generated, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_IA_BIT + */ +bool L3GD20H::getInterruptActive() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_IA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a Z high event has occurred + * @return True if a Z high event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_ZH_BIT + */ +bool L3GD20H::getZHigh() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_ZH_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a Z low event has occurred + * @return True if a Z low event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_ZL_BIT + */ +bool L3GD20H::getZLow() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_ZL_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a Y high event has occurred + * @return True if a Y high event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_YH_BIT + */ +bool L3GD20H::getYHigh() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_YH_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a Y low event has occurred + * @return True if a Y low event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_YL_BIT + */ +bool L3GD20H::getYLow() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_YL_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a X high event has occurred + * @return True if a X high event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_XH_BIT + */ +bool L3GD20H::getXHigh() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_XH_BIT, + buffer); + return buffer[0]; +} + +/** Get whether a X low event has occurred + * @return True if a X low event has occurred, false otherwise + * @see L3GD20H_RA_IG_SRC + * @see L3GD20H_XL_BIT + */ +bool L3GD20H::getXLow() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_SRC, L3GD20H_XL_BIT, + buffer); + return buffer[0]; +} + +// IG_THS_* registers, r/w + +/** Set the interrupt generation counter mode selection. + * @param enabled New enabled state for X low interrupt. + * @see L3GD20H_IG_THS_XH + * @see L3GD20H_DCRM_BIT + * @see L3GD20H_DCRM_RESET + * @see L3GD20H_DCRM_DEC + */ +void L3GD20H::setDecrementMode(bool mode) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_THS_XH, L3GD20H_DCRM_BIT, mode); +} + +/** Get the interrupt generation counter mode selection. + * @return Mode Interrupt generation counter mode + * @see L3GD20H_IG_THS_XH + * @see L3GD20H_DCRM_BIT + * @see L3GD20H_DCRM_RESET + * @see L3GD20H_DCRM_DEC + */ +bool L3GD20H::getDecrementMode() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_THS_XH, L3GD20H_DCRM_BIT, + buffer); + return buffer[0]; +} + +/** Set the 7-bit threshold for a high interrupt on the X axis + * @param threshold New 7-bit threshold for a high interrupt on the X axis + * @see L3GD20H_IG_THS_XH + */ +void L3GD20H::setXHighThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_XH, threshold); +} + +/** Retrieve the threshold for a high interrupt on the X axis + * @return X high interrupt threshold + * @see L3GD20H_IG_THS_XH + */ +uint8_t L3GD20H::getXHighThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_XH, buffer); + return buffer[0]; +} + +/** Set the 8-bit threshold for a low interrupt on the X axis + * @param threshold New 8-bit threshold for a low interrupt on the X axis + * @see L3GD20H_IG_THS_XL + */ +void L3GD20H::setXLowThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_XL, threshold); +} + +/** Retrieve the threshold for a low interrupt on the X axis + * @return X low interrupt threshold + * @see L3GD20H_IG_THS_XL + */ +uint8_t L3GD20H::getXLowThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_XL, buffer); + return buffer[0]; +} + +/** Set the 7-bit threshold for a high interrupt on the Y axis + * @param threshold New 7-bit threshold for a high interrupt on the Y axis + * @see L3GD20H_IG_THS_YH + */ +void L3GD20H::setYHighThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_YH, threshold); +} + +/** Retrieve the threshold for a high interrupt on the Y axis + * @return Y high interrupt threshold + * @see L3GD20H_IG_THS_YH + */ +uint8_t L3GD20H::getYHighThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_YH, buffer); + return buffer[0]; +} + +/** Set the 8-bit threshold for a low interrupt on the Y axis + * @param threshold New 8-bit threshold for a low interrupt on the Y axis + * @see L3GD20H_IG_THS_YL + */ +void L3GD20H::setYLowThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_YL, threshold); +} + +/** Retrieve the threshold for a low interrupt on the Y axis + * @return Y low interrupt threshold + * @see L3GD20H_IG_THS_YL + */ +uint8_t L3GD20H::getYLowThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_YL, buffer); + return buffer[0]; +} + +/** Set the 7-bit threshold for a high interrupt on the Z axis + * @param threshold New 7-bit threshold for a high interrupt on the Z axis + * @see L3GD20H_IG_THS_ZH + */ +void L3GD20H::setZHighThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_ZH, threshold); +} + +/** Retrieve the threshold for a high interrupt on the Z axis + * @return Z high interrupt threshold + * @see L3GD20H_IG_THS_ZH + */ +uint8_t L3GD20H::getZHighThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_ZH, buffer); + return buffer[0]; +} + +/** Set the 8-bit threshold for a low interrupt on the Z axis + * @param threshold New 8-bit threshold for a low interrupt on the Z axis + * @see L3GD20H_RA_IG_THS_ZL + */ +void L3GD20H::setZLowThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, L3GD20H_RA_IG_THS_ZL, threshold); +} + +/** Retrieve the threshold for a low interrupt on the Z axis + * @return Z low interrupt threshold + * @see L3GD20H_IG_THS_ZL + */ +uint8_t L3GD20H::getZLowThreshold() { + I2Cdev::readByte(devAddr, L3GD20H_RA_IG_THS_ZL, buffer); + return buffer[0]; +} + +// IG_DURATION register, r/w + +/* Set the minimum duration for an interrupt event to be recognized + * This depends on the chosen output data rate + * @param duration New 7-bit duration value necessary for an interrupt event to be + * recognized + * @see L3GD20H_RA_IG_DURATION + * @see L3GD20H_DUR_BIT + * @see L3GD20H_DUR_LENGTH + */ +void L3GD20H::setDuration(uint8_t duration) { + I2Cdev::writeBits(devAddr, L3GD20H_RA_IG_DURATION, L3GD20H_DUR_BIT, + L3GD20H_DUR_LENGTH, duration); +} + +/** Get the minimum duration for an interrupt event to be recognized + * @return Duration value necessary for an interrupt event to be recognized + * @see L3GD20H_RA_IG_DURATION + * @see L3GD20H_DUR_BIT + * @see L3GD20H_DUR_LENGTH + */ +uint8_t L3GD20H::getDuration() { + I2Cdev::readBits(devAddr, L3GD20H_RA_IG_DURATION, + L3GD20H_DUR_BIT, L3GD20H_DUR_LENGTH, buffer); + return buffer[0]; +} + +/** Set whether the interrupt wait feature is enabled + * If false, the interrupt falls immediately if signal crosses the selected + * threshold. Otherwise, if signal crosses the selected threshold, the interrupt + * falls only after the duration has counted number of samples at the selected + * data rate, written into the duration counter register. + * @param enabled New enabled state of the interrupt wait + * @see L3GD20H_RA_IG_DURATION + * @see L3GD20H_WAIT_BIT + */ +void L3GD20H::setWaitEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, L3GD20H_RA_IG_DURATION, L3GD20H_WAIT_BIT, + enabled); +} + +/** Get whether the interrupt wait feature is enabled + * @return True if the wait feature is enabled, false otherwise + * @see L3GD20H_RA_IG_DURATION + * @see L3GD20H_WAIT_BIT + */ +bool L3GD20H::getWaitEnabled() { + I2Cdev::readBit(devAddr, L3GD20H_RA_IG_DURATION, + L3GD20H_WAIT_BIT, buffer); + return buffer[0]; +} + +// LOW_ODR register, r/w + +/** Set whether the DRDY/INT2 pin is active low. If enabled is true then the + * DRDY/INT2 pin will be active low. + * @param enabled New enabled DRDY/INT2 active low configuration + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_DRDY_HL_BIT + */ +void L3GD20H::setINT2DataReadyActiveLowEnabled(bool enabled){ + I2Cdev::writeBit(devAddr, L3GD20H_RA_LOW_ODR, L3GD20H_DRDY_HL_BIT, + enabled); +} + +/** Get whether the DRDY/INT2 pin is active low. If true then the + * DRDY/INT2 pin IS active low. + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_I2C_DIS_BIT + */ +bool L3GD20H::getINT2DataReadyActiveLowEnabled(){ + I2Cdev::readBit(devAddr, L3GD20H_RA_LOW_ODR, + L3GD20H_DRDY_HL_BIT, buffer); + return buffer[0]; +} + +/** Set whether only the SPI interface is enabled (i.e., I2C interface disabled) + * @param enabled New SPI interface only enabled + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_I2C_DIS_BIT + */ +void L3GD20H::setSPIOnlyEnabled(bool enabled){ + I2Cdev::writeBit(devAddr, L3GD20H_RA_LOW_ODR, L3GD20H_I2C_DIS_BIT, + enabled); +} + +/** Get whether only the SPI interface is enabled (i.e., I2C interface disabled) + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_I2C_DIS_BIT + */ +bool L3GD20H::getSPIOnlyEnabled(){ + I2Cdev::readBit(devAddr, L3GD20H_RA_LOW_ODR, + L3GD20H_I2C_DIS_BIT, buffer); + return buffer[0]; +} + +/** Reset the device. Sets a software reset flag which is auto cleared upon boot. + * @param reset Value that resets the device if true + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_SW_RESET_BIT + */ +void L3GD20H::setSoftwareReset(bool reset){ + I2Cdev::writeBit(devAddr, L3GD20H_RA_LOW_ODR, L3GD20H_SW_RESET_BIT, + reset); +} + +/** Set whether the low output data rate is enabled. + * @param enabled New low output data rate enabled + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_LOW_ODR_BIT + */ +void L3GD20H::setLowODREnabled(bool enabled){ + I2Cdev::writeBit(devAddr, L3GD20H_RA_LOW_ODR, L3GD20H_LOW_ODR_BIT, + enabled); +} + +/** Get whether the low output data rate is enabled. + * @see L3GD20H_RA_LOW_ODR + * @see L3GD20H_LOW_ODR_BIT + */ +bool L3GD20H::getLowODREnabled(){ + I2Cdev::readBit(devAddr, L3GD20H_RA_LOW_ODR, + L3GD20H_LOW_ODR_BIT, buffer); + return buffer[0]; +} diff --git a/Arduino/L3GD20H/L3GD20H.h b/Arduino/L3GD20H/L3GD20H.h new file mode 100644 index 00000000..329890e2 --- /dev/null +++ b/Arduino/L3GD20H/L3GD20H.h @@ -0,0 +1,398 @@ +// I2Cdev library collection - L3GD20H I2C device class header file +// Based on STMicroelectronics L3GD20H datasheet rev. 2, 3/2013 +// 3/05/2015 by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-05 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jonathan Arnett, 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 _L3GD20H_H_ +#define _L3GD20H_H_ + +#include "I2Cdev.h" + +#define L3GD20H_ADDRESS 0x6B // I think this is correct. See SAD in doc. +#define L3GD20H_DEFAULT_ADDRESS 0x6B // I think this is correct. See SAD in doc. + +#define L3GD20H_RA_WHO_AM_I 0x0F +#define L3GD20H_RA_CTRL1 0x20 +#define L3GD20H_RA_CTRL2 0x21 +#define L3GD20H_RA_CTRL3 0x22 +#define L3GD20H_RA_CTRL4 0x23 +#define L3GD20H_RA_CTRL5 0x24 +#define L3GD20H_RA_REFERENCE 0x25 +#define L3GD20H_RA_OUT_TEMP 0x26 +#define L3GD20H_RA_STATUS 0x27 +#define L3GD20H_RA_OUT_X_L 0x28 +#define L3GD20H_RA_OUT_X_H 0x29 +#define L3GD20H_RA_OUT_Y_L 0x2A +#define L3GD20H_RA_OUT_Y_H 0x2B +#define L3GD20H_RA_OUT_Z_L 0x2C +#define L3GD20H_RA_OUT_Z_H 0x2D +#define L3GD20H_RA_FIFO_CTRL 0x2E +#define L3GD20H_RA_FIFO_SRC 0x2F +#define L3GD20H_RA_IG_CFG 0x30 +#define L3GD20H_RA_IG_SRC 0x31 +#define L3GD20H_RA_IG_THS_XH 0x32 +#define L3GD20H_RA_IG_THS_XL 0X33 +#define L3GD20H_RA_IG_THS_YH 0X34 +#define L3GD20H_RA_IG_THS_YL 0x35 +#define L3GD20H_RA_IG_THS_ZH 0X36 +#define L3GD20H_RA_IG_THS_ZL 0x37 +#define L3GD20H_RA_IG_DURATION 0X38 +#define L3GD20H_RA_LOW_ODR 0x39 + +#define L3GD20H_ODR_BIT 7 +#define L3GD20H_ODR_LENGTH 2 +#define L3GD20H_BW_BIT 5 +#define L3GD20H_BW_LENGTH 2 +#define L3GD20H_PD_BIT 3 +#define L3GD20H_ZEN_BIT 2 +#define L3GD20H_YEN_BIT 1 +#define L3GD20H_XEN_BIT 0 + +#define L3GD20H_RATE_100_12 0b00 //selection of high vs low rate is via Low_ODR +#define L3GD20H_RATE_200_25 0b01 //selection of high vs low rate is via Low_ODR +#define L3GD20H_RATE_400_50 0b10 //selection of high vs low rate is via Low_ODR +#define L3GD20H_RATE_800_50 0b11 //selection of high vs low rate is via Low_ODR + +#define L3GD20H_BW_LOW 0b00 +#define L3GD20H_BW_MED_LOW 0b01 +#define L3GD20H_BW_MED_HIGH 0b10 +#define L3GD20H_BW_HIGH 0b11 + +#define L3GD20H_HPM_BIT 5 +#define L3GD20H_HPM_LENGTH 2 +#define L3GD20H_HPCF_BIT 3 +#define L3GD20H_HPCF_LENGTH 4 + +#define L3GD20H_HPM_HRF 0b00 //this resets on reading REFERENCE +#define L3GD20H_HPM_REFERENCE 0b01 +#define L3GD20H_HPM_NORMAL 0b10 +#define L3GD20H_HPM_AUTORESET 0b11 + +#define L3GD20H_HPCF1 0b0000 +#define L3GD20H_HPCF2 0b0001 +#define L3GD20H_HPCF3 0b0010 +#define L3GD20H_HPCF4 0b0011 +#define L3GD20H_HPCF5 0b0100 +#define L3GD20H_HPCF6 0b0101 +#define L3GD20H_HPCF7 0b0110 +#define L3GD20H_HPCF8 0b0111 +#define L3GD20H_HPCF9 0b1000 +#define L3GD20H_HPCF10 0b1001 + +#define L3GD20H_INT1_IG_BIT 7 +#define L3GD20H_INT1_BOOT_BIT 6 +#define L3GD20H_H_LACTIVE_BIT 5 +#define L3GD20H_PP_OD_BIT 4 +#define L3GD20H_INT2_DRDY_BIT 3 +#define L3GD20H_INT2_FTH_BIT 2 +#define L3GD20H_INT2_ORUN_BIT 1 +#define L3GD20H_INT2_EMPTY_BIT 0 + +#define L3GD20H_PUSH_PULL 0 +#define L3GD20H_OPEN_DRAIN 1 + +#define L3GD20H_BDU_BIT 7 +#define L3GD20H_BLE_BIT 6 +#define L3GD20H_FS_BIT 5 +#define L3GD20H_FS_LENGTH 2 +#define L3GD20H_IMPEN_BIT 3 //new for this IC: Level sensitive latched enalble +#define L3GD20H_ST_BIT 2 +#define L3GD20H_ST_LENGTH 2 +#define L3GD20H_SIM_BIT 0 + +#define L3GD20H_BIG_ENDIAN 1 +#define L3GD20H_LITTLE_ENDIAN 0 + +#define L3GD20H_FS_250 0b00 +#define L3GD20H_FS_500 0b01 +#define L3GD20H_FS_2000 0b10 + +#define L3GD20H_SELF_TEST_NORMAL 0b00 +#define L3GD20H_SELF_TEST_0 0b01 +#define L3GD20H_SELF_TEST_1 0b11 + +#define L3GD20H_SPI_4_WIRE 0 +#define L3GD20H_SPI_3_WIRE 1 + +#define L3GD20H_BOOT_BIT 7 +#define L3GD20H_FIFO_EN_BIT 6 +#define L3GD20H_STOPONFTH_BIT 5 +#define L3GD20H_HPEN_BIT 4 +#define L3GD20H_IG_SEL_BIT 3 +#define L3GD20H_IG_SEL_LENGTH 2 +#define L3GD20H_OUT_SEL_BIT 1 +#define L3GD20H_OUT_SEL_LENGTH 2 + +#define L3GD20H_NON_HIGH_PASS 0b00 +#define L3GD20H_HIGH_PASS 0b01 +#define L3GD20H_LOW_PASS 0b10 //depends on HPEN +#define L3GD20H_LOW_HIGH_PASS 0b11 //depends on HPEN + +#define L3GD20H_ZYXOR_BIT 7 +#define L3GD20H_ZOR_BIT 6 +#define L3GD20H_YOR_BIT 5 +#define L3GD20H_XOR_BIT 4 +#define L3GD20H_ZYXDA_BIT 3 +#define L3GD20H_ZDA_BIT 2 +#define L3GD20H_YDA_BIT 1 +#define L3GD20H_XDA_BIT 0 + +#define L3GD20H_FIFO_MODE_BIT 7 +#define L3GD20H_FIFO_MODE_LENGTH 3 +#define L3GD20H_FIFO_TH_BIT 4 +#define L3GD20H_FIFO_TH_LENGTH 5 + +#define L3GD20H_FM_BYPASS 0b000 +#define L3GD20H_FM_FIFO 0b001 +#define L3GD20H_FM_STREAM 0b010 +#define L3GD20H_FM_STREAM_FIFO 0b011 +#define L3GD20H_FM_BYPASS_STREAM 0b100 +#define L3GD20H_FM_DYNAMIC_STREAM 0b110 +#define L3GD20H_FM_BYPASS_FIFO 0b111 + + +#define L3GD20H_FIFO_TH_STATUS_BIT 7 +#define L3GD20H_OVRN_BIT 6 +#define L3GD20H_EMPTY_BIT 5 +#define L3GD20H_FIFO_FSS_BIT 4 +#define L3GD20H_FIFO_FSS_LENGTH 5 + +#define L3GD20H_AND_OR_BIT 7 +#define L3GD20H_LIR_BIT 6 +#define L3GD20H_ZHIE_BIT 5 +#define L3GD20H_ZLIE_BIT 4 +#define L3GD20H_YHIE_BIT 3 +#define L3GD20H_YLIE_BIT 2 +#define L3GD20H_XHIE_BIT 1 +#define L3GD20H_XLIE_BIT 0 + +#define L3GD20H_AND_OR_OR 0 +#define L3GD20H_AND_OR_AND 1 + +#define L3GD20H_IA_BIT 6 +#define L3GD20H_ZH_BIT 5 +#define L3GD20H_ZL_BIT 4 +#define L3GD20H_YH_BIT 3 +#define L3GD20H_YL_BIT 2 +#define L3GD20H_XH_BIT 1 +#define L3GD20H_XL_BIT 0 + +#define L3GD20H_DCRM_BIT 7 + +#define L3GD20H_DCRM_RESET 0 +#define L3GD20H_DCRM_DEC 1 + +#define L3GD20H_WAIT_BIT 7 +#define L3GD20H_DUR_BIT 6 +#define L3GD20H_DUR_LENGTH 7 + +#define L3GD20H_LOW_ODR_BIT 0 +#define L3GD20H_SW_RESET_BIT 2 +#define L3GD20H_I2C_DIS_BIT 3 +#define L3GD20H_DRDY_HL_BIT 5 + + + +class L3GD20H { + public: + L3GD20H(); + L3GD20H(uint8_t address); + + void initialize(); + bool testConnection(); + + // WHO_AM_I register, read-only + uint8_t getDeviceID(); + + // CTRL1 register, r/w + void setOutputDataRate(uint16_t rate); + uint16_t getOutputDataRate(); + void setBandwidthCutOffMode(uint8_t mode); + uint8_t getBandwidthCutOffMode(); + // float getBandwidthCutOff(); + void setPowerOn(bool on); + bool getPowerOn(); + void setZEnabled(bool enabled); + bool getZEnabled(); + void setYEnabled(bool enabled); + bool getYEnabled(); + void setXEnabled(bool enabled); + bool getXEnabled(); + + // CTRL2 register, r/w + void setHighPassMode(uint8_t mode); + uint8_t getHighPassMode(); + void setHighPassFilterCutOffFrequencyLevel(uint8_t level); + uint8_t getHighPassFilterCutOffFrequencyLevel(); + + // CTRL3 register, r/w + void setINT1InterruptEnabled(bool enabled); + bool getINT1InterruptEnabled(); + void setINT1BootStatusEnabled(bool enabled); + bool getINT1BootStatusEnabled(); + void interruptActiveINT1Config(); + void setOutputMode(bool mode); + bool getOutputMode(); + void setINT2DataReadyEnabled(bool enabled); + bool getINT2DataReadyEnabled(); + void setINT2FIFOWatermarkInterruptEnabled(bool enabled); + bool getINT2FIFOWatermarkInterruptEnabled(); + void setINT2FIFOOverrunInterruptEnabled(bool enabled); + bool getINT2FIFOOverrunInterruptEnabled(); + void setINT2FIFOEmptyInterruptEnabled(bool enabled); + bool getINT2FIFOEmptyInterruptEnabled(); + + // CTRL4 register, r/w + void setBlockDataUpdateEnabled(bool enabled); + bool getBlockDataUpdateEnabled(); + void setEndianMode(bool endianness); + bool getEndianMode(); + void setFullScale(uint16_t scale); + uint16_t getFullScale(); + void setSelfTestMode(uint8_t mode); + uint8_t getSelfTestMode(); + void setSPIMode(bool mode); + bool getSPIMode(); + + // CTRL5 register, r/w + void rebootMemoryContent(); + void setFIFOEnabled(bool enabled); + bool getFIFOEnabled(); + void setStopOnFIFOThresholdEnabled(bool enabled); + bool getStopOnFIFOThresholdEnabled(); + void setHighPassFilterEnabled(bool enabled); + bool getHighPassFilterEnabled(); + void setDataFilter(uint8_t filter); + uint8_t getDataFilter(); + + // REFERENCE/DATACAPTURE register, r/w + void setHighPassFilterReference(uint8_t reference); + uint8_t getHighPassFilterReference(); + + // OUT_TEMP register, read-only + uint8_t getTemperature(); + + // STATUS register, read-only + bool getXYZOverrun(); + bool getZOverrun(); + bool getYOverrun(); + bool getXOverrun(); + bool getXYZDataAvailable(); + bool getZDataAvailable(); + bool getYDataAvailable(); + bool getXDataAvailable(); + + // OUT_* registers, read-only + void getAngularVelocity(int16_t* x, int16_t* y, int16_t* z); + int16_t getAngularVelocityX(); + int16_t getAngularVelocityY(); + int16_t getAngularVelocityZ(); + + // FIFO_CTRL register, r/w + void setFIFOMode(uint8_t mode); + uint8_t getFIFOMode(); + void setFIFOThreshold(uint8_t wtm); + uint8_t getFIFOThreshold(); + + // FIFO_SRC register, read-only + bool getFIFOAtWatermark(); + bool getFIFOOverrun(); + bool getFIFOEmpty(); + uint8_t getFIFOStoredDataLevel(); + + // IG_CFG register, r/w + void setInterruptCombination(bool combination); + bool getInterruptCombination(); + void setInterruptRequestLatched(bool latched); + bool getInterruptRequestLatched(); + void setZHighInterruptEnabled(bool enabled); + bool getZHighInterruptEnabled(); + void setYHighInterruptEnabled(bool enabled); + bool getYHighInterruptEnabled(); + void setXHighInterruptEnabled(bool enabled); + bool getXHighInterruptEnabled(); + void setZLowInterruptEnabled(bool enabled); + bool getZLowInterruptEnabled(); + void setYLowInterruptEnabled(bool enabled); + bool getYLowInterruptEnabled(); + void setXLowInterruptEnabled(bool enabled); + bool getXLowInterruptEnabled(); + + // IG_SRC register, read-only + bool getInterruptActive(); + bool getZHigh(); + bool getZLow(); + bool getYHigh(); + bool getYLow(); + bool getXHigh(); + bool getXLow(); + + // IG_THS_* registers, r/w + void setDecrementMode(bool mode); + bool getDecrementMode(); + void setXHighThreshold(uint8_t threshold); + uint8_t getXHighThreshold(); + void setXLowThreshold(uint8_t threshold); + uint8_t getXLowThreshold(); + void setYHighThreshold(uint8_t threshold); + uint8_t getYHighThreshold(); + void setYLowThreshold(uint8_t threshold); + uint8_t getYLowThreshold(); + void setZHighThreshold(uint8_t threshold); + uint8_t getZHighThreshold(); + void setZLowThreshold(uint8_t threshold); + uint8_t getZLowThreshold(); + + // IG_DURATION register, r/w + void setDuration(uint8_t duration); + uint8_t getDuration(); + void setWaitEnabled(bool enabled); + bool getWaitEnabled(); + + // LOW_ODR register, r/w + void setINT2DataReadyActiveLowEnabled(bool enabled); + bool getINT2DataReadyActiveLowEnabled(); + void setSPIOnlyEnabled(bool enabled); + bool getSPIOnlyEnabled(); + void setSoftwareReset(bool reset); + void setLowODREnabled(bool enabled); + bool getLowODREnabled(); + + + + + + private: + uint8_t devAddr; + uint8_t buffer[6]; + bool endianMode; +}; + +#endif /* _L3GD20H_H_ */ diff --git a/Arduino/L3GD20H/examples/L3GD20H_basic/L3GD20H_basic.ino b/Arduino/L3GD20H/examples/L3GD20H_basic/L3GD20H_basic.ino new file mode 100644 index 00000000..82953495 --- /dev/null +++ b/Arduino/L3GD20H/examples/L3GD20H_basic/L3GD20H_basic.ino @@ -0,0 +1,122 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for L3GD20H class +// 3/05/2015 by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-05 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jonathan Arnett, 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. +=============================================== +*/ + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#include + +// I2Cdev and L3GD20H must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include +#include + +// specific I2C address may be passed here +L3GD20H gyro; + +int16_t avx, avy, avz; + +#define LED_PIN 13 // (Arduino is 13, Teensy is 6) +bool blinkState = false; + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) + Wire.begin(); + + // initialize serial communication + Serial.begin(9600); + + // initialize device + Serial.println("Initializing I2C devices..."); + gyro.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(gyro.testConnection() ? "L3GD20H connection successful" : "L3GD20H connection failed"); + + //test endian functions + gyro.setEndianMode(not gyro.getEndianMode()); + Serial.print("EndianMode: "); + Serial.print(gyro.getEndianMode()); + gyro.setEndianMode(not gyro.getEndianMode()); + Serial.print(" EndianMode: "); + Serial.println(gyro.getEndianMode()); + + + // configure LED for output + pinMode(LED_PIN, OUTPUT); + + // set scale to 250 + gyro.setFullScale(250); + +} + +void loop() { + // read raw angular velocity measurements from device + gyro.getAngularVelocity(&avx, &avy, &avz); + // int16_t x_single = gyro.getAngularVelocityX(); + // int16_t y_single = gyro.getAngularVelocityY(); + + // //read X memory address directly + // uint8_t xval_l, xval_h; + // uint8_t devAddress = 0x6B; + // uint8_t regAddXL = 0x28; + // uint8_t regAddXH = 0x29; + // I2Cdev::readByte(devAddress, regAddXL, &xval_l); + // I2Cdev::readByte(devAddress, regAddXH, &xval_h); + // //read X memory addresses in single sequential + // uint8_t data[2]; + // I2Cdev::readBytes(devAddress, regAddXL| 0x80, 2, data); + + + // Serial.print("Direct Mem Read: Xl: "); + // Serial.print(xval_l); Serial.print("\tXh: "); + // Serial.print(xval_h); Serial.print("\t"); + // Serial.print("Direct readBytes data[0] data[1]: "); + // Serial.print(data[0]); Serial.print("\t"); + // Serial.print(data[1]); Serial.print("\t"); + // Serial.print("Bitshifted: "); Serial.print((((int16_t)data[1]) << 8) | data[0]); + Serial.print("angular velocity (dps):\t"); + Serial.print(avx*0.00875F,DEC); Serial.print("\t"); + Serial.print(avy*0.00875F,DEC); Serial.print("\t"); + Serial.print(avz*0.00875F,DEC); Serial.print("\t"); + // Serial.print(" x read only: "); Serial.print(x_single); + // Serial.print(" y read only: "); Serial.println(y_single); + Serial.println(); + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + + + +} + + diff --git a/Arduino/LM73/library.json b/Arduino/LM73/library.json new file mode 100644 index 00000000..490ec364 --- /dev/null +++ b/Arduino/LM73/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-LM73", + "version": "1.0.0", + "keywords": "temperature, sensor, i2cdevlib, i2c", + "description": "The LM73 is an integrated, digital-output temperature sensor featuring an incremental Delta-Sigma ADC", + "include": "Arduino/LM73", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/LSM303DLHC/LSM303DLHC.cpp b/Arduino/LSM303DLHC/LSM303DLHC.cpp new file mode 100644 index 00000000..26e121c4 --- /dev/null +++ b/Arduino/LSM303DLHC/LSM303DLHC.cpp @@ -0,0 +1,2149 @@ +// I2Cdev library collection - LSM303DLHC I2C device class +// Based on ST LSM303DLHC datasheet, REV 2, 11/2013 +// [current release date] by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-10 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 [Author Name], 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 "LSM303DLHC.h" + +/** Default constructor, uses default I2C address. + * @see LSM303DLHC_DEFAULT_ADDRESS_A + * @see LSM303DLHC_DEFAULT_ADDRESS_M + */ +LSM303DLHC::LSM303DLHC() { + devAddrA = LSM303DLHC_DEFAULT_ADDRESS_A; + devAddrM = LSM303DLHC_DEFAULT_ADDRESS_M; + endianMode = 0; +} + +/** Specific address constructor. +@param addressA I2C accelerometer address +@param addressM I2C magnetometer address +@see LSM303DLHC_DEFAULT_ADDRESS_A +@see LSM303DLHC_DEFAULT_ADDRESS_M +@see LSM303DLHC_ADDRESS_A +@see LSM303DLHC_ADDRESS_M + */ +LSM303DLHC::LSM303DLHC(uint8_t addressA, uint8_t addressM) { + devAddrA = addressA; + devAddrM = addressM; + endianMode = 0; +} + +/** Power on and prepare for general usage. +All values are defaults except for the data rates for the accelerometer and +magnetometer data rates (200hz and 220hz respectively). +@see LSM303DLHC_RA_CTRL_REG1_A +@see LSM303DLHC_RA_CRA_REG_M +*/ +void LSM303DLHC::initialize() { + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, 0b01100111); + I2Cdev::writeByte(devAddrM, LSM303DLHC_RA_CRA_REG_M, 0b00011100); + // ---------------------------------------------------------------------------- + // STUB TODO: + // Perform any important initialization here. Maybe nothing is required, but + // the method should exist anyway. + // ---------------------------------------------------------------------------- +} + +/** Verify the I2C connection. +Make sure the device is connected and responds as expected. This device has no +device ID or WHO_AM_I register. To test the connection, bits are written to control +register, checked, and then the original contents are written back. + * @return True if connection is valid, false otherwise + */ +bool LSM303DLHC::testConnection() { + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, buffer); + uint8_t origValA = buffer[0]; + I2Cdev::readByte(devAddrM, LSM303DLHC_RA_CRA_REG_M, buffer); + uint8_t origValM = buffer[0]; + + uint8_t zeros = 0b00000000; + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, zeros); + I2Cdev::writeByte(devAddrM, LSM303DLHC_RA_CRA_REG_M, zeros); + + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, buffer); + uint8_t newValA = buffer[0]; + I2Cdev::readByte(devAddrM, LSM303DLHC_RA_CRA_REG_M, buffer); + uint8_t newValM = buffer[0]; + + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, origValA); + I2Cdev::writeByte(devAddrM, LSM303DLHC_RA_CRA_REG_M, origValM); + + if ((newValM == zeros) and (newValA == zeros)) { + return true; + } + return false; +} + +// ---------------------------------------------------------------------------- +// STUB TODO: +// Define methods to fully cover all available functionality provided by the +// device, according to the datasheet and/or register map. Unless there is very +// clear reason not to, try to follow the get/set naming convention for all +// values, for instance: +// - uint8_t getThreshold() +// - void setThreshold(uint8_t threshold) +// - uint8_t getRate() +// - void setRate(uint8_t rate) +// +// Some methods may be named differently if it makes sense. As long as all +// functionality is covered, that's the important part. The methods here are +// only examples and should not be kept for your real device. +// ---------------------------------------------------------------------------- + +//CTRL_REG1_A, r/w + +/** Set the output data rate + * @param rate The new data output rate (can be 1, 10, 25, 50, 100, 200, 400, 1620, 1344, or 5376) + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_ODR_BIT + * @see LSM303DLHC_ODR_LENGTH + * @see LSM303DLHC_RATE_1 + * @see LSM303DLHC_RATE_10 + * @see LSM303DLHC_RATE_25 + * @see LSM303DLHC_RATE_50 + * @see LSM303DLHC_RATE_100 + * @see LSM303DLHC_RATE_200 + * @see LSM303DLHC_RATE_400 + * @see LSM303DLHC_RATE_1620_LP + * @see LSM303DLHC_RATE_1344_N_5376_LP + */ +void LSM303DLHC::setAccelOutputDataRate(uint16_t rate) { + uint8_t writeVal; + + if (rate == 0) { + writeVal = LSM303DLHC_ODR_RATE_POWERDOWN; + } else if (rate == 1) { + writeVal = LSM303DLHC_ODR_RATE_1; + } else if (rate == 10) { + writeVal = LSM303DLHC_ODR_RATE_10; + } else if (rate == 25) { + writeVal = LSM303DLHC_ODR_RATE_25; + } else if (rate == 50) { + writeVal = LSM303DLHC_ODR_RATE_50; + } else if (rate == 100) { + writeVal = LSM303DLHC_ODR_RATE_100; + } else if (rate == 200) { + writeVal = LSM303DLHC_ODR_RATE_200; + } else if (rate == 400) { + writeVal = LSM303DLHC_ODR_RATE_400; + } else if (rate == 1620) { + writeVal = LSM303DLHC_ODR_RATE_1620_LP; + } else if (rate == 1344) { + writeVal = LSM303DLHC_ODR_RATE_1344_N_5376_LP; + } else if (rate == 5376) { + writeVal = LSM303DLHC_ODR_RATE_1344_N_5376_LP; + } + + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_ODR_BIT, + LSM303DLHC_ODR_LENGTH, writeVal); +} + +/** Get the output data rate + * @return The current data output rate (can be 1, 10, 25, 50, 100, 200, 400, 1620, or 1344 (implies 5376)) + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_ODR_BIT + * @see LSM303DLHC_ODR_LENGTH + * @see LSM303DLHC_RATE_1 + * @see LSM303DLHC_RATE_10 + * @see LSM303DLHC_RATE_25 + * @see LSM303DLHC_RATE_50 + * @see LSM303DLHC_RATE_100 + * @see LSM303DLHC_RATE_200 + * @see LSM303DLHC_RATE_400 + * @see LSM303DLHC_RATE_1620_LP + * @see LSM303DLHC_RATE_1344_N_5376_LP + */ +uint16_t LSM303DLHC::getAccelOutputDataRate() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_ODR_BIT, + LSM303DLHC_ODR_LENGTH, buffer); + uint8_t rate = buffer[0]; + + if (rate == LSM303DLHC_ODR_RATE_POWERDOWN) { + return 0; + } else if (rate == LSM303DLHC_ODR_RATE_1) { + return 1; + } else if (rate == LSM303DLHC_ODR_RATE_10) { + return 10; + } else if (rate == LSM303DLHC_ODR_RATE_25) { + return 25; + } else if (rate == LSM303DLHC_ODR_RATE_50) { + return 50; + } else if (rate == LSM303DLHC_ODR_RATE_100) { + return 100; + } else if (rate == LSM303DLHC_ODR_RATE_200) { + return 200; + } else if (rate == LSM303DLHC_ODR_RATE_400) { + return 400; + } else if (rate == LSM303DLHC_ODR_RATE_1620_LP) { + return 1620; + } else if (rate == LSM303DLHC_ODR_RATE_1344_N_5376_LP) { + return 1344; + } else if (rate == LSM303DLHC_ODR_RATE_1344_N_5376_LP) { + return 5376; + } +} + +/*Enables or disables the accelerometer low power mode +@param enabled The new enabled state of the low power mode +@see LSM303DLHC_RA_CTRL_REG1_A +@see LSM303DLHC_LPEN_BIT +*/ +void LSM303DLHC::setAccelLowPowerEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_LPEN_BIT, enabled); +} + +/*Get whether the accelerometer low power mode is enabled +@return True if the acceleromer low power mode is enabled +@see LSM303DLHC_RA_CTRL_REG1_A +@see LSM303DLHC_LPEN_BIT +*/ +bool LSM303DLHC::getAccelLowPowerEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_LPEN_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get Z data + * @param enabled The new enabled state of the Z axis + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_ZEN_BIT + */ +void LSM303DLHC::setAccelZEnabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_ZEN_BIT, enabled); +} + +/** Get whether Z axis data is enabled + * @return True if the Z axis is enabled, false otherwise + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_ZEN_BIT + */ +bool LSM303DLHC::getAccelZEnabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_ZEN_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get Y data + * @param enabled The new enabled state of the Y axis + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_YEN_BIT + */ +void LSM303DLHC::setAccelYEnabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_YEN_BIT, enabled); +} + +/** Get whether Y axis data is enabled + * @return True if the Y axis is enabled, false otherwise + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_YEN_BIT + */ +bool LSM303DLHC::getAccelYEnabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_YEN_BIT, buffer); + return buffer[0]; +} + +/** Enables or disables the ability to get X data + * @param enabled The new enabled state of the X axis + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_XEN_BIT + */ +void LSM303DLHC::setAccelXEnabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_XEN_BIT, enabled); +} + +/** Get whether X axis data is enabled + * @return True if the X axis is enabled, false otherwise + * @see LSM303DLHC_RA_CTRL_REG1_A + * @see LSM303DLHC_XEN_BIT + */ +bool LSM303DLHC::getAccelXEnabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG1_A, LSM303DLHC_XEN_BIT, buffer); + return buffer[0]; +} + +//CTRL_REG2_A r/w + +/** Set the high pass mode + * @param mode The new high pass mode + * @see LSM303DLHC_RA_CTRL_REG2 + * @see LSM303DLHC_HPM_BIT + * @see LSM303DLHC_HPM_LENGTH + * @see LSM303DLHC_HPM_HRF + * @see LSM303DLHC_HPM_REFERENCE + * @see LSM303DLHC_HPM_NORMAL + * @see LSM303DLHC_HPM_AUTORESET + */ +void LSM303DLHC::setAccelHighPassMode(uint8_t mode) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_CTRL_REG2_A, LSM303DLHC_HPM_BIT, + LSM303DLHC_HPM_LENGTH, mode); +} + +/** Get the high pass mode + * @return High pass mode + * @see LSM303DLHC_RA_CTRL_REG2_A + * @see LSM303DLHC_HPM_BIT + * @see LSM303DLHC_HPM_LENGTH + * @see LSM303DLHC_HPM_HRF + * @see LSM303DLHC_HPM_REFERENCE + * @see LSM303DLHC_HPM_NORMAL + * @see LSM303DLHC_HPM_AUTORESET + */ +uint8_t LSM303DLHC::getAccelHighPassMode() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_CTRL_REG2_A, LSM303DLHC_HPM_BIT, + LSM303DLHC_HPM_LENGTH, buffer); + return buffer[0]; +} + +/** Set the high pass filter cut off frequency level (1 - 10) + * @param level The new level for the hpcf, using one of the defined levels + * @see LSM303DLHC_RA_CTRL_REG2_A + * @see LSM303DLHC_HPCF_BIT + * @see LSM303DLHC_HPCF_LENGTH + * @see LSM303DLHC_HPCF1 + * @see LSM303DLHC_HPCF2 + * @see LSM303DLHC_HPCF3 + * @see LSM303DLHC_HPCF4 + */ +void LSM303DLHC::setAccelHighPassFilterCutOffFrequencyLevel(uint8_t level) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_CTRL_REG2_A, LSM303DLHC_HPCF_BIT, + LSM303DLHC_HPCF_LENGTH, level); +} + +/** Get the high pass filter cut off frequency level (1 - 10) + * @return High pass filter cut off frequency level + * @see LSM303DLHC_RA_CTRL_REG2_A + * @see LSM303DLHC_HPCF_BIT + * @see LSM303DLHC_HPCF_LENGTH + * @see LSM303DLHC_HPCF1 + * @see LSM303DLHC_HPCF2 + * @see LSM303DLHC_HPCF3 + * @see LSM303DLHC_HPCF4 + */ +uint8_t LSM303DLHC::getAccelHighPassFilterCutOffFrequencyLevel() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_CTRL_REG2_A, LSM303DLHC_HPCF_BIT, + LSM303DLHC_HPCF_LENGTH, buffer); + return buffer[0]; +} + +//CTRL_REG3_A r/w + +/*Enable the Click interrupt routed to the INT1 pin. +@param enabled The new enabled state of the Click interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_CLICK_BIT +*/ +void LSM303DLHC::setAccelINT1ClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_CLICK_BIT, enabled); +} + +/*Get whether the Click interrupt is routed to the INT1 pin. +@return True if the Click interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_CLICK_BIT +*/ +bool LSM303DLHC::getAccelINT1ClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_CLICK_BIT, buffer); + return buffer[0]; +} + +/*Enable the AOR1 interrupt routed to the INT1 pin. +@param enabled The new enabled state of the AOR1 interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_AOI1_BIT +*/ +void LSM303DLHC::setAccelINT1AOI1Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_AOI1_BIT, enabled); +} + +/*Get whether the AOR1 interrupt is routed to the INT1 pin. +@return True if the AOR1 interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_AOI1_BIT +*/ +bool LSM303DLHC::getAccelINT1AOI1Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_AOI1_BIT, buffer); + return buffer[0]; +} + +/*Enable the AOR2 interrupt routed to the INT1 pin. +@param enabled The new enabled state of the AOR2 interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_AOI2_BIT +*/ +void LSM303DLHC::setAccelINT1AOI2Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_AOI2_BIT, enabled); +} + +/*Get whether the AOR2 interrupt is routed to the INT1 pin. +@return True if the AOR2 interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_AOI2_BIT +*/ +bool LSM303DLHC::getAccelINT1AOI2Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_AOI2_BIT, buffer); + return buffer[0]; +} + +/*Enable the Data Ready 1 interrupt routed to the INT1 pin. +@param enabled The new enabled state of the Data Ready 1 interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_DRDY1_BIT +*/ +void LSM303DLHC::setAccelINT1DataReady1Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_DRDY1_BIT, enabled); +} + +/*Get whether the Data Ready 1 interrupt is routed to the INT1 pin. +@return True if the Data Ready 1 interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_DRDY1_BIT +*/ +bool LSM303DLHC::getAccelINT1DataReady1Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_DRDY1_BIT, buffer); + return buffer[0]; +} + +/*Enable the Data Ready 2 interrupt routed to the INT1 pin. +@param enabled The new enabled state of the Data Ready 2 interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_DRDY2_BIT +*/ +void LSM303DLHC::setAccelINT1DataReady2Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_DRDY2_BIT, enabled); +} + +/*Get whether the Data Ready 2 interrupt is routed to the INT1 pin. +@return True if the Data Ready 2 interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_DRDY2_BIT +*/ +bool LSM303DLHC::getAccelINT1DataReady2Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_DRDY2_BIT, buffer); + return buffer[0]; +} + +/*Enable the FIFO watermark interrupt routed to the INT1 pin. +@param enabled The new enabled state of the FIFO watermark interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_WTM_BIT +*/ +void LSM303DLHC::setAccelINT1FIFOWatermarkEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_WTM_BIT, enabled); +} + +/*Get whether the FIFO watermark interrupt is routed to the INT1 pin. +@return True if the FIFO watermark interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_WTM_BIT +*/ +bool LSM303DLHC::getAccelINT1FIFOWatermarkEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_WTM_BIT, buffer); + return buffer[0]; +} + +/*Enable the FIFO overrun interrupt routed to the INT1 pin. +@param enabled The new enabled state of the FIFO overrun interrupt routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_OVERRUN_BIT +*/ +void LSM303DLHC::setAccelINT1FIFOOverunEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_OVERRUN_BIT, enabled); +} + +/*Get whether the FIFO overrun interrupt is routed to the INT1 pin. +@return True if the FIFO overrun interrupt is routed to the INT1 pin +@see LSM303DLHC_RA_CTRL_REG3_A +@see LSM303DLHC_I1_OVERRUN_BIT +*/ +bool LSM303DLHC::getAccelINT1FIFOOverunEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG3_A, LSM303DLHC_I1_OVERRUN_BIT, buffer); + return buffer[0]; +} + +//CTRL_REG4_A r/w + +/** Set the Block Data Update (BDU) enabled state + * @param enabled New BDU enabled state + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_BDU_BIT + */ +void LSM303DLHC::setAccelBlockDataUpdateEnabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_BDU_BIT, enabled); +} + +/** Get the BDU enabled state + * @return True if Block Data Update is enabled, false otherwise + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_BDU_BIT + */ +bool LSM303DLHC::getAccelBlockDataUpdateEnabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_BDU_BIT, buffer); + return buffer[0]; +} + +/** Set the data endian modes + * In Big Endian mode, the Most Significat Byte (MSB) is on the lower address, + * and the Least Significant Byte (LSB) is on the higher address. Little Endian + * mode reverses this order. Little Endian is the default mode. + * @param endianness New endian mode + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_BLE_BIT + * @see LSM303DLHC_BIG_ENDIAN + * @see LSM303DLHC_LITTLE_ENDIAN + */ +void LSM303DLHC::setAccelEndianMode(bool endianness) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_BLE_BIT, + endianness); + endianMode = getAccelEndianMode(); +} + +/** Get the data endian mode + * @return Current endian mode + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_BLE_BIT + * @see LSM303DLHC_BIG_ENDIAN + * @see LSM303DLHC_LITTLE_ENDIAN + */ +bool LSM303DLHC::getAccelEndianMode() { + return endianMode; + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_BLE_BIT, + buffer); + return buffer[0]; +} + +/** Set the full scale of the data output (in dps) + * @param scale The new scale of the data output (250, 500, 2000) + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_FS_BIT + * @see LSM303DLHC_FS_LENGTH + * @see LSM303DLHC_FS_2 + * @see LSM303DLHC_FS_4 + * @see LSM303DLHC_FS_8 + * @see LSM303DLHC_FS_16 + */ +void LSM303DLHC::setAccelFullScale(uint8_t scale) { + uint8_t writeBits; + + if (scale == 2) { + writeBits = LSM303DLHC_FS_2; + } else if (scale == 4) { + writeBits = LSM303DLHC_FS_4; + } else if (scale == 8) { + writeBits = LSM303DLHC_FS_8; + } else { + writeBits = LSM303DLHC_FS_16; + } + + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_FS_BIT, + LSM303DLHC_FS_LENGTH, writeBits); +} + +/** Get the current full scale of the output data (in dps) + * @return Current scale of the output data + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_FS_BIT + * @see LSM303DLHC_FS_LENGTH + * @see LSM303DLHC_FS_2 + * @see LSM303DLHC_FS_4 + * @see LSM303DLHC_FS_8 + * @see LSM303DLHC_FS_16 + */ +uint8_t LSM303DLHC::getAccelFullScale() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, + LSM303DLHC_FS_BIT, LSM303DLHC_FS_LENGTH, buffer); + uint8_t readBits = buffer[0]; + + if (readBits == LSM303DLHC_FS_2) { + return 2; + } else if (readBits == LSM303DLHC_FS_4) { + return 4; + } else if (readBits == LSM303DLHC_FS_8) { + return 8; + } else { + return 16; + } +} + +/*Enables or disables high resolution output. +@param enabled New enabled state of high resolution output +@see LSM303DLHC_RA_CTRL_REG4_A +@see LSM303DLHC_HR_BIT +*/ +void LSM303DLHC::setAccelHighResOutputEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_HR_BIT, + enabled); +} + +/*Gets whether high resolution output is enabled. +@return True if high resolution output is enabled +@see LSM303DLHC_RA_CTRL_REG4_A +@see LSM303DLHC_HR_BIT +*/ +bool LSM303DLHC::getAccelHighResOutputEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_HR_BIT, + buffer); + return buffer[0]; +} + +/** Set the SPI mode + * @param mode New SPI mode + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_SIM_BIT + * @see LSM303DLHC_SIM_4W + * @see LSM303DLHC_SIM_3W + */ +void LSM303DLHC::setAccelSPIMode(bool mode) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_SIM_BIT, mode); +} + +/** Get the SPI mode + * @return Current SPI mode + * @see LSM303DLHC_RA_CTRL_REG4_A + * @see LSM303DLHC_SIM_BIT + * @see LSM303DLHC_SIM_4W + * @see LSM303DLHC_SIM_3W + */ +bool LSM303DLHC::getAccelSPIMode() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG4_A, LSM303DLHC_SIM_BIT, + buffer); + return buffer[0]; +} + +//CTRL_REG5_A r/w + +/** Reboots the FIFO memory content + * @see LSM303DLHC_RA_CTRL_REG5_A + * @see LSM303DLHC_BOOT_BIT + */ +void LSM303DLHC::rebootAccelMemoryContent() { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_BOOT_BIT, true); +} + +/** Set whether the FIFO buffer is enabled + * @param enabled New enabled state of the FIFO buffer + * @see LSM303DLHC_RA_CTRL_REG5_A + * @see LSM303DLHC_FIFO_EN_BIT + */ +void LSM303DLHC::setAccelFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_FIFO_EN_BIT, + enabled); +} + +/** Get whether the FIFO buffer is enabled + * @return True if the FIFO buffer is enabled, false otherwise + * @see LSM303DLHC_RA_CTRL_REG5_A + * @see LSM303DLHC_FIFO_EN_BIT + */ +bool LSM303DLHC::getAccelFIFOEnabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_FIFO_EN_BIT, + buffer); + return buffer[0]; +} + +/*Enable latching of interrupt requrest 1 +@param latched New enabled state of latching interrupt request 1 +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_LIR_INT1_BIT +*/ +void LSM303DLHC::setAccelInterrupt1RequestLatched(bool latched){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_LIR_INT1_BIT, + latched); +} + +/*Get whether latching of interrupt request 1 is enabled. +@return True if latching of interrupt request 1 is enabled +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_LIR_INT1_BIT +*/ +bool LSM303DLHC::getAccelInterrupt1RequestLatched(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_LIR_INT1_BIT, + buffer); + return buffer[0]; +} + +/*Enable latching of interrupt requrest 2 +@param latched New enabled state of latching interrupt request 2 +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_LIR_INT2_BIT +*/ +void LSM303DLHC::setAccelInterrupt2RequestLatched(bool latched){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_LIR_INT2_BIT, + latched); +} + +/*Get whether latching of interrupt request 2 is enabled. +@return True if latching of interrupt request 2 is enabled +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_LIR_INT2_BIT +*/ +bool LSM303DLHC::getAccelInterrupt2RequestLatched(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_LIR_INT2_BIT, + buffer); + return buffer[0]; +} + +/*Enable 4D dectection interrupt 1 +@param enabled New enabled state of the 4D detection interrupt 1 +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_D4D_INT1_BIT +*/ +void LSM303DLHC::setAccelDetect4DInterrupt1Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_D4D_INT1_BIT, + enabled); +} + +/*Get whether 4D detection interrupt 1 is enabled. +@return True if 4D detection interrupt 1 is enabled +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_D4D_INT1_BIT +*/ +bool LSM303DLHC::getAccelDetect4DInterrupt1Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_D4D_INT1_BIT, + buffer); + return buffer[0]; +} + +/*Enable 4D dectection interrupt 2 +@param enabled New enabled state of the 4D detection interrupt 2 +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_D4D_INT2_BIT +*/ +void LSM303DLHC::setAccelDetect4DInterrupt2Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_D4D_INT2_BIT, + enabled); +} + +/*Get whether 4D detection interrupt 2 is enabled. +@return True if 4D detection interrupt 2 is enabled +@see LSM303DLHC_RA_CTRL_REG5_A +@see LSM303DLHC_D4D_INT2_BIT +*/ +bool LSM303DLHC::getAccelDetect4DInterrupt2Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG5_A, LSM303DLHC_D4D_INT2_BIT, + buffer); + return buffer[0]; +} + +//CTRL_REG6_A r/w + +//TODO: +/*Enable the Click interrupt routed to the INT2 pin. +@param enabled The new enabled state of the Click interrupt routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_CLICK_BIT +*/ +void LSM303DLHC::setAccelINT2ClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_CLICK_BIT, enabled); +} + +/*Get whether the Click interrupt is routed to the INT2 pin. +@return True if the Click interrupt is routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_CLICK_BIT +*/ +bool LSM303DLHC::getAccelINT2ClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_CLICK_BIT, buffer); + return buffer[0]; +} + +/*Enable interrupt 1 routed to the INT2 pin. +@param enabled The new enabled state of whether interrupt 1 routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_INT1_BIT +*/ +void LSM303DLHC::setAccelINT2Interrupt1Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_INT1_BIT, enabled); +} + +/*Get whether interrupt 1 is routed to the INT2 pin. +@return True if interrupt 1 is routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_INT1_BIT +*/ +bool LSM303DLHC::getAccelINT2Interrupt1Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_INT1_BIT, buffer); + return buffer[0]; +} + +/*Enable interrupt 2 routed to the INT2 pin. +@param enabled The new enabled state of whether interrupt 2 routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_INT2_BIT +*/ +void LSM303DLHC::setAccelINT2Interrupt2Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_INT2_BIT, enabled); +} + +/*Get whether interrupt 2 is routed to the INT2 pin. +@return True if interrupt 2 is routed to the INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_I2_INT2_BIT +*/ +bool LSM303DLHC::getAccelINT2Interrupt2Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_I2_INT2_BIT, buffer); + return buffer[0]; +} + +/*Enable memory content reboot via INT2 pin +@param enabled The new state of whether memory content is rebooted via INT2 pin +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_BOOT_I1_BIT +*/ +void LSM303DLHC::setAccelRebootMemoryContentINT2Enabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_BOOT_I1_BIT, enabled); +} + +/*Get whether memory content reboot is enabled via INT2 pin +@return True if memory content reboot is via INT2 pin is enabled +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_BOOT_I1_BIT +*/ +bool LSM303DLHC::getAccelRebootMemoryContentINT2Enabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_BOOT_I1_BIT, buffer); + return buffer[0]; +} + +// // TODO Documentation +// void setAccelActiveFunctionStatusINT2Enabled(bool enabled); + +// // TODO Documentation +// bool getAccelActiveFunctionStatusINT2Enabled(); + +/*Enable active low interrupts. +@param enable The new state of whether active low interrupts are enabled +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_H_ACTIVE_BIT +*/ +void LSM303DLHC::setAccelInterruptActiveLowEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_H_ACTIVE_BIT, enabled); +} + +/*Get whether active low interrupts are enabled. +@return True if active low interrupts are enabled. +@see LSM303DLHC_RA_CTRL_REG6_A +@see LSM303DLHC_H_ACTIVE_BIT +*/ +bool LSM303DLHC::getAccelInterruptActiveLowEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CTRL_REG6_A, LSM303DLHC_H_ACTIVE_BIT, buffer); + return buffer[0]; +} + +//REFERENCE_A, r/w + +/** Set the reference value for interrupt generation + * @param reference New reference value for interrupt generation + * @see LSM303DLHC_RA_REFERENCE_A + */ +void LSM303DLHC::setAccelInterruptReference(uint8_t reference) { + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_REFERENCE_A, reference); +} + +/** Get the 8-bit reference value for interrupt generation + * @return 8-bit reference value for interrupt generation + * @see LSM303DLHC_RA_REFERENCE + */ +uint8_t LSM303DLHC::getAccelInterruptReference() { + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_REFERENCE_A, buffer); + return buffer[0]; +} + +//STATUS_REG_A r + +/** Get whether new data overwrote the last set of data before it was read + * @return True if the last set of data was overwritten before being read, false + * otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_ZYXOR_BIT + */ +bool LSM303DLHC::getAccelXYZOverrun() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_ZYXOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new Z data overwrote the last set of data before it was read + * @return True if the last set of Z data was overwritten before being read, + * false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_ZOR_BIT + */ +bool LSM303DLHC::getAccelZOverrun() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_ZOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new Y data overwrote the last set of data before it was read + * @return True if the last set of Y data was overwritten before being read, + * false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_YOR_BIT + */ +bool LSM303DLHC::getAccelYOverrun() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_YOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether new X data overwrote the last set of data before it was read + * @return True if the last set of X data was overwritten before being read, + * false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_XOR_BIT + */ +bool LSM303DLHC::getAccelXOverrun() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_XOR_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new data avaialable + * @return True if there is new data available, false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_ZYXDA_BIT + */ +bool LSM303DLHC::getAccelXYZDataAvailable() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_ZYXDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new Z data avaialable + * @return True if there is new Z data available, false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_ZDA_BIT + */ +bool LSM303DLHC::getAccelZDataAvailable() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_ZDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new Y data avaialable + * @return True if there is new Y data available, false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_YDA_BIT + */ +bool LSM303DLHC::getAccelYDataAvailable() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_YDA_BIT, + buffer); + return buffer[0]; +} + +/** Get whether there is new X data avaialable + * @return True if there is new X data available, false otherwise + * @see LSM303DLHC_RA_STATUS_REG_A + * @see LSM303DLHC_XDA_BIT + */ +bool LSM303DLHC::getAccelXDataAvailable() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_STATUS_REG_A, LSM303DLHC_XDA_BIT, + buffer); + return buffer[0]; +} + +//OUT_*_A, r + +//TODO: +//Fix getAccerlation to read all 6 regs, and fix endian internal state + +/** Get the acceleration for all 3 axes + * Due to the fact that this device supports two difference Endian modes, both + * must be accounted for when reading data. In Little Endian mode, the first + * byte (lowest address) is the least significant and in Big Endian mode the + * first byte is the most significant. + * @param x 16-bit integer container for the X-axis acceleration + * @param y 16-bit integer container for the Y-axis acceleration + * @param z 16-bit integer container for the Z-axis acceleration + */ +void LSM303DLHC::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddrA, LSM303DLHC_RA_OUT_X_L_A | 0x80, 6, buffer); + if (endianMode == LSM303DLHC_LITTLE_ENDIAN) { + *x = (((int16_t)buffer[1]) << 8) | buffer[0]; + *y = (((int16_t)buffer[3]) << 8) | buffer[2]; + *z = (((int16_t)buffer[5]) << 8) | buffer[4]; + } else { + *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 the acceleration along the X-axis + * @return acceleration along the X-axis + * @see LSM303DLHC_RA_OUT_X_L_A + * @see LSM303DLHC_RA_OUT_X_H_A + */ +int16_t LSM303DLHC::getAccelerationX() { + I2Cdev::readBytes(devAddrA, LSM303DLHC_RA_OUT_X_L_A | 0x80, 2, buffer); + if (endianMode == LSM303DLHC_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +/** Get the acceleration along the Y-axis + * @return acceleration along the Y-axis + * @see LSM303DLHC_RA_OUT_Y_L_A + * @see LSM303DLHC_RA_OUT_Y_H_A + */ +int16_t LSM303DLHC::getAccelerationY() { + I2Cdev::readBytes(devAddrA, LSM303DLHC_RA_OUT_Y_L_A | 0x80, 2, buffer); + if (endianMode == LSM303DLHC_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +/** Get the acceleration along the Z-axis + * @return acceleration along the Z-axis + * @see LSM303DLHC_RA_OUT_Z_L_A + * @see LSM303DLHC_RA_OUT_Z_H_A + */ +int16_t LSM303DLHC::getAccelerationZ() { + I2Cdev::readBytes(devAddrA, LSM303DLHC_RA_OUT_Z_L_A | 0x80, 2, buffer); + if (endianMode == LSM303DLHC_LITTLE_ENDIAN) { + return (((int16_t)buffer[1]) << 8) | buffer[0]; + } else { + return (((int16_t)buffer[0]) << 8) | buffer[1]; + } +} + +//FIFO_CTRL_REG_A, rw + +/** Set the FIFO mode to one of the defined modes + * @param mode New FIFO mode + * @see LSM303DLHC_RA_FIFO_CTRL_REG_A + * @see LSM303DLHC_FM_BIT + * @see LSM303DLHC_FM_LENGTH + * @see LSM303DLHC_FM_BYPASS + * @see LSM303DLHC_FM_FIFO + * @see LSM303DLHC_FM_STREAM + * @see LSM303DLHC_FM_TRIGGER + */ +void LSM303DLHC::setAccelFIFOMode(uint8_t mode) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, LSM303DLHC_FM_BIT, + LSM303DLHC_FM_LENGTH, mode); +} + +/** Get the FIFO mode to one of the defined modes + * @return Current FIFO mode + * @see LSM303DLHC_RA_FIFO_CTRL_REG_A + * @see LSM303DLHC_FM_BIT + * @see LSM303DLHC_FM_LENGTH + * @see LSM303DLHC_FM_BYPASS + * @see LSM303DLHC_FM_FIFO + * @see LSM303DLHC_FM_STREAM + * @see LSM303DLHC_FM_TRIGGER + */ +uint8_t LSM303DLHC::getAccelFIFOMode() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, + LSM303DLHC_FM_BIT, LSM303DLHC_FM_LENGTH, buffer); + return buffer[0]; +} + +/*Set the trigger selection to INT1 or INT2 +@param trigger The new trigger selection +@see LSM303DLHC_RA_FIFO_CTRL_REG_A +@see LSM303DLHC_TR_BIT +@see LSM303DLHC_TR_INT1 +@see LSM303DLHC_TR_INT2 +*/ +void LSM303DLHC::setAccelFIFOTriggerINT(bool trigger){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, + LSM303DLHC_TR_BIT, trigger); +} + +/*Get the trigger selection (INT1 or INT2) +@return trigger The new trigger selection (0 for INT1, 1 for INT2) +@see LSM303DLHC_RA_FIFO_CTRL_REG_A +@see LSM303DLHC_TR_BIT +@see LSM303DLHC_TR_INT1 +@see LSM303DLHC_TR_INT2 +*/ +bool LSM303DLHC::getAccelFIFOTriggerINT(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, + LSM303DLHC_TR_BIT, buffer); + return buffer[0]; +} + +/** Set the FIFO watermark threshold + * @param wtm New FIFO watermark threshold + * @see LSM303DLHC_RA_FIFO_CTRL_REG_A + * @see LSM303DLHC_FTH_BIT + * @see LSM303DLHC_FTH_LENGTH + */ +void LSM303DLHC::setAccelFIFOThreshold(uint8_t wtm) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, LSM303DLHC_FTH_BIT, + LSM303DLHC_FTH_LENGTH, wtm); +} + +/** Get the FIFO watermark threshold + * @return FIFO watermark threshold + * @see LSM303DLHC_RA_FIFO_CTRL_REG_A + * @see LSM303DLHC_FTH_BIT + * @see LSM303DLHC_FTH_LENGTH + */ +uint8_t LSM303DLHC::getAccelFIFOThreshold() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_FIFO_CTRL_REG_A, LSM303DLHC_FTH_BIT, + LSM303DLHC_FTH_LENGTH, buffer); + return buffer[0]; +} + + +//FIFO_SRC_REG_A, r + +/** Get whether the number of data sets in the FIFO buffer is less than the + * watermark + * @return True if the number of data sets in the FIFO buffer is more than or + * equal to the watermark, false otherwise. + * @see LSM303DLHC_RA_FIFO_SRC_REG_A + * @see LSM303DLHC_WTM_BIT + */ +bool LSM303DLHC::getAccelFIFOAtWatermark() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_FIFO_SRC_REG_A, LSM303DLHC_WTM_BIT, + buffer); + return buffer[0]; +} + +/** Get whether the FIFO buffer is full + * @return True if the FIFO buffer is full, false otherwise + * @see LSM303DLHC_RA_FIFO_SRC_REG_A + * @see LSM303DLHC_OVRN_FIFO_BIT + */ +bool LSM303DLHC::getAccelFIFOOverrun() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_FIFO_SRC_REG_A, + LSM303DLHC_OVRN_FIFO_BIT, buffer); + return buffer[0]; +} + +/** Get whether the FIFO buffer is empty + * @return True if the FIFO buffer is empty, false otherwise + * @see LSM303DLHC_RA_FIFO_SRC_REG_A + * @see LSM303DLHC_EMPTY_BIT + */ +bool LSM303DLHC::getAccelFIFOEmpty() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_FIFO_SRC_REG_A, + LSM303DLHC_EMPTY_BIT, buffer); + return buffer[0]; +} + +/** Get the number of filled FIFO buffer slots + * @return Number of filled slots in the FIFO buffer + * @see LSM303DLHC_RA_FIFO_SRC_REG_A + * @see LSM303DLHC_FSS_BIT + * @see LSM303DLHC_FSS_LENGTH + */ +uint8_t LSM303DLHC::getAccelFIFOStoredSamples() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_FIFO_SRC_REG_A, + LSM303DLHC_FSS_BIT, LSM303DLHC_FSS_LENGTH, buffer); + return buffer[0]; +} + +//INT1_CFG_A, w/r + +/** Set the combination mode for interrupt 1events + * @param combination New combination mode for interrupt 1 events. + * LSM303DLHC_INT1_OR for OR and LSM303DLHC_INT1_AND for AND + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_AOI_BIT + * @see LSM303DLHC_INT1_OR + * @see LSM303DLHC_INT1_AND + */ +void LSM303DLHC::setAccelInterrupt1Combination(bool combination) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_AOI_BIT, + combination); +} + +/** Get the combination mode for interrupt 1 events + * @return Combination mode for interrupt 1 events. LSM303DLHC_INT1_OR for OR and + * LSM303DLHC_INT1_AND for AND + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_AOI_BIT + * @see LSM303DLHC_INT1_OR + * @see LSM303DLHC_INT1_AND + */ +bool LSM303DLHC::getAccelInterrupt1Combination() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_AOI_BIT, + buffer); + return buffer[0]; +} + +/*Enable 6D dectection interrupt 1. See datasheet for how 4D detection is affected. +@param enabled New enabled state of the 6D detection interrupt 1 +@see LSM303DLHC_RA_INT1_CFG_A +@see LSM303DLHC_INT1_6D_BIT +*/ +void LSM303DLHC::setAccelInterrupt16DEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_6D_BIT, enabled); +} + +/*Get enable status of 6D dectection interrupt 1. See datasheet for how 4D detection is affected. +@return True if 6D detection interrupt 1 is enabled +@see LSM303DLHC_RA_INT1_CFG_A +@see LSM303DLHC_INT1_6D_BIT +*/ +bool LSM303DLHC::getAccelInterrupt16DEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_6D_BIT, buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for Z high/up is enabled + * @param enabled New enabled state for Z high/up interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_ZHIE_ZUPE_BIT + */ +void LSM303DLHC::setAccelZHighUpInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_ZHIE_ZUPE_BIT, enabled); +} + +/** Get whether the interrupt 1 for Z high/up is enabled + * @return True if the interrupt 1 for Z high/up is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_ZHIE_ZUPE_BIT + */ +bool LSM303DLHC::getAccelZHighUpInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_ZHIE_ZUPE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for Z low/down is enabled + * @param enabled New enabled state for Z low/down interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_ZLIE_ZDOWNE_BIT + */ +void LSM303DLHC::setAccelZLowDownInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_ZLIE_ZDOWNE_BIT, enabled); +} + +/** Get whether the interrupt 1 for Z low/down is enabled + * @return True if the interrupt 1 for Z low/down is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_ZLIE_ZDOWNE_BIT + */ +bool LSM303DLHC::getAccelZLowDownInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_ZLIE_ZDOWNE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for Y high/up is enabled + * @param enabled New enabled state for Y high/up interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_YHIE_YUPE_BIT + */ +void LSM303DLHC::setAccelYHighUpInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_YHIE_YUPE_BIT, enabled); +} + +/** Get whether the interrupt 1 for Y high/up is enabled + * @return True if the interrupt 1 for Y high/up is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_YHIE_YUPE_BIT + */ +bool LSM303DLHC::getAccelYHighUpInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_YHIE_YUPE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for Y low/down is enabled + * @param enabled New enabled state for Y low/down interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_YLIE_YDOWNE_BIT + */ +void LSM303DLHC::setAccelYLowDownInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_YLIE_YDOWNE_BIT, enabled); +} + +/** Get whether the interrupt 1 for Y low/down is enabled + * @return True if the interrupt 1 for Y low/down is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_YLIE_YDOWNE_BIT + */ +bool LSM303DLHC::getAccelYLowDownInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_YLIE_YDOWNE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for X high/up is enabled + * @param enabled New enabled state for X high/up interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_XHIE_XUPE_BIT + */ +void LSM303DLHC::setAccelXHighUpInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_XHIE_XUPE_BIT, enabled); +} + +/** Get whether the interrupt 1 for X high/up is enabled + * @return True if the interrupt 1 for X high/up is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_XHIE_XUPE_BIT + */ +bool LSM303DLHC::getAccelXHighUpInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_XHIE_XUPE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 1 for X low/down is enabled + * @param enabled New enabled state for/down X low interrupt. + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_XLIE_XDOWNE_BIT + */ +void LSM303DLHC::setAccelXLowDownInterrupt1Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_XLIE_XDOWNE_BIT, enabled); +} + +/** Get whether the interrupt 1 for X low/down is enabled + * @return True if the interrupt 1 for X low/down is enabled, false otherwise + * @see LSM303DLHC_RA_INT1_CFG_A + * @see LSM303DLHC_INT1_XLIE_XDOWNE_BIT + */ +bool LSM303DLHC::getAccelXLowDownInterrupt1Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT1_CFG_A, LSM303DLHC_INT1_XLIE_XDOWNE_BIT, + buffer); + return buffer[0]; +} + +//INT1_SRC_A, r + +/*Get the contents of the INT1_SRC_A register to determine if any and which interrupts have occured. +Reading this register will clear any interrupts that were lateched. +@return the 8 bit contents of the INT1_SRC_a register +@see LSM303DLHC_RA_INT1_SRC_A +@see LSM303DLHC_INT1_IA_BIT +@see LSM303DLHC_INT1_ZH_BIT +@see LSM303DLHC_INT1_ZL_BIT +@see LSM303DLHC_INT1_YH_BIT +@see LSM303DLHC_INT1_YL_BIT +@see LSM303DLHC_INT1_XH_BIT +@see LSM303DLHC_INT1_XL_BIT +*/ +uint8_t LSM303DLHC::getAccelInterrupt1Source() { + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_INT1_SRC_A, buffer); + return buffer[0]; +} + +//INT1_THS_A, w/r + +/** Set the threshold for interrupt 1 + * @param threshold New threshold for interrupt 1 + * @see LSM303DLHC_RA_INT1_THS_A + */ +void LSM303DLHC::setAccelInterrupt1Threshold(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_INT1_THS_A, value); +} + +/** Retrieve the threshold interrupt 1 + * @return interrupt 1 threshold + * @see LSM303DLHC_RA_INT1_THS_A + */ +uint8_t LSM303DLHC::getAccelInterupt1Threshold(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_INT1_THS_A, buffer); + return buffer[0]; +} + +//INT1_DURATION_A, r/w + +/* Set the minimum event duration for interrupt 1 to be generated + * This depends on the chosen output data rate + * @param duration New duration necessary for interrupt 1 to be + * generated + * @see LSM303DLHC_RA_INT1_DURATION_A + * @see LSM303DLHC_INT1_DURATION_BIT + * @see LSM303DLHC_INT1_DURATION_LENGTH + */ +void LSM303DLHC::setAccelInterrupt1Duration(uint8_t duration) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_INT1_DURATION_A, LSM303DLHC_INT1_DURATION_BIT, + LSM303DLHC_INT1_DURATION_LENGTH, duration); +} + +/** Get the minimum event duration for interrupt 1 to be generated + * @return Duration necessary for interrupt 1 to be generated + * @see LSM303DLHC_RA_INT1_DURATION_A + * @see LSM303DLHC_INT1_DURATION_BIT + * @see LSM303DLHC_INT1_DURATION_LENGTH + */ +uint8_t LSM303DLHC::getAccelInterrupt1Duration() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_INT1_DURATION_A, + LSM303DLHC_INT1_DURATION_BIT, LSM303DLHC_INT1_DURATION_LENGTH, buffer); + return buffer[0]; +} + +//INT2_CFG_A, r/w + +/** Set the combination mode for interrupt 2 events + * @param combination New combination mode for interrupt events. + * LSM303DLHC_INT1_OR for OR and LSM303DLHC_INT1_AND for AND + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_AOI_BIT + * @see LSM303DLHC_INT1_OR + * @see LSM303DLHC_INT1_AND + */ +void LSM303DLHC::setAccelInterrupt2Combination(bool combination) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_AOI_BIT, + combination); +} + +/** Get the combination mode for interrupt 2 events + * @return Combination mode for interrupt events. LSM303DLHC_INT1_OR for OR and + * LSM303DLHC_INT1_AND for AND + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_AOI_BIT + * @see LSM303DLHC_INT1_OR + * @see LSM303DLHC_INT1_AND + */ +bool LSM303DLHC::getAccelInterrupt2Combination() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_AOI_BIT, + buffer); + return buffer[0]; +} + +/*Enable 6D dectection interrupt 2. See datasheet for how 4D detection is affected. +@param enabled New enabled state of the 6D detection interrupt 1 +@see LSM303DLHC_RA_INT2_CFG_A +@see LSM303DLHC_INT2_6D_BIT +*/ +void LSM303DLHC::setAccelInterrupt26DEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_6D_BIT, enabled); +} + +/*Get enable status of 6D dectection interrupt 2. See datasheet for how 4D detection is affected. +@return True if 6D detection interrupt 1 is enabled +@see LSM303DLHC_RA_INT2_CFG_A +@see LSM303DLHC_INT2_6D_BIT +*/ +bool LSM303DLHC::getAccelInterrupt26DEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_6D_BIT, buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for Z high is enabled + * @param enabled New enabled state for Z high interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_ZHIE_BIT + */ +void LSM303DLHC::setAccelZHighInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_ZHIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for Z high is enabled + * @return True if the interrupt 2 for Z high is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_ZHIE_BIT + */ +bool LSM303DLHC::getAccelZHighInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_ZHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for Z low is enabled + * @param enabled New enabled state for Z low interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_ZLIE_BIT + */ +void LSM303DLHC::setAccelZLowInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_ZLIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for Z low is enabled + * @return True if the interrupt 2 for Z low is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_ZLIE_BIT + */ +bool LSM303DLHC::getAccelZLowInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_ZLIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for Y high is enabled + * @param enabled New enabled state for Y high interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_YHIE_BIT + */ +void LSM303DLHC::setAccelYHighInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_YHIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for Y high is enabled + * @return True if the interrupt 2 for Y high is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_YHIE_BIT + */ +bool LSM303DLHC::getAccelYHighInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_YHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for Y low is enabled + * @param enabled New enabled state for Y low interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_YLIE_BIT + */ +void LSM303DLHC::setAccelYLowInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_YLIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for Y low is enabled + * @return True if the interrupt 2 for Y low is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_YLIE_BIT + */ +bool LSM303DLHC::getAccelYLowInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_YLIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for X high is enabled + * @param enabled New enabled state for X high interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_XHIE_BIT + */ +void LSM303DLHC::setAccelXHighInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_XHIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for X high is enabled + * @return True if the interrupt 2 for X high is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_XHIE_BIT + */ +bool LSM303DLHC::getAccelXHighInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_XHIE_BIT, + buffer); + return buffer[0]; +} + +/** Set whether the interrupt 2 for X low is enabled + * @param enabled New enabled state for X low interrupt 2. + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_XLIE_BIT + */ +void LSM303DLHC::setAccelXLowInterrupt2Enabled(bool enabled) { + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_XLIE_BIT, enabled); +} + +/** Get whether the interrupt 2 for X low is enabled + * @return True if the interrupt 2 for X low is enabled, false otherwise + * @see LSM303DLHC_RA_INT2_CFG_A + * @see LSM303DLHC_INT2_XLIE_BIT + */ +bool LSM303DLHC::getAccelXLowInterrupt2Enabled() { + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_INT2_CFG_A, LSM303DLHC_INT2_XLIE_BIT, + buffer); + return buffer[0]; +} + + +//INT2_SRC_A, r + +/*Get the contents of the INT2_SRC_A register to determine if any and which interrupts have occured. +Reading this register will clear any interrupts that were lateched. +@return the 8 bit contents of the INT2_SRC_a register +@see LSM303DLHC_RA_INT2_SRC_A +@see LSM303DLHC_INT2_IA_BIT +@see LSM303DLHC_INT2_ZH_BIT +@see LSM303DLHC_INT2_ZL_BIT +@see LSM303DLHC_INT2_YH_BIT +@see LSM303DLHC_INT2_YL_BIT +@see LSM303DLHC_INT2_XH_BIT +@see LSM303DLHC_INT2_XL_BIT +*/ +uint8_t LSM303DLHC::getAccelInterrupt2Source() { + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_INT2_SRC_A, buffer); + return buffer[0]; +} + +//INT2_THS_A, r/w + +/** Set the threshold for interrupt 2 + * @param threshold New threshold for interrupt 2 + * @see LSM303DLHC_RA_INT2_THS_A + */ +void LSM303DLHC::setAccelInterrupt2Threshold(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_INT2_THS_A, value); +} + +/** Retrieve the threshold interrupt 2 + * @return interrupt 2 threshold + * @see LSM303DLHC_RA_INT2_THS_A + */ +uint8_t LSM303DLHC::getAccelInterupt2Threshold(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_INT2_THS_A, buffer); + return buffer[0]; +} + +//INT2_DURATION_A, r/w + +/* Set the minimum event duration for interrupt 2 to be generated + * This depends on the chosen output data rate + * @param duration New duration necessary for interrupt 2 to be + * generated + * @see LSM303DLHC_RA_INT2_DURATION_A + * @see LSM303DLHC_INT2_DURATION_BIT + * @see LSM303DLHC_INT2_DURATION_LENGTH + */ +void LSM303DLHC::setAccelInterrupt2Duration(uint8_t duration) { + I2Cdev::writeBits(devAddrA, LSM303DLHC_RA_INT2_DURATION_A, LSM303DLHC_INT2_DURATION_BIT, + LSM303DLHC_INT2_DURATION_LENGTH, duration); +} + +/** Get the minimum event duration for interrupt 2 to be generated + * @return Duration necessary for interrupt 2 to be generated + * @see LSM303DLHC_RA_INT2_DURATION_A + * @see LSM303DLHC_INT2_DURATION_BIT + * @see LSM303DLHC_INT2_DURATION_LENGTH + */ +uint8_t LSM303DLHC::getAccelInterrupt2Duration() { + I2Cdev::readBits(devAddrA, LSM303DLHC_RA_INT2_DURATION_A, + LSM303DLHC_INT2_DURATION_BIT, LSM303DLHC_INT2_DURATION_LENGTH, buffer); + return buffer[0]; +} + +//CLICK_CFG_A, r/w + +/*Enables interrupt double click on the Z axis. +@param enable The new status of double click interrupt on the Z axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_ZD_BIT +*/ +void LSM303DLHC::setAccelZDoubleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_ZD_BIT, enabled); +} + +/*Get status of interrupt double click on the Z axis. +@return True if double click interrupt on the Z axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_ZD_BIT +*/ +bool LSM303DLHC::getAccelZDoubleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_ZD_BIT, buffer); + return buffer[0]; +} + +/*Enables interrupt single click on the Z axis. +@param enable The new status of single click interrupt on the Z axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_ZS_BIT +*/ +void LSM303DLHC::setAccelZSingleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_ZS_BIT, enabled); +} + +/*Get status of interrupt single click on the Z axis. +@return True if single click interrupt on the Z axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_ZS_BIT +*/ +bool LSM303DLHC::getAccelZSingleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_ZS_BIT, buffer); + return buffer[0]; +} + +/*Enables interrupt double click on the Y axis. +@param enable The new status of double click interrupt on the Y axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_YD_BIT +*/ +void LSM303DLHC::setAccelYDoubleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_YD_BIT, enabled); +} + +/*Get status of interrupt double click on the Y axis. +@return True if double click interrupt on the Y axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_YD_BIT +*/ +bool LSM303DLHC::getAccelYDoubleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_YD_BIT, buffer); + return buffer[0]; +} + +/*Enables interrupt single click on the Y axis. +@param enable The new status of single click interrupt on the Y axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_YS_BIT +*/ +void LSM303DLHC::setAccelYSingleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_YS_BIT, enabled); +} + +/*Get status of interrupt single click on the Y axis. +@return True if single click interrupt on the Y axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_YS_BIT +*/ +bool LSM303DLHC::getAccelYSingleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_YS_BIT, buffer); + return buffer[0]; +} + +/*Enables interrupt double click on the X axis. +@param enable The new status of double click interrupt on the X axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_XD_BIT +*/ +void LSM303DLHC::setAccelXDoubleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_XD_BIT, enabled); +} + +/*Get status of interrupt double click on the X axis. +@return True if double click interrupt on the X axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_XD_BIT +*/ +bool LSM303DLHC::getAccelXDoubleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_XD_BIT, buffer); + return buffer[0]; +} + +/*Enables interrupt single click on the X axis. +@param enable The new status of single click interrupt on the X axis +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_XS_BIT +*/ +void LSM303DLHC::setAccelXSingleClickEnabled(bool enabled){ + I2Cdev::writeBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_XS_BIT, enabled); +} + +/*Get status of interrupt single click on the X axis. +@return True if single click interrupt on the X axis is enabled +@see LSM303DLHC_RA_CLICK_CFG_A +@see LSM303DLHC_CLICK_XS_BIT +*/ + +bool LSM303DLHC::getAccelXSingleClickEnabled(){ + I2Cdev::readBit(devAddrA, LSM303DLHC_RA_CLICK_CFG_A, LSM303DLHC_CLICK_XS_BIT, buffer); + return buffer[0]; +} + + +//CLICK_SRC_A, r + +/*Get the contents of the CLICK_SRC_A register to determine if any and which interrupts have occured. +Reading this register may (not sure) clear any interrupts that were lateched. +@return the 7 bit contents of the CLICK_SRC_A register +@see LSM303DLHC_RA_CLICK_SRC_A +@see LSM303DLHC_CLICK_IA_BIT +@see LSM303DLHC_CLICK_DCLICK_BIT +@see LSM303DLHC_CLICK_SCLICK_BIT +@see LSM303DLHC_CLICK_SIGN_BIT +@see LSM303DLHC_CLICK_Z_BIT +@see LSM303DLHC_CLICK_Y_BIT +@see LSM303DLHC_CLICK_X_BIT +*/ +uint8_t LSM303DLHC::getAccelClickSource(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_CLICK_SRC_A, buffer); + return buffer[0]; +} + +//CLICK_THS_A, r/w + +/*Set the threshold which is used to start the click-detection procedure. The threshold +value is expressed over 7 bits as an unsigned number. +@param value The value of the click detection threshold +@see LSM303DLHC_RA_CLICK_THS_A +*/ +void LSM303DLHC::setAcceLClickThreshold(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_CLICK_THS_A, value); +} + +/*Get the threshold which is used to start the click-detection procedure. The threshold +value is a 7 bit unsigned number. +@return The value of the click detection threshold +@see LSM303DLHC_RA_CLICK_THS_A +*/ +uint8_t LSM303DLHC::getAccelClickThreshold(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_CLICK_THS_A, buffer); + return buffer[0]; +} + +//TIME_LIMIT_A, r/w + +/*Set the maximum time interval that can elapse between the start of the click-detection +procedure (the acceleration on the selected channel exceeds the programmed threshold) +and when the acceleration falls below the threshold. +@param value The value of the maximum time interval for click detection +@see LSM303DLHC_RA_TIME_LIMIT_A +*/ +void LSM303DLHC::setAcceLClickTimeLimit(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_TIME_LIMIT_A, value); +} + +/*Get the maximum time interval that can elapse between the start of the click-detection +procedure (the acceleration on the selected channel exceeds the programmed threshold) +and when the acceleration falls below the threshold. +@return The value of the maximum time interval for click detection +@see LSM303DLHC_RA_TIME_LIMIT_A +*/ +uint8_t LSM303DLHC::getAccelClickTimeLimit(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_TIME_LIMIT_A, buffer); + return buffer[0]; +} + +//TIME_LATENCY_A, r/w + +/*Set the time interval that starts after the first click detection where the +click-detection procedure is disabled, in cases where the device is configured +for double-click detection. +@param value The double click interval +@see LSM303DLHC_RA_TIME_LATENCY_A +*/ +void LSM303DLHC::setAcceLClickTimeLatency(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_TIME_LATENCY_A, value); +} + +/*Get the time interval that starts after the first click detection where the +click-detection procedure is disabled, in cases where the device is configured +for double-click detection. +@return The double click interval +@see LSM303DLHC_RA_TIME_LATENCY_A +*/ +uint8_t LSM303DLHC::getAccelClickTimeLatency(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_TIME_LATENCY_A, buffer); + return buffer[0]; +} + +//TIME_WINDOW_A, r/w + +/*Set the maximum interval of time that can elapse after the end of the latency +interval in which the click detection procedure can start, in cases where the device +is configured for double-click detection. +@param value The time window +@see LSM303DLHC_RA_TIME_WINDOW_A +*/ +void LSM303DLHC::setAcceLClickTimeWindow(uint8_t value){ + I2Cdev::writeByte(devAddrA, LSM303DLHC_RA_TIME_WINDOW_A, value); +} + +/*Get the maximum interval of time that can elapse after the end of the latency +interval in which the click detection procedure can start, in cases where the device +is configured for double-click detection. +@return The double click time window +@see LSM303DLHC_RA_TIME_WINDOW_A +*/ +uint8_t LSM303DLHC::getAccelClickTimeWindow(){ + I2Cdev::readByte(devAddrA, LSM303DLHC_RA_TIME_WINDOW_A, buffer); + return buffer[0]; +} + +//CRA_REG_M_A, rw + +/*Set the temperature sensor to be enabled. +@param enabled The new enabled state of the temperature sensor. +@see LSM303DLHC_RA_CRA_REG_M +@see LSM303DLHC_TEMP_EN_BIT +*/ +void LSM303DLHC::setMagTemperatureEnabled(bool enabled){ + I2Cdev::writeBit(devAddrM, LSM303DLHC_RA_CRA_REG_M, LSM303DLHC_TEMP_EN_BIT, enabled); +} + +/*Get whether the temperature sensor is enabled. +@return The state of the temperature sensor. +@see LSM303DLHC_RA_CRA_REG_M +@see LSM303DLHC_TEMP_EN_BIT +*/ +bool LSM303DLHC::getMagTemperatureEnabled(){ + I2Cdev::readBit(devAddrM, LSM303DLHC_RA_CRA_REG_M, LSM303DLHC_TEMP_EN_BIT, buffer); + return buffer[0]; +} + +/*Set the magetometer output data rate. Valid rates are 0, 1, 3, 7, 15, 30, 75, 220. +@param rate The new output data rate of the magnetometer. +@see LSM303DLHC_RA_CRA_REG_M +@see LSM303DLHC_DO_BIT +@see LSM303DLHC_DO_LENGTH +@see LSM303DLHC_DO_RATE_0 +@see LSM303DLHC_DO_RATE_1 +@see LSM303DLHC_DO_RATE_3 +@see LSM303DLHC_DO_RATE_7 +@see LSM303DLHC_DO_RATE_15 +@see LSM303DLHC_DO_RATE_30 +@see LSM303DLHC_DO_RATE_75 +@see LSM303DLHC_DO_RATE_220 +*/ +void LSM303DLHC::setMagOutputDataRate(uint8_t rate){ + uint8_t writeBit; + if (rate == 0){ + writeBit = LSM303DLHC_DO_RATE_0; + } else if (rate == 1){ + writeBit = LSM303DLHC_DO_RATE_1; + } else if (rate == 3){ + writeBit = LSM303DLHC_DO_RATE_3; + } else if (rate == 7){ + writeBit = LSM303DLHC_DO_RATE_7; + } else if (rate == 15){ + writeBit = LSM303DLHC_DO_RATE_15; + } else if (rate == 30){ + writeBit = LSM303DLHC_DO_RATE_30; + } else if (rate == 75){ + writeBit = LSM303DLHC_DO_RATE_75; + } else if (rate == 220){ + writeBit = LSM303DLHC_DO_RATE_220; + } + + I2Cdev::writeBits(devAddrM, LSM303DLHC_RA_CRA_REG_M, LSM303DLHC_DO_BIT, + LSM303DLHC_DO_LENGTH, writeBit); +} + +/*Get the magetometer output data rate. +@return The output data rate of the magnetometer. +@see LSM303DLHC_RA_CRA_REG_M +@see LSM303DLHC_DO_BIT +@see LSM303DLHC_DO_LENGTH +@see LSM303DLHC_DO_RATE_0 +@see LSM303DLHC_DO_RATE_1 +@see LSM303DLHC_DO_RATE_3 +@see LSM303DLHC_DO_RATE_7 +@see LSM303DLHC_DO_RATE_15 +@see LSM303DLHC_DO_RATE_30 +@see LSM303DLHC_DO_RATE_75 +@see LSM303DLHC_DO_RATE_220 +*/ +uint8_t LSM303DLHC::getMagOutputDataRate(){ + I2Cdev::readBits(devAddrM, LSM303DLHC_RA_CRA_REG_M, LSM303DLHC_DO_BIT, + LSM303DLHC_DO_LENGTH, buffer); + uint8_t rate = buffer[0]; + + if (rate == LSM303DLHC_DO_RATE_0){ + return 0; + } else if (rate == LSM303DLHC_DO_RATE_1){ + return 1; + } else if (rate == LSM303DLHC_DO_RATE_3){ + return 3; + } else if (rate == LSM303DLHC_DO_RATE_7){ + return 7; + } else if (rate == LSM303DLHC_DO_RATE_15){ + return 15; + } else if (rate == LSM303DLHC_DO_RATE_30){ + return 30; + } else if (rate == LSM303DLHC_DO_RATE_75){ + return 75; + } else if (rate == LSM303DLHC_DO_RATE_220){ + return 220; + } +} + +//CRB_REG_M, rws + +/*Set the magnetometer gain. +@param gain The new gain of the magnetometer +@see LSM303DLHC_RA_CRB_REG_M +@see LSM303DLHC_GN_BIT +@see LSM303DLHC_GN_LENGTH +@see LSM303DLHC_GN_230 +@see LSM303DLHC_GN_330 +@see LSM303DLHC_GN_400 +@see LSM303DLHC_GN_450 +@see LSM303DLHC_GN_670 +@see LSM303DLHC_GN_855 +@see LSM303DLHC_GN_1100 +*/ +void LSM303DLHC::setMagGain(uint16_t gain){ + uint8_t writeBit; + if (gain == 230){ + writeBit = LSM303DLHC_GN_230; + } else if (gain == 330){ + writeBit = LSM303DLHC_GN_330; + } else if (gain == 400){ + writeBit = LSM303DLHC_GN_400; + } else if (gain == 450){ + writeBit = LSM303DLHC_GN_450; + } else if (gain == 670){ + writeBit = LSM303DLHC_GN_670; + } else if (gain == 855){ + writeBit = LSM303DLHC_GN_855; + } else if (gain == 1100){ + writeBit = LSM303DLHC_GN_1100; + } + + I2Cdev::writeBits(devAddrM, LSM303DLHC_RA_CRB_REG_M, LSM303DLHC_GN_BIT, LSM303DLHC_GN_LENGTH, + writeBit); +} + +/*Get the magnetometer gain. +@return The gain of the magnetometer +@see LSM303DLHC_RA_CRB_REG_M +@see LSM303DLHC_GN_230 +@see LSM303DLHC_GN_330 +@see LSM303DLHC_GN_400 +@see LSM303DLHC_GN_450 +@see LSM303DLHC_GN_670 +@see LSM303DLHC_GN_855 +@see LSM303DLHC_GN_1100 +*/ +uint16_t LSM303DLHC::getMagGain(){ + I2Cdev::readBits(devAddrM, LSM303DLHC_RA_CRB_REG_M, LSM303DLHC_GN_BIT, LSM303DLHC_GN_LENGTH, + buffer); + uint8_t gain = buffer[0]; + if (gain == LSM303DLHC_GN_230){ + return 230; + } else if (gain == LSM303DLHC_GN_330){ + return 330; + } else if (gain == LSM303DLHC_GN_400){ + return 400; + } else if (gain == LSM303DLHC_GN_450){ + return 450; + } else if (gain == LSM303DLHC_GN_670){ + return 670; + } else if (gain == LSM303DLHC_GN_855){ + return 855; + } else if (gain == LSM303DLHC_GN_1100){ + return 1100; + } +} + +//MR_REG_M, rw + +/*Set the magnetometer mode. +@param mode The new mode for the magnetometer. +@see LSM303DLHC_RA_MR_REG_M +@see LSM303DLHC_MD_BIT +@see LSM303DLHC_MD_LENGTH +@see LSM303DLHC_MD_CONTINUOUS +@see LSM303DLHC_MD_SINGLE +@see LSM303DLHC_MD_SLEEP +*/ +void LSM303DLHC::setMagMode(uint8_t mode){ + I2Cdev::writeBits(devAddrM, LSM303DLHC_RA_MR_REG_M, LSM303DLHC_MD_BIT, LSM303DLHC_MD_LENGTH, + mode); +} + +/*Get the magnetometer mode. +@return The mode for the magnetometer. +@see LSM303DLHC_RA_MR_REG_M +@see LSM303DLHC_MD_BIT +@see LSM303DLHC_MD_LENGTH +@see LSM303DLHC_MD_CONTINUOUS +@see LSM303DLHC_MD_SINGLE +@see LSM303DLHC_MD_SLEEP +*/ +uint8_t LSM303DLHC::getMagMode(){ + I2Cdev::readBits(devAddrM, LSM303DLHC_RA_MR_REG_M, LSM303DLHC_MD_BIT, LSM303DLHC_MD_LENGTH, + buffer); + return buffer[0]; +} + +//OUT_____M, r + +//TODO +/** Get the magnetic field for all 3 axes + * @param x 16-bit integer container for the X-axis magnetic field + * @param y 16-bit integer container for the Y-axis magnetic field + * @param z 16-bit integer container for the Z-axis magnetic field + */ +void LSM303DLHC::getMag(int16_t* x, int16_t* y, int16_t* z){ + I2Cdev::readBytes(devAddrM, LSM303DLHC_RA_OUT_X_H_M, 6, buffer); + *x = ((((int16_t)buffer[0]) << 8) | buffer[1]); + *z = ((((int16_t)buffer[2]) << 8) | buffer[3]); + *y = ((((int16_t)buffer[4]) << 8) | buffer[5]); + +} + +/** Get the magnetic field along the X-axis + * @return magnetic field along the X-axis + * @see LSM303DLHC_RA_OUT_X_L_M + * @see LSM303DLHC_RA_OUT_X_H_M + */ +int16_t LSM303DLHC::getMagX(){ + I2Cdev::readBytes(devAddrM, LSM303DLHC_RA_OUT_X_H_M, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +/** Get the magnetic field along the X-axis + * @return magnetic field along the X-axis + * @see LSM303DLHC_RA_OUT_Y_L_M + * @see LSM303DLHC_RA_OUT_Y_H_M + */ +int16_t LSM303DLHC::getMagY(){ + I2Cdev::readBytes(devAddrM, LSM303DLHC_RA_OUT_Y_H_M, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +/** Get the magnetic field along the X-axis + * @return magnetic field along the X-axis + * @see LSM303DLHC_RA_OUT_Z_L_M + * @see LSM303DLHC_RA_OUT_Z_H_M + */ +int16_t LSM303DLHC::getMagZ(){ + I2Cdev::readBytes(devAddrM, LSM303DLHC_RA_OUT_Z_H_M, 2, buffer); + return (((int16_t)buffer[0]) << 8) | buffer[1]; +} + +//SR_REG_M, r + +/*Get data output register lock. This bit is set when the first magnetic file +data register has been read. +@return the output data register lock +@see LSM303DLHC_RA_SR_REG_M +@see LSM303DLHC_RA_M_LOCK_BIT +*/ +bool LSM303DLHC::getMagOutputDataRegisterLock(){ + I2Cdev::readBit(devAddrM, LSM303DLHC_RA_SR_REG_M, LSM303DLHC_M_LOCK_BIT, buffer); + return buffer[0]; +} + +/*Get data ready bit. This bit is when a new set of measurements is available. +@return the data ready bit status +@see LSM303DLHC_RA_SR_REG_M +@see LSM303DLHC_RA_M_DRDY_BIT +*/ +bool LSM303DLHC::getMagDataReady(){ + I2Cdev::readBit(devAddrM, LSM303DLHC_RA_SR_REG_M, LSM303DLHC_M_DRDY_BIT, buffer); + return buffer[0]; +} + +//TEMP_OUT_*_M + +/*Get the tempearture data. 12-bit resolution. 8LSB/deg +@return The temperature data. +@see LSM303DLHC_RA_TEMP_OUT_H_M +@see LSM303DLHC_RA_TEMP_OUT_L_M +*/ +int16_t LSM303DLHC::getTemperature(){ + I2Cdev::readBytes(devAddrM, LSM303DLHC_RA_TEMP_OUT_H_M | 0x80, 2, buffer); + return ((((int16_t)buffer[0]) << 8) | buffer[1]) >> 4; +} + +// WHO_AM_I register, read-only +// There is no ID register for this device. +uint8_t LSM303DLHC::getDeviceID() { + return 0; + // read a single byte and return it + // I2Cdev::readByte(devAddrA, LSM303DLHC_RA_WHO_AM_I, buffer); + // return buffer[0]; +} diff --git a/Arduino/LSM303DLHC/LSM303DLHC.h b/Arduino/LSM303DLHC/LSM303DLHC.h new file mode 100644 index 00000000..8c3469c4 --- /dev/null +++ b/Arduino/LSM303DLHC/LSM303DLHC.h @@ -0,0 +1,658 @@ +// I2Cdev library collection - LSM303DLHC I2C device class header file +// Based on ST LSM303DLHC datasheet REV 2, 11/2013 +// 3/10/2015 by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-15 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 [Author Name], 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 _LSM303DLHC_H_ +#define _LSM303DLHC_H_ + +#include "I2Cdev.h" + +// ---------------------------------------------------------------------------- +// STUB TODO: +// List all possible device I2C addresses here, if they are predefined. Some +// devices only have one possible address, in which case that one should be +// defined as both [LSM303DLHC]_ADDRESS and [LSM303DLHC]_DEFAULT_ADDRESS for +// consistency. The *_DEFAULT address will be used if a device object is +// created without an address provided in the constructor. Note that I2C +// devices conventionally use 7-bit addresses, so these will generally be +// between 0x00 and 0x7F. +// The LSM303DLHC uses two device addresses, one for the accerometer, and on +// for the megnetometer. +// ---------------------------------------------------------------------------- +#define LSM303DLHC_ADDRESS_A 0x19 +#define LSM303DLHC_ADDRESS_M 0x1E +#define LSM303DLHC_DEFAULT_ADDRESS_A 0x19 +#define LSM303DLHC_DEFAULT_ADDRESS_M 0x1E + +// ---------------------------------------------------------------------------- +// STUB TODO: +// List all registers that this device has. The goal for all device libraries +// is to have complete coverage of all possible functions, so be sure to add +// everything in the datasheet. Register address constants should use the extra +// prefix "RA_", followed by the datasheet's given name for each register. +// ---------------------------------------------------------------------------- +#define LSM303DLHC_RA_CTRL_REG1_A 0x20 // rw 0000 0111 +#define LSM303DLHC_RA_CTRL_REG2_A 0x21 // rw 0000 0000 +#define LSM303DLHC_RA_CTRL_REG3_A 0x22 // rw 0000 0000 +#define LSM303DLHC_RA_CTRL_REG4_A 0x23 // rw 0000 0000 +#define LSM303DLHC_RA_CTRL_REG5_A 0x24 // rw 0000 0000 +#define LSM303DLHC_RA_CTRL_REG6_A 0x25 // rw 0000 0000 +#define LSM303DLHC_RA_REFERENCE_A 0x06 // rw 0000 0000 +#define LSM303DLHC_RA_STATUS_REG_A 0x27 // r 0000 0000 +#define LSM303DLHC_RA_OUT_X_L_A 0x28 // r +#define LSM303DLHC_RA_OUT_X_H_A 0x29 // r +#define LSM303DLHC_RA_OUT_Y_L_A 0x2A // r +#define LSM303DLHC_RA_OUT_Y_H_A 0x2B // r +#define LSM303DLHC_RA_OUT_Z_L_A 0x2C // r +#define LSM303DLHC_RA_OUT_Z_H_A 0x2D // r +#define LSM303DLHC_RA_FIFO_CTRL_REG_A 0x2E // rw 0000 0000 +#define LSM303DLHC_RA_FIFO_SRC_REG_A 0x2F // r +#define LSM303DLHC_RA_INT1_CFG_A 0x30 // rw 0000 0000 +#define LSM303DLHC_RA_INT1_SRC_A 0x31 // r 0000 0000 +#define LSM303DLHC_RA_INT1_THS_A 0x32 // rw 0000 0000 +#define LSM303DLHC_RA_INT1_DURATION_A 0x33 // rw 0000 0000 +#define LSM303DLHC_RA_INT2_CFG_A 0x34 // rw 0000 0000 +#define LSM303DLHC_RA_INT2_SRC_A 0x35 // r 0000 0000 +#define LSM303DLHC_RA_INT2_THS_A 0x36 // rw 0000 0000 +#define LSM303DLHC_RA_INT2_DURATION_A 0x37 // rw 0000 0000 +#define LSM303DLHC_RA_CLICK_CFG_A 0x38 // rw 0000 0000 +#define LSM303DLHC_RA_CLICK_SRC_A 0x39 // rw 0000 0000 +#define LSM303DLHC_RA_CLICK_THS_A 0x3A // rw 0000 0000 +#define LSM303DLHC_RA_TIME_LIMIT_A 0x3B // rw 0000 0000 +#define LSM303DLHC_RA_TIME_LATENCY_A 0x3C // rw 0000 0000 +#define LSM303DLHC_RA_TIME_WINDOW_A 0x3D // rw 0000 0000 + +#define LSM303DLHC_RA_CRA_REG_M 0x00 // rw 0001 0000 +#define LSM303DLHC_RA_CRB_REG_M 0x01 // rw 0010 0000 +#define LSM303DLHC_RA_MR_REG_M 0x02 // rw 0000 0011 +#define LSM303DLHC_RA_OUT_X_H_M 0x03 // r +#define LSM303DLHC_RA_OUT_X_L_M 0x04 // r +#define LSM303DLHC_RA_OUT_Z_H_M 0x05 // r +#define LSM303DLHC_RA_OUT_Z_L_M 0x06 // r +#define LSM303DLHC_RA_OUT_Y_H_M 0x07 // r +#define LSM303DLHC_RA_OUT_Y_L_M 0x08 // r +#define LSM303DLHC_RA_SR_REG_M 0x09 // r 0000 0000 +#define LSM303DLHC_RA_IRA_REG_M 0x0A // r 0100 1000 +#define LSM303DLHC_RA_IRB_REG_M 0x0B // r 0011 0100 +#define LSM303DLHC_RA_IRC_REG_M 0x0C // r 0011 0011 +#define LSM303DLHC_RA_TEMP_OUT_H_M 0x31 // r +#define LSM303DLHC_RA_TEMP_OUT_L_M 0x32 // r + +// ---------------------------------------------------------------------------- +// STUB TODO: +// List register structures wherever necessary. Bit position constants should +// end in "_BIT", and are defined from 7 to 0, with 7 being the left/MSB and +// 0 being the right/LSB. If the device uses 16-bit registers instead of the +// more common 8-bit registers, the range is 15 to 0. Field length constants +// should end in "_LENGTH", but otherwise match the name of bit position +// constant that it complements. Fields that are a single bit in length don't +// need a separate field length constant. +// ---------------------------------------------------------------------------- +// ---------------------------------------------------------------------------- +// STUB TODO: +// List any special predefined values for each register according to the +// datasheet. For example, MEMS devices often provide different options for +// measurement rates, say 25Hz, 50Hz, 100Hz, and 200Hz. These are typically +// represented by arbitrary bit values, say 0b00, 0b01, 0b10, and 0b11 (or 0x0, +// 0x1, 0x2, and 0x3). Defining them here makes it easy to know which options +// are available. +// ---------------------------------------------------------------------------- + +//CTRL_REG1_A +#define LSM303DLHC_ODR_BIT 7 +#define LSM303DLHC_ODR_LENGTH 4 +#define LSM303DLHC_LPEN_BIT 3 +#define LSM303DLHC_ZEN_BIT 2 +#define LSM303DLHC_YEN_BIT 1 +#define LSM303DLHC_XEN_BIT 0 + +#define LSM303DLHC_ODR_RATE_POWERDOWN 0b0000 +#define LSM303DLHC_ODR_RATE_1 0b0001 +#define LSM303DLHC_ODR_RATE_10 0b0010 +#define LSM303DLHC_ODR_RATE_25 0b0011 +#define LSM303DLHC_ODR_RATE_50 0b0100 +#define LSM303DLHC_ODR_RATE_100 0b0101 +#define LSM303DLHC_ODR_RATE_200 0b0110 +#define LSM303DLHC_ODR_RATE_400 0b0111 +#define LSM303DLHC_ODR_RATE_1620_LP 0b1000 +#define LSM303DLHC_ODR_RATE_1344_N_5376_LP 0b1001 + +//CTRL_REG2_A +#define LSM303DLHC_HPM_BIT 7 +#define LSM303DLHC_HPM_LENGTH 2 +#define LSM303DLHC_HPCF_BIT 5 +#define LSM303DLHC_HPCF_LENGTH 2 +#define LSM303DLHC_FDS_BIT 3 +#define LSM303DLHC_HPCLICK_BIT 2 +#define LSM303DLHC_HPIS2_BIT 1 +#define LSM303DLHC_HPIS1_BIT 0 + +#define LSM303DLHC_HPM_HRF 0b00 +#define LSM303DLHC_HPM_REFERANCE 0b01 +#define LSM303DLHC_HPM_NORMAL 0b10 +#define LSM303DLHC_HPM_AUTORESET 0b11 + +#define LSM303DLHC_HPCF_1 0b00 +#define LSM303DLHC_HPCF_2 0b01 +#define LSM303DLHC_HPCF_3 0b10 +#define LSM303DLHC_HPCF_4 0b11 + +//CTRL_REG3_A +#define LSM303DLHC_I1_CLICK_BIT 7 +#define LSM303DLHC_I1_AOI1_BIT 6 +#define LSM303DLHC_I1_AOI2_BIT 5 +#define LSM303DLHC_I1_DRDY1_BIT 4 +#define LSM303DLHC_I1_DRDY2_BIT 3 +#define LSM303DLHC_I1_WTM_BIT 2 +#define LSM303DLHC_I1_OVERRUN_BIT 1 + +//CTRL_REG4_A +#define LSM303DLHC_BDU_BIT 7 +#define LSM303DLHC_BLE_BIT 6 +#define LSM303DLHC_FS_BIT 5 +#define LSM303DLHC_FS_LENGTH 2 +#define LSM303DLHC_HR_BIT 3 +#define LSM303DLHC_SIM_BIT 0 + +#define LSM303DLHC_LITTLE_ENDIAN 0 +#define LSM303DLHC_BIG_ENDIAN 1 +#define LSM303DLHC_FS_2 0b00 +#define LSM303DLHC_FS_4 0b01 +#define LSM303DLHC_FS_8 0b10 +#define LSM303DLHC_FS_16 0b11 +#define LSM303DLHC_SIM_3W 1 +#define LSM303DLHC_SIM_4W 0 + +//CTRL_REG5_A +#define LSM303DLHC_BOOT_BIT 7 +#define LSM303DLHC_FIFO_EN_BIT 6 +#define LSM303DLHC_LIR_INT1_BIT 3 +#define LSM303DLHC_D4D_INT1_BIT 2 +#define LSM303DLHC_LIR_INT2_BIT 1 +#define LSM303DLHC_D4D_INT2_BIT 0 + +//CTRL_REG6_A +#define LSM303DLHC_I2_CLICK_BIT 7 +#define LSM303DLHC_I2_INT1_BIT 6 +#define LSM303DLHC_I2_INT2_BIT 5 +#define LSM303DLHC_BOOT_I1_BIT 4 +#define LSM303DLHC_P2_ACT_BIT 3 +#define LSM303DLHC_H_ACTIVE_BIT 1 + +//REFERENCE_A + +//STATUS_REG_A +#define LSM303DLHC_ZYXOR_BIT 7 +#define LSM303DLHC_ZOR_BIT 6 +#define LSM303DLHC_YOR_BIT 5 +#define LSM303DLHC_XOR_BIT 4 +#define LSM303DLHC_ZYXDA_BIT 3 +#define LSM303DLHC_ZDA_BIT 2 +#define LSM303DLHC_YDA_BIT 1 +#define LSM303DLHC_XDA_BIT 0 + +//FIFO_CTRL_REG_A +#define LSM303DLHC_FM_BIT 7 +#define LSM303DLHC_FM_LENGTH 2 +#define LSM303DLHC_TR_BIT 5 +#define LSM303DLHC_FTH_BIT 4 +#define LSM303DLHC_FTH_LENGTH 5 + +#define LSM303DLHC_FM_BYBASS 0b00 +#define LSM303DLHC_FM_FIFO 0b01 +#define LSM303DLHC_FM_STREAM 0b10 +#define LSM303DLHC_FM_TRIGGER 0b11 +#define LSM303DLHC_TR_INT1 0 +#define LSM303DLHC_TR_INT2 1 + +//FIFO_SRC_REG_A +#define LSM303DLHC_WTM_BIT 7 +#define LSM303DLHC_OVRN_FIFO_BIT 6 +#define LSM303DLHC_EMPTY_BIT 5 +#define LSM303DLHC_FSS_BIT 4 +#define LSM303DLHC_FSS_LENGTH 5 + +//INT1_CFG_A +#define LSM303DLHC_INT1_AOI_BIT 7 +#define LSM303DLHC_INT1_6D_BIT 6 +#define LSM303DLHC_INT1_ZHIE_ZUPE_BIT 5 +#define LSM303DLHC_INT1_ZLIE_ZDOWNE_BIT 4 +#define LSM303DLHC_INT1_YHIE_YUPE_BIT 3 +#define LSM303DLHC_INT1_YLIE_YDOWNE_BIT 2 +#define LSM303DLHC_INT1_XHIE_XUPE_BIT 1 +#define LSM303DLHC_INT1_XLIE_XDOWNE_BIT 0 + +//INT1_SRC_A +#define LSM303DLHC_INT1_IA_BIT 6 +#define LSM303DLHC_INT1_ZH_BIT 5 +#define LSM303DLHC_INT1_ZL_BIT 4 +#define LSM303DLHC_INT1_YH_BIT 3 +#define LSM303DLHC_INT1_YL_BIT 2 +#define LSM303DLHC_INT1_XH_BIT 1 +#define LSM303DLHC_INT1_XL_BIT 0 + +//INT1_THS_A +#define LSM303DLHC_INT1_THS_BIT 6 +#define LSM303DLHC_INT1_THS_LENGTH 7 + +//INT1_DURATION_A +#define LSM303DLHC_INT1_DURATION_BIT 6 +#define LSM303DLHC_INT1_DURATION_LENGTH 7 + +//INT2_CFG_A +#define LSM303DLHC_INT2_AOI_BIT 7 +#define LSM303DLHC_INT2_6D_BIT 6 +#define LSM303DLHC_INT2_ZHIE_BIT 5 +#define LSM303DLHC_INT2_ZLIE_BIT 4 +#define LSM303DLHC_INT2_YHIE_BIT 3 +#define LSM303DLHC_INT2_YLIE_BIT 2 +#define LSM303DLHC_INT2_XHIE_BIT 1 +#define LSM303DLHC_INT2_XLIE_BIT 0 + +//INT2_SRC_A +#define LSM303DLHC_INT2_IA_BIT 6 +#define LSM303DLHC_INT2_ZH_BIT 5 +#define LSM303DLHC_INT2_ZL_BIT 4 +#define LSM303DLHC_INT2_YH_BIT 3 +#define LSM303DLHC_INT2_YL_BIT 2 +#define LSM303DLHC_INT2_XH_BIT 1 +#define LSM303DLHC_INT2_XL_BIT 0 + +//INT2_THS_A +#define LSM303DLHC_INT2_THS_BIT 6 +#define LSM303DLHC_INT2_THS_LENGTH 7 + +//INT2_DURATION_A +#define LSM303DLHC_INT2_DURATION_BIT 6 +#define LSM303DLHC_INT2_DURATION_LENGTH 7 + +//CLICK_CFG_A +#define LSM303DLHC_CLICK_ZD_BIT 5 +#define LSM303DLHC_CLICK_ZS_BIT 4 +#define LSM303DLHC_CLICK_YD_BIT 3 +#define LSM303DLHC_CLICK_YS_BIT 2 +#define LSM303DLHC_CLICK_XD_BIT 1 +#define LSM303DLHC_CLICK_XS_BIT 0 + +//CLICK_SRC_A +#define LSM303DLHC_CLICK_IA_BIT 6 +#define LSM303DLHC_CLICK_DCLICK_BIT 5 +#define LSM303DLHC_CLICK_SCLICK_BIT 4 +#define LSM303DLHC_CLICK_SIGN_BIT 3 +#define LSM303DLHC_CLICK_Z_BIT 2 +#define LSM303DLHC_CLICK_Y_BIT 1 +#define LSM303DLHC_CLICK_X_BIT 0 + +//CLICK_THS_A +#define LSM303DLHC_CLICK_THS_BIT 6 +#define LSM303DLHC_CLICK_THS_LENGTH 7 + +//TIME_LIMIT_A +#define LSM303DLHC_TLI_BIT 6 +#define LSM303DLHC_TLI_LENGTH 7 + +//TIME_LATENCY_A + +//TIME_WINDOW_A + +//CRA_REG_M +#define LSM303DLHC_TEMP_EN_BIT 7 +#define LSM303DLHC_DO_BIT 4 +#define LSM303DLHC_DO_LENGTH 3 + +#define LSM303DLHC_DO_RATE_0 0b000 +#define LSM303DLHC_DO_RATE_1 0b001 +#define LSM303DLHC_DO_RATE_3 0b010 +#define LSM303DLHC_DO_RATE_7 0b011 +#define LSM303DLHC_DO_RATE_15 0b100 +#define LSM303DLHC_DO_RATE_30 0b101 +#define LSM303DLHC_DO_RATE_75 0b110 +#define LSM303DLHC_DO_RATE_220 0b111 + +//CRB_REG_M +#define LSM303DLHC_GN_BIT 7 +#define LSM303DLHC_GN_LENGTH 3 + +#define LSM303DLHC_GN_1100 0b001 +#define LSM303DLHC_GN_855 0b010 +#define LSM303DLHC_GN_670 0b011 +#define LSM303DLHC_GN_450 0b100 +#define LSM303DLHC_GN_400 0b101 +#define LSM303DLHC_GN_330 0b110 +#define LSM303DLHC_GN_230 0b111 + +//MR_REG_M +#define LSM303DLHC_MD_BIT 1 +#define LSM303DLHC_MD_LENGTH 2 + +#define LSM303DLHC_MD_CONTINUOUS 0b00 +#define LSM303DLHC_MD_SINGLE 0b01 +#define LSM303DLHC_MD_SLEEP 0b10 + +//OUT_X_H_M +//OUT_X_L_M +//OUT_Y_H_M +//OUT_Y_L_M +//OUT_Z_H_M +//OUT_Z_L_M + +//SR_REG_M +#define LSM303DLHC_M_DRDY_BIT 0 +#define LSM303DLHC_M_LOCK_BIT 1 + +//IRA_REG_M +//IRB_REG_M +//IRC_REG_M + +//TEMP_OUT_H_M +//TEMP_OUT_L_M + + + +class LSM303DLHC { + public: + LSM303DLHC(); + LSM303DLHC(uint8_t addressA, uint8_t addressM); + + void initialize(); + bool testConnection(); + +// ---------------------------------------------------------------------------- +// STUB TODO: +// Declare methods to fully cover all available functionality provided by the +// device, according to the datasheet and/or register map. Unless there is very +// clear reason not to, try to follow the get/set naming convention for all +// values, for instance: +// - uint8_t getThreshold() +// - void setThreshold(uint8_t threshold) +// - uint8_t getRate() +// - void setRate(uint8_t rate) +// +// Some methods may be named differently if it makes sense. As long as all +// functionality is covered, that's the important part. The methods here are +// only examples and should not be kept for your real device. +// ---------------------------------------------------------------------------- + //CTRL_REG1_A, r/w + void setAccelOutputDataRate(uint16_t rate); + uint16_t getAccelOutputDataRate(); + void setAccelLowPowerEnabled(bool enabled); + bool getAccelLowPowerEnabled(); + void setAccelZEnabled(bool enabled); + bool getAccelZEnabled(); + void setAccelYEnabled(bool enabled); + bool getAccelYEnabled(); + void setAccelXEnabled(bool enabled); + bool getAccelXEnabled(); + + //CTRL_REG2_A r/w + void setAccelHighPassMode(uint8_t mode); + uint8_t getAccelHighPassMode(); + void setAccelHighPassFilterCutOffFrequencyLevel(uint8_t level); + uint8_t getAccelHighPassFilterCutOffFrequencyLevel(); + + + //CTRL_REG3_A r/w + void setAccelINT1ClickEnabled(bool enabled); //routes Click interrupt to INT1 pin + bool getAccelINT1ClickEnabled(); + void setAccelINT1AOI1Enabled(bool enabled); //routes AOI1 interrupt to INT1 pin + bool getAccelINT1AOI1Enabled(); + void setAccelINT1AOI2Enabled(bool enabled); //routes AOI2 interrupt to INT1 pin + bool getAccelINT1AOI2Enabled(); + void setAccelINT1DataReady1Enabled(bool enabled); //routes DataReady1 int to INT1.. + bool getAccelINT1DataReady1Enabled(); + void setAccelINT1DataReady2Enabled(bool enabled); //routes DataReady2 int to INT1.. + bool getAccelINT1DataReady2Enabled(); + void setAccelINT1FIFOWatermarkEnabled(bool enabled);//routes FIFO WTM int to INT1 pin + bool getAccelINT1FIFOWatermarkEnabled(); + void setAccelINT1FIFOOverunEnabled(bool enabled);//routes FIFO Overrun int to INT1.. + bool getAccelINT1FIFOOverunEnabled(); + + //CTRL_REG4_A r/w + void setAccelBlockDataUpdateEnabled(bool enabled); + bool getAccelBlockDataUpdateEnabled(); + void setAccelEndianMode(bool endianness); + bool getAccelEndianMode(); + void setAccelFullScale(uint8_t scale); + uint8_t getAccelFullScale(); + void setAccelHighResOutputEnabled(bool enabled); + bool getAccelHighResOutputEnabled(); + void setAccelSPIMode(bool mode); + bool getAccelSPIMode(); + + //CTRL_REG5_A r/w + void rebootAccelMemoryContent(); + void setAccelFIFOEnabled(bool enabled); + bool getAccelFIFOEnabled(); + void setAccelInterrupt1RequestLatched(bool latched); + bool getAccelInterrupt1RequestLatched(); + void setAccelInterrupt2RequestLatched(bool latched); + bool getAccelInterrupt2RequestLatched(); + void setAccelDetect4DInterrupt1Enabled(bool enabled); + bool getAccelDetect4DInterrupt1Enabled(); + void setAccelDetect4DInterrupt2Enabled(bool enabled); + bool getAccelDetect4DInterrupt2Enabled(); + + //CTRL_REG6_A r/w + void setAccelINT2ClickEnabled(bool enabled); + bool getAccelINT2ClickEnabled(); + void setAccelINT2Interrupt1Enabled(bool enabled); + bool getAccelINT2Interrupt1Enabled(); + void setAccelINT2Interrupt2Enabled(bool enabled); + bool getAccelINT2Interrupt2Enabled(); + + void setAccelRebootMemoryContentINT2Enabled(bool enabled); + bool getAccelRebootMemoryContentINT2Enabled(); + // void setAccelActiveFunctionStatusINT2Enabled(bool enabled); + // bool getAccelActiveFunctionStatusINT2Enabled(); + void setAccelInterruptActiveLowEnabled(bool enabled); + bool getAccelInterruptActiveLowEnabled(); + + //REFERENCE_A r/w + void setAccelInterruptReference(uint8_t reference); + uint8_t getAccelInterruptReference(); + + //STATUS_REG_A r + bool getAccelXYZOverrun(); + bool getAccelZOverrun(); + bool getAccelYOverrun(); + bool getAccelXOverrun(); + bool getAccelXYZDataAvailable(); + bool getAccelZDataAvailable(); + bool getAccelYDataAvailable(); + bool getAccelXDataAvailable(); + + //OUT_*_A, r + // OUT_* registers, read-only + void getAcceleration(int16_t* x, int16_t* y, int16_t* z); + int16_t getAccelerationX(); + int16_t getAccelerationY(); + int16_t getAccelerationZ(); + + //FIFO_CTRL_REG_A, rw + void setAccelFIFOMode(uint8_t mode); + uint8_t getAccelFIFOMode(); + void setAccelFIFOTriggerINT(bool tirgger); + bool getAccelFIFOTriggerINT(); + void setAccelFIFOThreshold(uint8_t fth); + uint8_t getAccelFIFOThreshold(); + + //FIFO_SRC_REG_A, r + bool getAccelFIFOAtWatermark(); + bool getAccelFIFOOverrun(); + bool getAccelFIFOEmpty(); + uint8_t getAccelFIFOStoredSamples(); + + //Int1_CFG_A, wr + void setAccelInterrupt1Combination(bool combination); + bool getAccelInterrupt1Combination(); + void setAccelInterrupt16DEnabled(bool enabled); + bool getAccelInterrupt16DEnabled(); + + void setAccelZHighUpInterrupt1Enabled(bool enabled); + bool getAccelZHighUpInterrupt1Enabled(); + void setAccelYHighUpInterrupt1Enabled(bool enabled); + bool getAccelYHighUpInterrupt1Enabled(); + void setAccelXHighUpInterrupt1Enabled(bool enabled); + bool getAccelXHighUpInterrupt1Enabled(); + void setAccelZLowDownInterrupt1Enabled(bool enabled); + bool getAccelZLowDownInterrupt1Enabled(); + void setAccelYLowDownInterrupt1Enabled(bool enabled); + bool getAccelYLowDownInterrupt1Enabled(); + void setAccelXLowDownInterrupt1Enabled(bool enabled); + bool getAccelXLowDownInterrupt1Enabled(); + + //INT1_SRC_A, r + uint8_t getAccelInterrupt1Source(); //this should be read entirely since it clears + + //INT1_THS_A, rw + void setAccelInterrupt1Threshold(uint8_t value); + uint8_t getAccelInterupt1Threshold(); + + //INT1_DURATION_A, rw + void setAccelInterrupt1Duration(uint8_t value); + uint8_t getAccelInterrupt1Duration(); + + //INT2_CFG_A, rw + void setAccelInterrupt2Combination(bool combination); + bool getAccelInterrupt2Combination(); + void setAccelInterrupt26DEnabled(bool enabled); + bool getAccelInterrupt26DEnabled(); + + void setAccelZHighInterrupt2Enabled(bool enabled); + bool getAccelZHighInterrupt2Enabled(); + void setAccelYHighInterrupt2Enabled(bool enabled); + bool getAccelYHighInterrupt2Enabled(); + void setAccelXHighInterrupt2Enabled(bool enabled); + bool getAccelXHighInterrupt2Enabled(); + void setAccelZLowInterrupt2Enabled(bool enabled); + bool getAccelZLowInterrupt2Enabled(); + void setAccelYLowInterrupt2Enabled(bool enabled); + bool getAccelYLowInterrupt2Enabled(); + void setAccelXLowInterrupt2Enabled(bool enabled); + bool getAccelXLowInterrupt2Enabled(); + + //INT2_SRC_A, r + uint8_t getAccelInterrupt2Source(); //this should be read entirely since it clears + + //INT2_THS_A, rw + void setAccelInterrupt2Threshold(uint8_t value); + uint8_t getAccelInterupt2Threshold(); + + //INT2_DURATION_A, rw + void setAccelInterrupt2Duration(uint8_t value); + uint8_t getAccelInterrupt2Duration(); + + //CLICK_CFG_A, rw + void setAccelZDoubleClickEnabled(bool enabled); + bool getAccelZDoubleClickEnabled(); + void setAccelZSingleClickEnabled(bool enabled); + bool getAccelZSingleClickEnabled(); + void setAccelYDoubleClickEnabled(bool enabled); + bool getAccelYDoubleClickEnabled(); + void setAccelYSingleClickEnabled(bool enabled); + bool getAccelYSingleClickEnabled(); + void setAccelXDoubleClickEnabled(bool enabled); + bool getAccelXDoubleClickEnabled(); + void setAccelXSingleClickEnabled(bool enabled); + bool getAccelXSingleClickEnabled(); + + //CLICK_SRC_A, rw? + //I think the documentation is incorrect for DCLICK + //and SCLICK. I think these are flags, not enables. + //For now we will assume this is readonly and simply + //grab the whole byte consistant with the interrupt + //source registers. + uint8_t getAccelClickSource(); + + //CLICK_THS_A, rw + void setAcceLClickThreshold(uint8_t value); + uint8_t getAccelClickThreshold(); + + //TIME_LIMIT_A, rw + void setAcceLClickTimeLimit(uint8_t value); + uint8_t getAccelClickTimeLimit(); + + //TIME_LATENCY_A, rw + void setAcceLClickTimeLatency(uint8_t value); + uint8_t getAccelClickTimeLatency(); + + //TIME_WINDOW_A, rw + void setAcceLClickTimeWindow(uint8_t value); + uint8_t getAccelClickTimeWindow(); + + //CRA_REG_M_A, rw + void setMagTemperatureEnabled(bool enabled); + bool getMagTemperatureEnabled(); + void setMagOutputDataRate(uint8_t rate); + uint8_t getMagOutputDataRate(); + + //CRB_REG_M, rw + void setMagGain(uint16_t gain); + uint16_t getMagGain(); + + //MR_REG_M, rw + void setMagMode(uint8_t); + uint8_t getMagMode(); + + //OUT_____M, r + void getMag(int16_t* x, int16_t* y, int16_t* z); + int16_t getMagX(); + int16_t getMagY(); + int16_t getMagZ(); + + //SR_REG_M, r + bool getMagOutputDataRegisterLock(); + bool getMagDataReady(); + + //TEMP_OUT_*_M + int16_t getTemperature(); + + // WHO_AM_I register, read-only + uint8_t getDeviceID(); + +// ---------------------------------------------------------------------------- +// STUB TODO: +// Declare private object helper variables or local storage for particular +// device settings, if required. All devices need a "devAddr" variable to store +// the I2C slave address for easy access. Most devices also need a buffer for +// reads (the I2Cdev class' read methods use a pointer for the storage location +// of any read data). The buffer should be of type "uint8_t" if the device uses +// 8-bit registers, or type "uint16_t" if the device uses 16-bit registers. +// Many devices will not require more member variables than this. +// ---------------------------------------------------------------------------- + private: + uint8_t devAddrA; + uint8_t devAddrM; + uint8_t buffer[6]; + bool endianMode; +}; + +#endif /* _LSM303DLHC_H_ */ diff --git a/Arduino/LSM303DLHC/examples/LSM303DLHC_test/LSM303DLHC_test.ino b/Arduino/LSM303DLHC/examples/LSM303DLHC_test/LSM303DLHC_test.ino new file mode 100644 index 00000000..463c25d7 --- /dev/null +++ b/Arduino/LSM303DLHC/examples/LSM303DLHC_test/LSM303DLHC_test.ino @@ -0,0 +1,148 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for LSM303DLHC class +// 3/10/2015 by Nate Costello +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-03-10 - initial release + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 Jonathan Arnett, 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. +=============================================== +*/ + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#include + +// I2Cdev and LSM303DLHC must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include +#include + +// default address is 105 +// specific I2C address may be passed here +LSM303DLHC accelMag; + +int16_t ax, ay, az; +int16_t mx, my, mz; + +#define LED_PIN 13 // (Arduino is 13, Teensy is 6) +bool blinkState = false; + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) + Wire.begin(); + + // initialize serial communication + // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but + // it's really up to you depending on your project) + Serial.begin(9600); + + // initialize device + Serial.println("Initializing I2C devices..."); + accelMag.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelMag.testConnection() ? "LSM303DLHC connection successful" : "LSM303DLHC connection failed"); + + // configure LED for output + pinMode(LED_PIN, OUTPUT); + + // set scale to 2Gs + accelMag.setAccelFullScale(2); + + // set accel data rate to 200Hz + accelMag.setAccelOutputDataRate(200); + + // test scale + Serial.print("Accel Scale: "); + Serial.println(accelMag.getAccelFullScale()); + + // test data rate + Serial.print("Accel Output Data Rate: "); + Serial.println(accelMag.getAccelOutputDataRate()); + + // set mag data rate to 220Hz + accelMag.setMagOutputDataRate(220); + + // test mag data rate + Serial.print("Mag Output Data Rate: "); + Serial.println(accelMag.getMagOutputDataRate()); + + //set mag gain + accelMag.setMagGain(1100); + + // test mag gain + Serial.print("Mag Gain: "); + Serial.println(accelMag.getMagGain()); + + //enable mag + accelMag.setMagMode(LSM303DLHC_MD_CONTINUOUS); + Serial.println(accelMag.getMagMode()); + + +} + +void loop() { + // read raw angular velocity measurements from device + accelMag.getAcceleration(&ax, &ay, &az); + accelMag.getMag(&mx, &my, &mz); + + // Serial.print("Acceleration:\t"); + // Serial.print(ax,HEX); Serial.print("\t"); + // Serial.print(ay,HEX); Serial.print("\t"); + // Serial.print(az,HEX); + + // Serial.print(" Magnetic Field:\t"); + // Serial.print(mx,HEX); Serial.print("\t"); + // Serial.print(my,HEX); Serial.print("\t"); + // Serial.println(mz,HEX); + + // Serial.print("Acceleration:\t"); + // Serial.print(ax); Serial.print("\t"); + // Serial.print(ay); Serial.print("\t"); + // Serial.print(az); + + // Serial.print(" Magnetic Field:\t"); + // Serial.print(mx); Serial.print("\t"); + // Serial.print(my); Serial.print("\t"); + // Serial.println(mz); + + Serial.print("Acceleration:\t"); + Serial.print(ax*0.0000625F,4); Serial.print("\t"); + Serial.print(ay*0.0000625F,4); Serial.print("\t"); + Serial.print(az*0.0000625F,4); + + Serial.print(" Magnetic Field:\t"); + Serial.print(mx); Serial.print("\t"); + Serial.print(my); Serial.print("\t"); + Serial.println(mz); + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + + delay(1000); +} + + diff --git a/Arduino/MPR121/library.json b/Arduino/MPR121/library.json new file mode 100644 index 00000000..877ebe3c --- /dev/null +++ b/Arduino/MPR121/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-MPR121", + "version": "1.0.0", + "keywords": "touch, capacitance, proximity, sensor, i2cdevlib, i2c", + "description": "The MPR121 is a 12-bit proximity capacitive touch sensor", + "include": "Arduino/MPR121", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/MPU6050/MPU6050.cpp b/Arduino/MPU6050/MPU6050.cpp index 3f531efc..ae145004 100644 --- a/Arduino/MPU6050/MPU6050.cpp +++ b/Arduino/MPU6050/MPU6050.cpp @@ -4,6 +4,8 @@ // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2021-09-27 - split implementations out of header files, finally +// 2019-07-08 - Added Auto Calibration routine // ... - ongoing debug release // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE @@ -36,21 +38,13 @@ THE SOFTWARE. #include "MPU6050.h" -/** Default constructor, uses default I2C address. - * @see MPU6050_DEFAULT_ADDRESS - */ -MPU6050::MPU6050() { - devAddr = MPU6050_DEFAULT_ADDRESS; -} - /** Specific address constructor. - * @param address I2C address + * @param address I2C address, uses default I2C address if none is specified * @see MPU6050_DEFAULT_ADDRESS * @see MPU6050_ADDRESS_AD0_LOW * @see MPU6050_ADDRESS_AD0_HIGH */ -MPU6050::MPU6050(uint8_t address) { - devAddr = address; +MPU6050_Base::MPU6050_Base(uint8_t address, void *wireObj):devAddr(address), wireObj(wireObj) { } /** Power on and prepare for general usage. @@ -60,7 +54,7 @@ MPU6050::MPU6050(uint8_t address) { * the clock source to use the X Gyro for reference, which is slightly better than * the default internal clock source. */ -void MPU6050::initialize() { +void MPU6050_Base::initialize() { setClockSource(MPU6050_CLOCK_PLL_XGYRO); setFullScaleGyroRange(MPU6050_GYRO_FS_250); setFullScaleAccelRange(MPU6050_ACCEL_FS_2); @@ -71,7 +65,7 @@ void MPU6050::initialize() { * Make sure the device is connected and responds as expected. * @return True if connection is valid, false otherwise */ -bool MPU6050::testConnection() { +bool MPU6050_Base::testConnection() { return getDeviceID() == 0x34; } @@ -83,8 +77,8 @@ bool MPU6050::testConnection() { * the MPU-6000, which does not have a VLOGIC pin. * @return I2C supply voltage level (0=VLOGIC, 1=VDD) */ -uint8_t MPU6050::getAuxVDDIOLevel() { - I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer); +uint8_t MPU6050_Base::getAuxVDDIOLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the auxiliary I2C supply voltage level. @@ -93,8 +87,8 @@ uint8_t MPU6050::getAuxVDDIOLevel() { * the MPU-6000, which does not have a VLOGIC pin. * @param level I2C supply voltage level (0=VLOGIC, 1=VDD) */ -void MPU6050::setAuxVDDIOLevel(uint8_t level) { - I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level); +void MPU6050_Base::setAuxVDDIOLevel(uint8_t level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, level, wireObj); } // SMPLRT_DIV register @@ -120,8 +114,8 @@ void MPU6050::setAuxVDDIOLevel(uint8_t level) { * @return Current sample rate * @see MPU6050_RA_SMPLRT_DIV */ -uint8_t MPU6050::getRate() { - I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer); +uint8_t MPU6050_Base::getRate() { + I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set gyroscope sample rate divider. @@ -129,8 +123,8 @@ uint8_t MPU6050::getRate() { * @see getRate() * @see MPU6050_RA_SMPLRT_DIV */ -void MPU6050::setRate(uint8_t rate) { - I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate); +void MPU6050_Base::setRate(uint8_t rate) { + I2Cdev::writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate, wireObj); } // CONFIG register @@ -162,8 +156,8 @@ void MPU6050::setRate(uint8_t rate) { * * @return FSYNC configuration value */ -uint8_t MPU6050::getExternalFrameSync() { - I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer); +uint8_t MPU6050_Base::getExternalFrameSync() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set external FSYNC configuration. @@ -171,8 +165,8 @@ uint8_t MPU6050::getExternalFrameSync() { * @see MPU6050_RA_CONFIG * @param sync New FSYNC configuration value */ -void MPU6050::setExternalFrameSync(uint8_t sync) { - I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync); +void MPU6050_Base::setExternalFrameSync(uint8_t sync) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync, wireObj); } /** Get digital low-pass filter configuration. * The DLPF_CFG parameter sets the digital low pass filter configuration. It @@ -202,8 +196,8 @@ void MPU6050::setExternalFrameSync(uint8_t sync) { * @see MPU6050_CFG_DLPF_CFG_BIT * @see MPU6050_CFG_DLPF_CFG_LENGTH */ -uint8_t MPU6050::getDLPFMode() { - I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer); +uint8_t MPU6050_Base::getDLPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set digital low-pass filter configuration. @@ -214,8 +208,8 @@ uint8_t MPU6050::getDLPFMode() { * @see MPU6050_CFG_DLPF_CFG_BIT * @see MPU6050_CFG_DLPF_CFG_LENGTH */ -void MPU6050::setDLPFMode(uint8_t mode) { - I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode); +void MPU6050_Base::setDLPFMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode, wireObj); } // GYRO_CONFIG register @@ -237,8 +231,8 @@ void MPU6050::setDLPFMode(uint8_t mode) { * @see MPU6050_GCONFIG_FS_SEL_BIT * @see MPU6050_GCONFIG_FS_SEL_LENGTH */ -uint8_t MPU6050::getFullScaleGyroRange() { - I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer); +uint8_t MPU6050_Base::getFullScaleGyroRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set full-scale gyroscope range. @@ -249,8 +243,66 @@ uint8_t MPU6050::getFullScaleGyroRange() { * @see MPU6050_GCONFIG_FS_SEL_BIT * @see MPU6050_GCONFIG_FS_SEL_LENGTH */ -void MPU6050::setFullScaleGyroRange(uint8_t range) { - I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range); +void MPU6050_Base::setFullScaleGyroRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range, wireObj); +} + +// SELF TEST FACTORY TRIM VALUES + +/** Get self-test factory trim value for accelerometer X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050_Base::getAccelXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0], I2Cdev::readTimeout, wireObj); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout, wireObj); + return (buffer[0]>>3) | ((buffer[1]>>4) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Y axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Y + */ +uint8_t MPU6050_Base::getAccelYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0], I2Cdev::readTimeout, wireObj); + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout, wireObj); + return (buffer[0]>>3) | ((buffer[1]>>2) & 0x03); +} + +/** Get self-test factory trim value for accelerometer Z axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_Z + */ +uint8_t MPU6050_Base::getAccelZSelfTestFactoryTrim() { + I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer, I2Cdev::readTimeout, wireObj); + return (buffer[0]>>3) | (buffer[1] & 0x03); +} + +/** Get self-test factory trim value for gyro X axis. + * @return factory trim value + * @see MPU6050_RA_SELF_TEST_X + */ +uint8_t MPU6050_Base::getGyroXSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer, I2Cdev::readTimeout, wireObj); + 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_Base::getGyroYSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer, I2Cdev::readTimeout, wireObj); + 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_Base::getGyroZSelfTestFactoryTrim() { + I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer, I2Cdev::readTimeout, wireObj); + return (buffer[0] & 0x1F); } // ACCEL_CONFIG register @@ -259,46 +311,46 @@ void MPU6050::setFullScaleGyroRange(uint8_t range) { * @return Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -bool MPU6050::getAccelXSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer); +bool MPU6050_Base::getAccelXSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get self-test enabled setting for accelerometer X axis. * @param enabled Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -void MPU6050::setAccelXSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled); +void MPU6050_Base::setAccelXSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, enabled, wireObj); } /** Get self-test enabled value for accelerometer Y axis. * @return Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -bool MPU6050::getAccelYSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer); +bool MPU6050_Base::getAccelYSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get self-test enabled value for accelerometer Y axis. * @param enabled Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -void MPU6050::setAccelYSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled); +void MPU6050_Base::setAccelYSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, enabled, wireObj); } /** Get self-test enabled value for accelerometer Z axis. * @return Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -bool MPU6050::getAccelZSelfTest() { - I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer); +bool MPU6050_Base::getAccelZSelfTest() { + I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set self-test enabled value for accelerometer Z axis. * @param enabled Self-test enabled value * @see MPU6050_RA_ACCEL_CONFIG */ -void MPU6050::setAccelZSelfTest(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled); +void MPU6050_Base::setAccelZSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, enabled, wireObj); } /** Get full-scale accelerometer range. * The FS_SEL parameter allows setting the full-scale range of the accelerometer @@ -317,16 +369,16 @@ void MPU6050::setAccelZSelfTest(bool enabled) { * @see MPU6050_ACONFIG_AFS_SEL_BIT * @see MPU6050_ACONFIG_AFS_SEL_LENGTH */ -uint8_t MPU6050::getFullScaleAccelRange() { - I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer); +uint8_t MPU6050_Base::getFullScaleAccelRange() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set full-scale accelerometer range. * @param range New full-scale accelerometer range setting * @see getFullScaleAccelRange() */ -void MPU6050::setFullScaleAccelRange(uint8_t range) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range); +void MPU6050_Base::setFullScaleAccelRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range, wireObj); } /** Get the high-pass filter configuration. * The DHPF is a filter module in the path leading to motion detectors (Free @@ -363,8 +415,8 @@ void MPU6050::setFullScaleAccelRange(uint8_t range) { * @see MPU6050_DHPF_RESET * @see MPU6050_RA_ACCEL_CONFIG */ -uint8_t MPU6050::getDHPFMode() { - I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer); +uint8_t MPU6050_Base::getDHPFMode() { + I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the high-pass filter configuration. @@ -373,8 +425,8 @@ uint8_t MPU6050::getDHPFMode() { * @see MPU6050_DHPF_RESET * @see MPU6050_RA_ACCEL_CONFIG */ -void MPU6050::setDHPFMode(uint8_t bandwidth) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth); +void MPU6050_Base::setDHPFMode(uint8_t bandwidth) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, bandwidth, wireObj); } // FF_THR register @@ -394,8 +446,8 @@ void MPU6050::setDHPFMode(uint8_t bandwidth) { * @return Current free-fall acceleration threshold value (LSB = 2mg) * @see MPU6050_RA_FF_THR */ -uint8_t MPU6050::getFreefallDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer); +uint8_t MPU6050_Base::getFreefallDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get free-fall event acceleration threshold. @@ -403,8 +455,8 @@ uint8_t MPU6050::getFreefallDetectionThreshold() { * @see getFreefallDetectionThreshold() * @see MPU6050_RA_FF_THR */ -void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold); +void MPU6050_Base::setFreefallDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_THR, threshold, wireObj); } // FF_DUR register @@ -426,8 +478,8 @@ void MPU6050::setFreefallDetectionThreshold(uint8_t threshold) { * @return Current free-fall duration threshold value (LSB = 1ms) * @see MPU6050_RA_FF_DUR */ -uint8_t MPU6050::getFreefallDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer); +uint8_t MPU6050_Base::getFreefallDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get free-fall event duration threshold. @@ -435,8 +487,8 @@ uint8_t MPU6050::getFreefallDetectionDuration() { * @see getFreefallDetectionDuration() * @see MPU6050_RA_FF_DUR */ -void MPU6050::setFreefallDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration); +void MPU6050_Base::setFreefallDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FF_DUR, duration, wireObj); } // MOT_THR register @@ -460,17 +512,17 @@ void MPU6050::setFreefallDetectionDuration(uint8_t duration) { * @return Current motion detection acceleration threshold value (LSB = 2mg) * @see MPU6050_RA_MOT_THR */ -uint8_t MPU6050::getMotionDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer); +uint8_t MPU6050_Base::getMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -/** Set free-fall event acceleration threshold. +/** 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); +void MPU6050_Base::setMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_THR, threshold, wireObj); } // MOT_DUR register @@ -490,8 +542,8 @@ void MPU6050::setMotionDetectionThreshold(uint8_t threshold) { * @return Current motion detection duration threshold value (LSB = 1ms) * @see MPU6050_RA_MOT_DUR */ -uint8_t MPU6050::getMotionDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer); +uint8_t MPU6050_Base::getMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set motion detection event duration threshold. @@ -499,8 +551,8 @@ uint8_t MPU6050::getMotionDetectionDuration() { * @see getMotionDetectionDuration() * @see MPU6050_RA_MOT_DUR */ -void MPU6050::setMotionDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration); +void MPU6050_Base::setMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MOT_DUR, duration, wireObj); } // ZRMOT_THR register @@ -530,8 +582,8 @@ void MPU6050::setMotionDetectionDuration(uint8_t duration) { * @return Current zero motion detection acceleration threshold value (LSB = 2mg) * @see MPU6050_RA_ZRMOT_THR */ -uint8_t MPU6050::getZeroMotionDetectionThreshold() { - I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer); +uint8_t MPU6050_Base::getZeroMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set zero motion detection event acceleration threshold. @@ -539,8 +591,8 @@ uint8_t MPU6050::getZeroMotionDetectionThreshold() { * @see getZeroMotionDetectionThreshold() * @see MPU6050_RA_ZRMOT_THR */ -void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) { - I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold); +void MPU6050_Base::setZeroMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_THR, threshold, wireObj); } // ZRMOT_DUR register @@ -561,8 +613,8 @@ void MPU6050::setZeroMotionDetectionThreshold(uint8_t threshold) { * @return Current zero motion detection duration threshold value (LSB = 64ms) * @see MPU6050_RA_ZRMOT_DUR */ -uint8_t MPU6050::getZeroMotionDetectionDuration() { - I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer); +uint8_t MPU6050_Base::getZeroMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set zero motion detection event duration threshold. @@ -570,8 +622,8 @@ uint8_t MPU6050::getZeroMotionDetectionDuration() { * @see getZeroMotionDetectionDuration() * @see MPU6050_RA_ZRMOT_DUR */ -void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) { - I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration); +void MPU6050_Base::setZeroMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU6050_RA_ZRMOT_DUR, duration, wireObj); } // FIFO_EN register @@ -582,8 +634,8 @@ void MPU6050::setZeroMotionDetectionDuration(uint8_t duration) { * @return Current temperature FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getTempFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getTempFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set temperature FIFO enabled value. @@ -591,8 +643,8 @@ bool MPU6050::getTempFIFOEnabled() { * @see getTempFIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setTempFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled); +void MPU6050_Base::setTempFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, enabled, wireObj); } /** Get gyroscope X-axis FIFO enabled value. * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and @@ -600,8 +652,8 @@ void MPU6050::setTempFIFOEnabled(bool enabled) { * @return Current gyroscope X-axis FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getXGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getXGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set gyroscope X-axis FIFO enabled value. @@ -609,8 +661,8 @@ bool MPU6050::getXGyroFIFOEnabled() { * @see getXGyroFIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setXGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled); +void MPU6050_Base::setXGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled, wireObj); } /** Get gyroscope Y-axis FIFO enabled value. * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and @@ -618,8 +670,8 @@ void MPU6050::setXGyroFIFOEnabled(bool enabled) { * @return Current gyroscope Y-axis FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getYGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getYGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set gyroscope Y-axis FIFO enabled value. @@ -627,8 +679,8 @@ bool MPU6050::getYGyroFIFOEnabled() { * @see getYGyroFIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setYGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled); +void MPU6050_Base::setYGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled, wireObj); } /** Get gyroscope Z-axis FIFO enabled value. * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and @@ -636,8 +688,8 @@ void MPU6050::setYGyroFIFOEnabled(bool enabled) { * @return Current gyroscope Z-axis FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getZGyroFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getZGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set gyroscope Z-axis FIFO enabled value. @@ -645,8 +697,8 @@ bool MPU6050::getZGyroFIFOEnabled() { * @see getZGyroFIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setZGyroFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled); +void MPU6050_Base::setZGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled, wireObj); } /** Get accelerometer FIFO enabled value. * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H, @@ -655,8 +707,8 @@ void MPU6050::setZGyroFIFOEnabled(bool enabled) { * @return Current accelerometer FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getAccelFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getAccelFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set accelerometer FIFO enabled value. @@ -664,8 +716,8 @@ bool MPU6050::getAccelFIFOEnabled() { * @see getAccelFIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setAccelFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled); +void MPU6050_Base::setAccelFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled, wireObj); } /** Get Slave 2 FIFO enabled value. * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) @@ -673,8 +725,8 @@ void MPU6050::setAccelFIFOEnabled(bool enabled) { * @return Current Slave 2 FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getSlave2FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getSlave2FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Slave 2 FIFO enabled value. @@ -682,8 +734,8 @@ bool MPU6050::getSlave2FIFOEnabled() { * @see getSlave2FIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setSlave2FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled); +void MPU6050_Base::setSlave2FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, enabled, wireObj); } /** Get Slave 1 FIFO enabled value. * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) @@ -691,8 +743,8 @@ void MPU6050::setSlave2FIFOEnabled(bool enabled) { * @return Current Slave 1 FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getSlave1FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getSlave1FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Slave 1 FIFO enabled value. @@ -700,8 +752,8 @@ bool MPU6050::getSlave1FIFOEnabled() { * @see getSlave1FIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setSlave1FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled); +void MPU6050_Base::setSlave1FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, enabled, wireObj); } /** Get Slave 0 FIFO enabled value. * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) @@ -709,8 +761,8 @@ void MPU6050::setSlave1FIFOEnabled(bool enabled) { * @return Current Slave 0 FIFO enabled value * @see MPU6050_RA_FIFO_EN */ -bool MPU6050::getSlave0FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getSlave0FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Slave 0 FIFO enabled value. @@ -718,8 +770,8 @@ bool MPU6050::getSlave0FIFOEnabled() { * @see getSlave0FIFOEnabled() * @see MPU6050_RA_FIFO_EN */ -void MPU6050::setSlave0FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled); +void MPU6050_Base::setSlave0FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, enabled, wireObj); } // I2C_MST_CTRL register @@ -739,8 +791,8 @@ void MPU6050::setSlave0FIFOEnabled(bool enabled) { * @return Current multi-master enabled value * @see MPU6050_RA_I2C_MST_CTRL */ -bool MPU6050::getMultiMasterEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer); +bool MPU6050_Base::getMultiMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set multi-master enabled value. @@ -748,8 +800,8 @@ bool MPU6050::getMultiMasterEnabled() { * @see getMultiMasterEnabled() * @see MPU6050_RA_I2C_MST_CTRL */ -void MPU6050::setMultiMasterEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled); +void MPU6050_Base::setMultiMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, enabled, wireObj); } /** Get wait-for-external-sensor-data enabled value. * When the WAIT_FOR_ES bit is set to 1, the Data Ready interrupt will be @@ -762,8 +814,8 @@ void MPU6050::setMultiMasterEnabled(bool enabled) { * @return Current wait-for-external-sensor-data enabled value * @see MPU6050_RA_I2C_MST_CTRL */ -bool MPU6050::getWaitForExternalSensorEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer); +bool MPU6050_Base::getWaitForExternalSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set wait-for-external-sensor-data enabled value. @@ -771,8 +823,8 @@ bool MPU6050::getWaitForExternalSensorEnabled() { * @see getWaitForExternalSensorEnabled() * @see MPU6050_RA_I2C_MST_CTRL */ -void MPU6050::setWaitForExternalSensorEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled); +void MPU6050_Base::setWaitForExternalSensorEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, enabled, wireObj); } /** Get Slave 3 FIFO enabled value. * When set to 1, this bit enables EXT_SENS_DATA registers (Registers 73 to 96) @@ -780,8 +832,8 @@ void MPU6050::setWaitForExternalSensorEnabled(bool enabled) { * @return Current Slave 3 FIFO enabled value * @see MPU6050_RA_MST_CTRL */ -bool MPU6050::getSlave3FIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getSlave3FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Slave 3 FIFO enabled value. @@ -789,8 +841,8 @@ bool MPU6050::getSlave3FIFOEnabled() { * @see getSlave3FIFOEnabled() * @see MPU6050_RA_MST_CTRL */ -void MPU6050::setSlave3FIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled); +void MPU6050_Base::setSlave3FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, enabled, wireObj); } /** Get slave read/write transition enabled value. * The I2C_MST_P_NSR bit configures the I2C Master's transition from one slave @@ -802,8 +854,8 @@ void MPU6050::setSlave3FIFOEnabled(bool enabled) { * @return Current slave read/write transition enabled value * @see MPU6050_RA_I2C_MST_CTRL */ -bool MPU6050::getSlaveReadWriteTransitionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer); +bool MPU6050_Base::getSlaveReadWriteTransitionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set slave read/write transition enabled value. @@ -811,8 +863,8 @@ bool MPU6050::getSlaveReadWriteTransitionEnabled() { * @see getSlaveReadWriteTransitionEnabled() * @see MPU6050_RA_I2C_MST_CTRL */ -void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled); +void MPU6050_Base::setSlaveReadWriteTransitionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, enabled, wireObj); } /** Get I2C master clock speed. * I2C_MST_CLK is a 4 bit unsigned value which configures a divider on the @@ -843,16 +895,16 @@ void MPU6050::setSlaveReadWriteTransitionEnabled(bool enabled) { * @return Current I2C master clock speed * @see MPU6050_RA_I2C_MST_CTRL */ -uint8_t MPU6050::getMasterClockSpeed() { - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer); +uint8_t MPU6050_Base::getMasterClockSpeed() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set I2C master clock speed. * @reparam speed Current I2C master clock speed * @see MPU6050_RA_I2C_MST_CTRL */ -void MPU6050::setMasterClockSpeed(uint8_t speed) { - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed); +void MPU6050_Base::setMasterClockSpeed(uint8_t speed) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed, wireObj); } // I2C_SLV* registers (Slave 0-3) @@ -898,9 +950,9 @@ void MPU6050::setMasterClockSpeed(uint8_t speed) { * @return Current address for specified slave * @see MPU6050_RA_I2C_SLV0_ADDR */ -uint8_t MPU6050::getSlaveAddress(uint8_t num) { +uint8_t MPU6050_Base::getSlaveAddress(uint8_t num) { if (num > 3) return 0; - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer); + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the I2C address of the specified slave (0-3). @@ -909,9 +961,9 @@ uint8_t MPU6050::getSlaveAddress(uint8_t num) { * @see getSlaveAddress() * @see MPU6050_RA_I2C_SLV0_ADDR */ -void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) { +void MPU6050_Base::setSlaveAddress(uint8_t num, uint8_t address) { if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, address, wireObj); } /** Get the active internal register for the specified slave (0-3). * Read/write operations for this slave will be done to whatever internal @@ -924,9 +976,9 @@ void MPU6050::setSlaveAddress(uint8_t num, uint8_t address) { * @return Current active register for specified slave * @see MPU6050_RA_I2C_SLV0_REG */ -uint8_t MPU6050::getSlaveRegister(uint8_t num) { +uint8_t MPU6050_Base::getSlaveRegister(uint8_t num) { if (num > 3) return 0; - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer); + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the active internal register for the specified slave (0-3). @@ -935,9 +987,9 @@ uint8_t MPU6050::getSlaveRegister(uint8_t num) { * @see getSlaveRegister() * @see MPU6050_RA_I2C_SLV0_REG */ -void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) { +void MPU6050_Base::setSlaveRegister(uint8_t num, uint8_t reg) { if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, reg, wireObj); } /** Get the enabled value for the specified slave (0-3). * When set to 1, this bit enables Slave 0 for data transfer operations. When @@ -946,9 +998,9 @@ void MPU6050::setSlaveRegister(uint8_t num, uint8_t reg) { * @return Current enabled value for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ -bool MPU6050::getSlaveEnabled(uint8_t num) { +bool MPU6050_Base::getSlaveEnabled(uint8_t num) { if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer); + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the enabled value for the specified slave (0-3). @@ -957,9 +1009,9 @@ bool MPU6050::getSlaveEnabled(uint8_t num) { * @see getSlaveEnabled() * @see MPU6050_RA_I2C_SLV0_CTRL */ -void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) { +void MPU6050_Base::setSlaveEnabled(uint8_t num, bool enabled) { if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled, wireObj); } /** Get word pair byte-swapping enabled for the specified slave (0-3). * When set to 1, this bit enables byte swapping. When byte swapping is enabled, @@ -972,9 +1024,9 @@ void MPU6050::setSlaveEnabled(uint8_t num, bool enabled) { * @return Current word pair byte-swapping enabled value for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ -bool MPU6050::getSlaveWordByteSwap(uint8_t num) { +bool MPU6050_Base::getSlaveWordByteSwap(uint8_t num) { if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer); + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set word pair byte-swapping enabled for the specified slave (0-3). @@ -983,9 +1035,9 @@ bool MPU6050::getSlaveWordByteSwap(uint8_t num) { * @see getSlaveWordByteSwap() * @see MPU6050_RA_I2C_SLV0_CTRL */ -void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) { +void MPU6050_Base::setSlaveWordByteSwap(uint8_t num, bool enabled) { if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled, wireObj); } /** Get write mode for the specified slave (0-3). * When set to 1, the transaction will read or write data only. When cleared to @@ -997,9 +1049,9 @@ void MPU6050::setSlaveWordByteSwap(uint8_t num, bool enabled) { * @return Current write mode for specified slave (0 = register address + data, 1 = data only) * @see MPU6050_RA_I2C_SLV0_CTRL */ -bool MPU6050::getSlaveWriteMode(uint8_t num) { +bool MPU6050_Base::getSlaveWriteMode(uint8_t num) { if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer); + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set write mode for the specified slave (0-3). @@ -1008,9 +1060,9 @@ bool MPU6050::getSlaveWriteMode(uint8_t num) { * @see getSlaveWriteMode() * @see MPU6050_RA_I2C_SLV0_CTRL */ -void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) { +void MPU6050_Base::setSlaveWriteMode(uint8_t num, bool mode) { if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode, wireObj); } /** Get word pair grouping order offset for the specified slave (0-3). * This sets specifies the grouping order of word pairs received from registers. @@ -1023,9 +1075,9 @@ void MPU6050::setSlaveWriteMode(uint8_t num, bool mode) { * @return Current word pair grouping order offset for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ -bool MPU6050::getSlaveWordGroupOffset(uint8_t num) { +bool MPU6050_Base::getSlaveWordGroupOffset(uint8_t num) { if (num > 3) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer); + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set word pair grouping order offset for the specified slave (0-3). @@ -1034,9 +1086,9 @@ bool MPU6050::getSlaveWordGroupOffset(uint8_t num) { * @see getSlaveWordGroupOffset() * @see MPU6050_RA_I2C_SLV0_CTRL */ -void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) { +void MPU6050_Base::setSlaveWordGroupOffset(uint8_t num, bool enabled) { if (num > 3) return; - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled); + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled, wireObj); } /** Get number of bytes to read for the specified slave (0-3). * Specifies the number of bytes transferred to and from Slave 0. Clearing this @@ -1045,9 +1097,9 @@ void MPU6050::setSlaveWordGroupOffset(uint8_t num, bool enabled) { * @return Number of bytes to read for specified slave * @see MPU6050_RA_I2C_SLV0_CTRL */ -uint8_t MPU6050::getSlaveDataLength(uint8_t num) { +uint8_t MPU6050_Base::getSlaveDataLength(uint8_t num) { if (num > 3) return 0; - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer); + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set number of bytes to read for the specified slave (0-3). @@ -1056,9 +1108,9 @@ uint8_t MPU6050::getSlaveDataLength(uint8_t num) { * @see getSlaveDataLength() * @see MPU6050_RA_I2C_SLV0_CTRL */ -void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) { +void MPU6050_Base::setSlaveDataLength(uint8_t num, uint8_t length) { if (num > 3) return; - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length); + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length, wireObj); } // I2C_SLV* registers (Slave 4) @@ -1072,8 +1124,8 @@ void MPU6050::setSlaveDataLength(uint8_t num, uint8_t length) { * @see getSlaveAddress() * @see MPU6050_RA_I2C_SLV4_ADDR */ -uint8_t MPU6050::getSlave4Address() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer); +uint8_t MPU6050_Base::getSlave4Address() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the I2C address of Slave 4. @@ -1081,8 +1133,8 @@ uint8_t MPU6050::getSlave4Address() { * @see getSlave4Address() * @see MPU6050_RA_I2C_SLV4_ADDR */ -void MPU6050::setSlave4Address(uint8_t address) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address); +void MPU6050_Base::setSlave4Address(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, address, wireObj); } /** Get the active internal register for the Slave 4. * Read/write operations for this slave will be done to whatever internal @@ -1091,8 +1143,8 @@ void MPU6050::setSlave4Address(uint8_t address) { * @return Current active register for Slave 4 * @see MPU6050_RA_I2C_SLV4_REG */ -uint8_t MPU6050::getSlave4Register() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer); +uint8_t MPU6050_Base::getSlave4Register() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the active internal register for Slave 4. @@ -1100,8 +1152,8 @@ uint8_t MPU6050::getSlave4Register() { * @see getSlave4Register() * @see MPU6050_RA_I2C_SLV4_REG */ -void MPU6050::setSlave4Register(uint8_t reg) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg); +void MPU6050_Base::setSlave4Register(uint8_t reg) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_REG, reg, wireObj); } /** Set new byte to write to Slave 4. * This register stores the data to be written into the Slave 4. If I2C_SLV4_RW @@ -1109,8 +1161,8 @@ void MPU6050::setSlave4Register(uint8_t reg) { * @param data New byte to write to Slave 4 * @see MPU6050_RA_I2C_SLV4_DO */ -void MPU6050::setSlave4OutputByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data); +void MPU6050_Base::setSlave4OutputByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV4_DO, data, wireObj); } /** Get the enabled value for the Slave 4. * When set to 1, this bit enables Slave 4 for data transfer operations. When @@ -1118,8 +1170,8 @@ void MPU6050::setSlave4OutputByte(uint8_t data) { * @return Current enabled value for Slave 4 * @see MPU6050_RA_I2C_SLV4_CTRL */ -bool MPU6050::getSlave4Enabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer); +bool MPU6050_Base::getSlave4Enabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the enabled value for Slave 4. @@ -1127,8 +1179,8 @@ bool MPU6050::getSlave4Enabled() { * @see getSlave4Enabled() * @see MPU6050_RA_I2C_SLV4_CTRL */ -void MPU6050::setSlave4Enabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled); +void MPU6050_Base::setSlave4Enabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, enabled, wireObj); } /** Get the enabled value for Slave 4 transaction interrupts. * When set to 1, this bit enables the generation of an interrupt signal upon @@ -1139,8 +1191,8 @@ void MPU6050::setSlave4Enabled(bool enabled) { * @return Current enabled value for Slave 4 transaction interrupts. * @see MPU6050_RA_I2C_SLV4_CTRL */ -bool MPU6050::getSlave4InterruptEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer); +bool MPU6050_Base::getSlave4InterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set the enabled value for Slave 4 transaction interrupts. @@ -1148,8 +1200,8 @@ bool MPU6050::getSlave4InterruptEnabled() { * @see getSlave4InterruptEnabled() * @see MPU6050_RA_I2C_SLV4_CTRL */ -void MPU6050::setSlave4InterruptEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled); +void MPU6050_Base::setSlave4InterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, enabled, wireObj); } /** Get write mode for Slave 4. * When set to 1, the transaction will read or write data only. When cleared to @@ -1160,8 +1212,8 @@ void MPU6050::setSlave4InterruptEnabled(bool enabled) { * @return Current write mode for Slave 4 (0 = register address + data, 1 = data only) * @see MPU6050_RA_I2C_SLV4_CTRL */ -bool MPU6050::getSlave4WriteMode() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer); +bool MPU6050_Base::getSlave4WriteMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set write mode for the Slave 4. @@ -1169,8 +1221,8 @@ bool MPU6050::getSlave4WriteMode() { * @see getSlave4WriteMode() * @see MPU6050_RA_I2C_SLV4_CTRL */ -void MPU6050::setSlave4WriteMode(bool mode) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode); +void MPU6050_Base::setSlave4WriteMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mode, wireObj); } /** Get Slave 4 master delay value. * This configures the reduced access rate of I2C slaves relative to the Sample @@ -1187,8 +1239,8 @@ void MPU6050::setSlave4WriteMode(bool mode) { * @return Current Slave 4 master delay value * @see MPU6050_RA_I2C_SLV4_CTRL */ -uint8_t MPU6050::getSlave4MasterDelay() { - I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer); +uint8_t MPU6050_Base::getSlave4MasterDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Slave 4 master delay value. @@ -1196,8 +1248,8 @@ uint8_t MPU6050::getSlave4MasterDelay() { * @see getSlave4MasterDelay() * @see MPU6050_RA_I2C_SLV4_CTRL */ -void MPU6050::setSlave4MasterDelay(uint8_t delay) { - I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay); +void MPU6050_Base::setSlave4MasterDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay, wireObj); } /** Get last available byte read from Slave 4. * This register stores the data read from Slave 4. This field is populated @@ -1205,8 +1257,8 @@ void MPU6050::setSlave4MasterDelay(uint8_t delay) { * @return Last available byte read from to Slave 4 * @see MPU6050_RA_I2C_SLV4_DI */ -uint8_t MPU6050::getSlate4InputByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer); +uint8_t MPU6050_Base::getSlate4InputByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } @@ -1221,8 +1273,8 @@ uint8_t MPU6050::getSlate4InputByte() { * @return FSYNC interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getPassthroughStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer); +bool MPU6050_Base::getPassthroughStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 4 transaction done status. @@ -1233,8 +1285,8 @@ bool MPU6050::getPassthroughStatus() { * @return Slave 4 transaction done status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave4IsDone() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer); +bool MPU6050_Base::getSlave4IsDone() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get master arbitration lost status. @@ -1244,8 +1296,8 @@ bool MPU6050::getSlave4IsDone() { * @return Master arbitration lost status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getLostArbitration() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer); +bool MPU6050_Base::getLostArbitration() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 4 NACK status. @@ -1255,8 +1307,8 @@ bool MPU6050::getLostArbitration() { * @return Slave 4 NACK interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave4Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer); +bool MPU6050_Base::getSlave4Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 3 NACK status. @@ -1266,8 +1318,8 @@ bool MPU6050::getSlave4Nack() { * @return Slave 3 NACK interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave3Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer); +bool MPU6050_Base::getSlave3Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 2 NACK status. @@ -1277,8 +1329,8 @@ bool MPU6050::getSlave3Nack() { * @return Slave 2 NACK interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave2Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer); +bool MPU6050_Base::getSlave2Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 1 NACK status. @@ -1288,8 +1340,8 @@ bool MPU6050::getSlave2Nack() { * @return Slave 1 NACK interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave1Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer); +bool MPU6050_Base::getSlave1Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Slave 0 NACK status. @@ -1299,8 +1351,8 @@ bool MPU6050::getSlave1Nack() { * @return Slave 0 NACK interrupt status * @see MPU6050_RA_I2C_MST_STATUS */ -bool MPU6050::getSlave0Nack() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer); +bool MPU6050_Base::getSlave0Nack() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } @@ -1312,8 +1364,8 @@ bool MPU6050::getSlave0Nack() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_LEVEL_BIT */ -bool MPU6050::getInterruptMode() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer); +bool MPU6050_Base::getInterruptMode() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set interrupt logic level mode. @@ -1322,8 +1374,8 @@ bool MPU6050::getInterruptMode() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_LEVEL_BIT */ -void MPU6050::setInterruptMode(bool mode) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode); +void MPU6050_Base::setInterruptMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mode, wireObj); } /** Get interrupt drive mode. * Will be set 0 for push-pull, 1 for open-drain. @@ -1331,8 +1383,8 @@ void MPU6050::setInterruptMode(bool mode) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_OPEN_BIT */ -bool MPU6050::getInterruptDrive() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer); +bool MPU6050_Base::getInterruptDrive() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set interrupt drive mode. @@ -1341,8 +1393,8 @@ bool MPU6050::getInterruptDrive() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_OPEN_BIT */ -void MPU6050::setInterruptDrive(bool drive) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive); +void MPU6050_Base::setInterruptDrive(bool drive) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, drive, wireObj); } /** Get interrupt latch mode. * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared. @@ -1350,8 +1402,8 @@ void MPU6050::setInterruptDrive(bool drive) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_LATCH_INT_EN_BIT */ -bool MPU6050::getInterruptLatch() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer); +bool MPU6050_Base::getInterruptLatch() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set interrupt latch mode. @@ -1360,8 +1412,8 @@ bool MPU6050::getInterruptLatch() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_LATCH_INT_EN_BIT */ -void MPU6050::setInterruptLatch(bool latch) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch); +void MPU6050_Base::setInterruptLatch(bool latch) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, latch, wireObj); } /** Get interrupt latch clear mode. * Will be set 0 for status-read-only, 1 for any-register-read. @@ -1369,8 +1421,8 @@ void MPU6050::setInterruptLatch(bool latch) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT */ -bool MPU6050::getInterruptLatchClear() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer); +bool MPU6050_Base::getInterruptLatchClear() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set interrupt latch clear mode. @@ -1379,8 +1431,8 @@ bool MPU6050::getInterruptLatchClear() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_INT_RD_CLEAR_BIT */ -void MPU6050::setInterruptLatchClear(bool clear) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear); +void MPU6050_Base::setInterruptLatchClear(bool clear) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, clear, wireObj); } /** Get FSYNC interrupt logic level mode. * @return Current FSYNC interrupt mode (0=active-high, 1=active-low) @@ -1388,8 +1440,8 @@ void MPU6050::setInterruptLatchClear(bool clear) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT */ -bool MPU6050::getFSyncInterruptLevel() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer); +bool MPU6050_Base::getFSyncInterruptLevel() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set FSYNC interrupt logic level mode. @@ -1398,8 +1450,8 @@ bool MPU6050::getFSyncInterruptLevel() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT */ -void MPU6050::setFSyncInterruptLevel(bool level) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level); +void MPU6050_Base::setFSyncInterruptLevel(bool level) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, level, wireObj); } /** Get FSYNC pin interrupt enabled setting. * Will be set 0 for disabled, 1 for enabled. @@ -1407,8 +1459,8 @@ void MPU6050::setFSyncInterruptLevel(bool level) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT */ -bool MPU6050::getFSyncInterruptEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer); +bool MPU6050_Base::getFSyncInterruptEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set FSYNC pin interrupt enabled setting. @@ -1417,8 +1469,8 @@ bool MPU6050::getFSyncInterruptEnabled() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_FSYNC_INT_EN_BIT */ -void MPU6050::setFSyncInterruptEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled); +void MPU6050_Base::setFSyncInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, enabled, wireObj); } /** Get I2C bypass enabled status. * When this bit is equal to 1 and I2C_MST_EN (Register 106 bit[5]) is equal to @@ -1431,8 +1483,8 @@ void MPU6050::setFSyncInterruptEnabled(bool enabled) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT */ -bool MPU6050::getI2CBypassEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer); +bool MPU6050_Base::getI2CBypassEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set I2C bypass enabled status. @@ -1446,8 +1498,8 @@ bool MPU6050::getI2CBypassEnabled() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_I2C_BYPASS_EN_BIT */ -void MPU6050::setI2CBypassEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled); +void MPU6050_Base::setI2CBypassEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, enabled, wireObj); } /** Get reference clock output enabled status. * When this bit is equal to 1, a reference clock output is provided at the @@ -1458,8 +1510,8 @@ void MPU6050::setI2CBypassEnabled(bool enabled) { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_CLKOUT_EN_BIT */ -bool MPU6050::getClockOutputEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer); +bool MPU6050_Base::getClockOutputEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set reference clock output enabled status. @@ -1471,8 +1523,8 @@ bool MPU6050::getClockOutputEnabled() { * @see MPU6050_RA_INT_PIN_CFG * @see MPU6050_INTCFG_CLKOUT_EN_BIT */ -void MPU6050::setClockOutputEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled); +void MPU6050_Base::setClockOutputEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, enabled, wireObj); } // INT_ENABLE register @@ -1484,8 +1536,8 @@ void MPU6050::setClockOutputEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FF_BIT **/ -uint8_t MPU6050::getIntEnabled() { - I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer); +uint8_t MPU6050_Base::getIntEnabled() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set full interrupt enabled status. @@ -1496,8 +1548,8 @@ uint8_t MPU6050::getIntEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FF_BIT **/ -void MPU6050::setIntEnabled(uint8_t enabled) { - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled); +void MPU6050_Base::setIntEnabled(uint8_t enabled) { + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, enabled, wireObj); } /** Get Free Fall interrupt enabled status. * Will be set 0 for disabled, 1 for enabled. @@ -1505,8 +1557,8 @@ void MPU6050::setIntEnabled(uint8_t enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FF_BIT **/ -bool MPU6050::getIntFreefallEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer); +bool MPU6050_Base::getIntFreefallEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Free Fall interrupt enabled status. @@ -1515,8 +1567,8 @@ bool MPU6050::getIntFreefallEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FF_BIT **/ -void MPU6050::setIntFreefallEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled); +void MPU6050_Base::setIntFreefallEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, enabled, wireObj); } /** Get Motion Detection interrupt enabled status. * Will be set 0 for disabled, 1 for enabled. @@ -1524,8 +1576,8 @@ void MPU6050::setIntFreefallEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_MOT_BIT **/ -bool MPU6050::getIntMotionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer); +bool MPU6050_Base::getIntMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Motion Detection interrupt enabled status. @@ -1534,8 +1586,8 @@ bool MPU6050::getIntMotionEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_MOT_BIT **/ -void MPU6050::setIntMotionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled); +void MPU6050_Base::setIntMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, enabled, wireObj); } /** Get Zero Motion Detection interrupt enabled status. * Will be set 0 for disabled, 1 for enabled. @@ -1543,8 +1595,8 @@ void MPU6050::setIntMotionEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_ZMOT_BIT **/ -bool MPU6050::getIntZeroMotionEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer); +bool MPU6050_Base::getIntZeroMotionEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Zero Motion Detection interrupt enabled status. @@ -1553,8 +1605,8 @@ bool MPU6050::getIntZeroMotionEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_ZMOT_BIT **/ -void MPU6050::setIntZeroMotionEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled); +void MPU6050_Base::setIntZeroMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, enabled, wireObj); } /** Get FIFO Buffer Overflow interrupt enabled status. * Will be set 0 for disabled, 1 for enabled. @@ -1562,8 +1614,8 @@ void MPU6050::setIntZeroMotionEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT **/ -bool MPU6050::getIntFIFOBufferOverflowEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); +bool MPU6050_Base::getIntFIFOBufferOverflowEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set FIFO Buffer Overflow interrupt enabled status. @@ -1572,8 +1624,8 @@ bool MPU6050::getIntFIFOBufferOverflowEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT **/ -void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled); +void MPU6050_Base::setIntFIFOBufferOverflowEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, enabled, wireObj); } /** Get I2C Master interrupt enabled status. * This enables any of the I2C Master interrupt sources to generate an @@ -1582,8 +1634,8 @@ void MPU6050::setIntFIFOBufferOverflowEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT **/ -bool MPU6050::getIntI2CMasterEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); +bool MPU6050_Base::getIntI2CMasterEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set I2C Master interrupt enabled status. @@ -1592,8 +1644,8 @@ bool MPU6050::getIntI2CMasterEnabled() { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT **/ -void MPU6050::setIntI2CMasterEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled); +void MPU6050_Base::setIntI2CMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, enabled, wireObj); } /** Get Data Ready interrupt enabled setting. * This event occurs each time a write operation to all of the sensor registers @@ -1602,8 +1654,8 @@ void MPU6050::setIntI2CMasterEnabled(bool enabled) { * @see MPU6050_RA_INT_ENABLE * @see MPU6050_INTERRUPT_DATA_RDY_BIT */ -bool MPU6050::getIntDataReadyEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); +bool MPU6050_Base::getIntDataReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Data Ready interrupt enabled status. @@ -1612,8 +1664,8 @@ bool MPU6050::getIntDataReadyEnabled() { * @see MPU6050_RA_INT_CFG * @see MPU6050_INTERRUPT_DATA_RDY_BIT */ -void MPU6050::setIntDataReadyEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled); +void MPU6050_Base::setIntDataReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, enabled, wireObj); } // INT_STATUS register @@ -1625,8 +1677,8 @@ void MPU6050::setIntDataReadyEnabled(bool enabled) { * @return Current interrupt status * @see MPU6050_RA_INT_STATUS */ -uint8_t MPU6050::getIntStatus() { - I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer); +uint8_t MPU6050_Base::getIntStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Free Fall interrupt status. @@ -1636,8 +1688,8 @@ uint8_t MPU6050::getIntStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_FF_BIT */ -bool MPU6050::getIntFreefallStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer); +bool MPU6050_Base::getIntFreefallStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Motion Detection interrupt status. @@ -1647,8 +1699,8 @@ bool MPU6050::getIntFreefallStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_MOT_BIT */ -bool MPU6050::getIntMotionStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer); +bool MPU6050_Base::getIntMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Zero Motion Detection interrupt status. @@ -1658,8 +1710,8 @@ bool MPU6050::getIntMotionStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_ZMOT_BIT */ -bool MPU6050::getIntZeroMotionStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer); +bool MPU6050_Base::getIntZeroMotionStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get FIFO Buffer Overflow interrupt status. @@ -1669,8 +1721,8 @@ bool MPU6050::getIntZeroMotionStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_FIFO_OFLOW_BIT */ -bool MPU6050::getIntFIFOBufferOverflowStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer); +bool MPU6050_Base::getIntFIFOBufferOverflowStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get I2C Master interrupt status. @@ -1681,8 +1733,8 @@ bool MPU6050::getIntFIFOBufferOverflowStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_I2C_MST_INT_BIT */ -bool MPU6050::getIntI2CMasterStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer); +bool MPU6050_Base::getIntI2CMasterStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Data Ready interrupt status. @@ -1692,8 +1744,8 @@ bool MPU6050::getIntI2CMasterStatus() { * @see MPU6050_RA_INT_STATUS * @see MPU6050_INTERRUPT_DATA_RDY_BIT */ -bool MPU6050::getIntDataReadyStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer); +bool MPU6050_Base::getIntDataReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } @@ -1715,7 +1767,11 @@ bool MPU6050::getIntDataReadyStatus() { * @see getRotation() * @see MPU6050_RA_ACCEL_XOUT_H */ -void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { +void MPU6050_Base::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) { + (void)mx; // unused parameter + (void)my; // unused parameter + (void)mz; // unused parameter + getMotion6(ax, ay, az, gx, gy, gz); // TODO: magnetometer integration } @@ -1731,8 +1787,8 @@ void MPU6050::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int * @see getRotation() * @see MPU6050_RA_ACCEL_XOUT_H */ -void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer); +void MPU6050_Base::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer, I2Cdev::readTimeout, wireObj); *ax = (((int16_t)buffer[0]) << 8) | buffer[1]; *ay = (((int16_t)buffer[2]) << 8) | buffer[3]; *az = (((int16_t)buffer[4]) << 8) | buffer[5]; @@ -1765,10 +1821,10 @@ void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int * 
  * AFS_SEL | Full Scale Range | LSB Sensitivity
  * --------+------------------+----------------
- * 0       | +/- 2g           | 8192 LSB/mg
- * 1       | +/- 4g           | 4096 LSB/mg
- * 2       | +/- 8g           | 2048 LSB/mg
- * 3       | +/- 16g          | 1024 LSB/mg
+ * 0       | +/- 2g           | 16384 LSB/mg
+ * 1       | +/- 4g           | 8192 LSB/mg
+ * 2       | +/- 8g           | 4096 LSB/mg
+ * 3       | +/- 16g          | 2048 LSB/mg
  *
* * @param x 16-bit signed integer container for X-axis acceleration @@ -1776,8 +1832,8 @@ void MPU6050::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int * @param z 16-bit signed integer container for Z-axis acceleration * @see MPU6050_RA_GYRO_XOUT_H */ -void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer); +void MPU6050_Base::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer, I2Cdev::readTimeout, wireObj); *x = (((int16_t)buffer[0]) << 8) | buffer[1]; *y = (((int16_t)buffer[2]) << 8) | buffer[3]; *z = (((int16_t)buffer[4]) << 8) | buffer[5]; @@ -1787,8 +1843,8 @@ void MPU6050::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { * @see getMotion6() * @see MPU6050_RA_ACCEL_XOUT_H */ -int16_t MPU6050::getAccelerationX() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer); +int16_t MPU6050_Base::getAccelerationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } /** Get Y-axis accelerometer reading. @@ -1796,8 +1852,8 @@ int16_t MPU6050::getAccelerationX() { * @see getMotion6() * @see MPU6050_RA_ACCEL_YOUT_H */ -int16_t MPU6050::getAccelerationY() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer); +int16_t MPU6050_Base::getAccelerationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } /** Get Z-axis accelerometer reading. @@ -1805,8 +1861,8 @@ int16_t MPU6050::getAccelerationY() { * @see getMotion6() * @see MPU6050_RA_ACCEL_ZOUT_H */ -int16_t MPU6050::getAccelerationZ() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer); +int16_t MPU6050_Base::getAccelerationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } @@ -1816,8 +1872,8 @@ int16_t MPU6050::getAccelerationZ() { * @return Temperature reading in 16-bit 2's complement format * @see MPU6050_RA_TEMP_OUT_H */ -int16_t MPU6050::getTemperature() { - I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer); +int16_t MPU6050_Base::getTemperature() { + I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } @@ -1855,8 +1911,8 @@ int16_t MPU6050::getTemperature() { * @see getMotion6() * @see MPU6050_RA_GYRO_XOUT_H */ -void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer); +void MPU6050_Base::getRotation(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer, I2Cdev::readTimeout, wireObj); *x = (((int16_t)buffer[0]) << 8) | buffer[1]; *y = (((int16_t)buffer[2]) << 8) | buffer[3]; *z = (((int16_t)buffer[4]) << 8) | buffer[5]; @@ -1866,8 +1922,8 @@ void MPU6050::getRotation(int16_t* x, int16_t* y, int16_t* z) { * @see getMotion6() * @see MPU6050_RA_GYRO_XOUT_H */ -int16_t MPU6050::getRotationX() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer); +int16_t MPU6050_Base::getRotationX() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } /** Get Y-axis gyroscope reading. @@ -1875,8 +1931,8 @@ int16_t MPU6050::getRotationX() { * @see getMotion6() * @see MPU6050_RA_GYRO_YOUT_H */ -int16_t MPU6050::getRotationY() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer); +int16_t MPU6050_Base::getRotationY() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } /** Get Z-axis gyroscope reading. @@ -1884,8 +1940,8 @@ int16_t MPU6050::getRotationY() { * @see getMotion6() * @see MPU6050_RA_GYRO_ZOUT_H */ -int16_t MPU6050::getRotationZ() { - I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer); +int16_t MPU6050_Base::getRotationZ() { + I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } @@ -1965,8 +2021,8 @@ int16_t MPU6050::getRotationZ() { * @param position Starting position (0-23) * @return Byte read from register */ -uint8_t MPU6050::getExternalSensorByte(int position) { - I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer); +uint8_t MPU6050_Base::getExternalSensorByte(int position) { + I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Read word (2 bytes) from external sensor data registers. @@ -1974,8 +2030,8 @@ uint8_t MPU6050::getExternalSensorByte(int position) { * @return Word read from register * @see getExternalSensorByte() */ -uint16_t MPU6050::getExternalSensorWord(int position) { - I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer); +uint16_t MPU6050_Base::getExternalSensorWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer, I2Cdev::readTimeout, wireObj); return (((uint16_t)buffer[0]) << 8) | buffer[1]; } /** Read double word (4 bytes) from external sensor data registers. @@ -1983,20 +2039,28 @@ uint16_t MPU6050::getExternalSensorWord(int position) { * @return Double word read from registers * @see getExternalSensorByte() */ -uint32_t MPU6050::getExternalSensorDWord(int position) { - I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer); +uint32_t MPU6050_Base::getExternalSensorDWord(int position) { + I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer, I2Cdev::readTimeout, wireObj); return (((uint32_t)buffer[0]) << 24) | (((uint32_t)buffer[1]) << 16) | (((uint16_t)buffer[2]) << 8) | buffer[3]; } // 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_Base::getMotionStatus() { + I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} /** Get X-axis negative motion detection interrupt status. * @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); +bool MPU6050_Base::getXNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get X-axis positive motion detection interrupt status. @@ -2004,8 +2068,8 @@ bool MPU6050::getXNegMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_XPOS_BIT */ -bool MPU6050::getXPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer); +bool MPU6050_Base::getXPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Y-axis negative motion detection interrupt status. @@ -2013,8 +2077,8 @@ bool MPU6050::getXPosMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_YNEG_BIT */ -bool MPU6050::getYNegMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer); +bool MPU6050_Base::getYNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Y-axis positive motion detection interrupt status. @@ -2022,8 +2086,8 @@ bool MPU6050::getYNegMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_YPOS_BIT */ -bool MPU6050::getYPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer); +bool MPU6050_Base::getYPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Z-axis negative motion detection interrupt status. @@ -2031,8 +2095,8 @@ bool MPU6050::getYPosMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_ZNEG_BIT */ -bool MPU6050::getZNegMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer); +bool MPU6050_Base::getZNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get Z-axis positive motion detection interrupt status. @@ -2040,8 +2104,8 @@ bool MPU6050::getZNegMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_ZPOS_BIT */ -bool MPU6050::getZPosMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer); +bool MPU6050_Base::getZPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Get zero motion detection interrupt status. @@ -2049,8 +2113,8 @@ bool MPU6050::getZPosMotionDetected() { * @see MPU6050_RA_MOT_DETECT_STATUS * @see MPU6050_MOTION_MOT_ZRMOT_BIT */ -bool MPU6050::getZeroMotionDetected() { - I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer); +bool MPU6050_Base::getZeroMotionDetected() { + I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } @@ -2064,9 +2128,9 @@ bool MPU6050::getZeroMotionDetected() { * @param data Byte to write * @see MPU6050_RA_I2C_SLV0_DO */ -void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) { +void MPU6050_Base::setSlaveOutputByte(uint8_t num, uint8_t data) { if (num > 3) return; - I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data); + I2Cdev::writeByte(devAddr, MPU6050_RA_I2C_SLV0_DO + num, data, wireObj); } // I2C_MST_DELAY_CTRL register @@ -2079,8 +2143,8 @@ void MPU6050::setSlaveOutputByte(uint8_t num, uint8_t data) { * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT */ -bool MPU6050::getExternalShadowDelayEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer); +bool MPU6050_Base::getExternalShadowDelayEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set external data shadow delay enabled status. @@ -2089,8 +2153,8 @@ bool MPU6050::getExternalShadowDelayEnabled() { * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT */ -void MPU6050::setExternalShadowDelayEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled); +void MPU6050_Base::setExternalShadowDelayEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, enabled, wireObj); } /** Get slave delay enabled status. * When a particular slave delay is enabled, the rate of access for the that @@ -2110,10 +2174,10 @@ void MPU6050::setExternalShadowDelayEnabled(bool enabled) { * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT */ -bool MPU6050::getSlaveDelayEnabled(uint8_t num) { +bool MPU6050_Base::getSlaveDelayEnabled(uint8_t num) { // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. if (num > 4) return 0; - I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer); + I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set slave delay enabled status. @@ -2122,8 +2186,8 @@ bool MPU6050::getSlaveDelayEnabled(uint8_t num) { * @see MPU6050_RA_I2C_MST_DELAY_CTRL * @see MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT */ -void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled); +void MPU6050_Base::setSlaveDelayEnabled(uint8_t num, bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, enabled, wireObj); } // SIGNAL_PATH_RESET register @@ -2134,8 +2198,8 @@ void MPU6050::setSlaveDelayEnabled(uint8_t num, bool enabled) { * @see MPU6050_RA_SIGNAL_PATH_RESET * @see MPU6050_PATHRESET_GYRO_RESET_BIT */ -void MPU6050::resetGyroscopePath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true); +void MPU6050_Base::resetGyroscopePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_GYRO_RESET_BIT, true, wireObj); } /** Reset accelerometer signal path. * The reset will revert the signal path analog to digital converters and @@ -2143,8 +2207,8 @@ void MPU6050::resetGyroscopePath() { * @see MPU6050_RA_SIGNAL_PATH_RESET * @see MPU6050_PATHRESET_ACCEL_RESET_BIT */ -void MPU6050::resetAccelerometerPath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true); +void MPU6050_Base::resetAccelerometerPath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_ACCEL_RESET_BIT, true, wireObj); } /** Reset temperature sensor signal path. * The reset will revert the signal path analog to digital converters and @@ -2152,8 +2216,8 @@ void MPU6050::resetAccelerometerPath() { * @see MPU6050_RA_SIGNAL_PATH_RESET * @see MPU6050_PATHRESET_TEMP_RESET_BIT */ -void MPU6050::resetTemperaturePath() { - I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true); +void MPU6050_Base::resetTemperaturePath() { + I2Cdev::writeBit(devAddr, MPU6050_RA_SIGNAL_PATH_RESET, MPU6050_PATHRESET_TEMP_RESET_BIT, true, wireObj); } // MOT_DETECT_CTRL register @@ -2172,8 +2236,8 @@ void MPU6050::resetTemperaturePath() { * @see MPU6050_RA_MOT_DETECT_CTRL * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT */ -uint8_t MPU6050::getAccelerometerPowerOnDelay() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer); +uint8_t MPU6050_Base::getAccelerometerPowerOnDelay() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set accelerometer power-on delay. @@ -2182,8 +2246,8 @@ uint8_t MPU6050::getAccelerometerPowerOnDelay() { * @see MPU6050_RA_MOT_DETECT_CTRL * @see MPU6050_DETECT_ACCEL_ON_DELAY_BIT */ -void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay); +void MPU6050_Base::setAccelerometerPowerOnDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay, wireObj); } /** Get Free Fall detection counter decrement configuration. * Detection is registered by the Free Fall detection module after accelerometer @@ -2211,8 +2275,8 @@ void MPU6050::setAccelerometerPowerOnDelay(uint8_t delay) { * @see MPU6050_RA_MOT_DETECT_CTRL * @see MPU6050_DETECT_FF_COUNT_BIT */ -uint8_t MPU6050::getFreefallDetectionCounterDecrement() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer); +uint8_t MPU6050_Base::getFreefallDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Free Fall detection counter decrement configuration. @@ -2221,8 +2285,8 @@ uint8_t MPU6050::getFreefallDetectionCounterDecrement() { * @see MPU6050_RA_MOT_DETECT_CTRL * @see MPU6050_DETECT_FF_COUNT_BIT */ -void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement); +void MPU6050_Base::setFreefallDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, decrement, wireObj); } /** Get Motion detection counter decrement configuration. * Detection is registered by the Motion detection module after accelerometer @@ -2247,8 +2311,8 @@ void MPU6050::setFreefallDetectionCounterDecrement(uint8_t decrement) { * please refer to Registers 29 to 32. * */ -uint8_t MPU6050::getMotionDetectionCounterDecrement() { - I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer); +uint8_t MPU6050_Base::getMotionDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Motion detection counter decrement configuration. @@ -2257,8 +2321,8 @@ uint8_t MPU6050::getMotionDetectionCounterDecrement() { * @see MPU6050_RA_MOT_DETECT_CTRL * @see MPU6050_DETECT_MOT_COUNT_BIT */ -void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) { - I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement); +void MPU6050_Base::setMotionDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, decrement, wireObj); } // USER_CTRL register @@ -2271,8 +2335,8 @@ void MPU6050::setMotionDetectionCounterDecrement(uint8_t decrement) { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_FIFO_EN_BIT */ -bool MPU6050::getFIFOEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer); +bool MPU6050_Base::getFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set FIFO enabled status. @@ -2281,8 +2345,8 @@ bool MPU6050::getFIFOEnabled() { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_FIFO_EN_BIT */ -void MPU6050::setFIFOEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled); +void MPU6050_Base::setFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled, wireObj); } /** Get I2C Master Mode enabled status. * When this mode is enabled, the MPU-60X0 acts as the I2C Master to the @@ -2295,8 +2359,8 @@ void MPU6050::setFIFOEnabled(bool enabled) { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_I2C_MST_EN_BIT */ -bool MPU6050::getI2CMasterModeEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer); +bool MPU6050_Base::getI2CMasterModeEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set I2C Master Mode enabled status. @@ -2305,15 +2369,15 @@ bool MPU6050::getI2CMasterModeEnabled() { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_I2C_MST_EN_BIT */ -void MPU6050::setI2CMasterModeEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled); +void MPU6050_Base::setI2CMasterModeEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, enabled, wireObj); } /** Switch from I2C to SPI mode (MPU-6000 only) * If this is set, the primary SPI interface will be enabled in place of the * disabled primary I2C interface. */ -void MPU6050::switchSPIEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled); +void MPU6050_Base::switchSPIEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled, wireObj); } /** Reset the FIFO. * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This @@ -2321,8 +2385,8 @@ void MPU6050::switchSPIEnabled(bool enabled) { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_FIFO_RESET_BIT */ -void MPU6050::resetFIFO() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true); +void MPU6050_Base::resetFIFO() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true, wireObj); } /** Reset the I2C Master. * This bit resets the I2C Master when set to 1 while I2C_MST_EN equals 0. @@ -2330,8 +2394,8 @@ void MPU6050::resetFIFO() { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_I2C_MST_RESET_BIT */ -void MPU6050::resetI2CMaster() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true); +void MPU6050_Base::resetI2CMaster() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true, wireObj); } /** Reset all sensor registers and signal paths. * When set to 1, this bit resets the signal paths for all sensors (gyroscopes, @@ -2345,8 +2409,8 @@ void MPU6050::resetI2CMaster() { * @see MPU6050_RA_USER_CTRL * @see MPU6050_USERCTRL_SIG_COND_RESET_BIT */ -void MPU6050::resetSensors() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true); +void MPU6050_Base::resetSensors() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_SIG_COND_RESET_BIT, true, wireObj); } // PWR_MGMT_1 register @@ -2356,8 +2420,8 @@ void MPU6050::resetSensors() { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_DEVICE_RESET_BIT */ -void MPU6050::reset() { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true); +void MPU6050_Base::reset() { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true, wireObj); } /** Get sleep mode status. * Setting the SLEEP bit in the register puts the device into very low power @@ -2370,8 +2434,8 @@ void MPU6050::reset() { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_SLEEP_BIT */ -bool MPU6050::getSleepEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer); +bool MPU6050_Base::getSleepEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set sleep mode status. @@ -2380,8 +2444,8 @@ bool MPU6050::getSleepEnabled() { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_SLEEP_BIT */ -void MPU6050::setSleepEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled); +void MPU6050_Base::setSleepEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled, wireObj); } /** Get wake cycle enabled status. * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle @@ -2391,8 +2455,8 @@ void MPU6050::setSleepEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_CYCLE_BIT */ -bool MPU6050::getWakeCycleEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer); +bool MPU6050_Base::getWakeCycleEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set wake cycle enabled status. @@ -2401,8 +2465,8 @@ bool MPU6050::getWakeCycleEnabled() { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_CYCLE_BIT */ -void MPU6050::setWakeCycleEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled); +void MPU6050_Base::setWakeCycleEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, enabled, wireObj); } /** Get temperature sensor enabled status. * Control the usage of the internal temperature sensor. @@ -2415,8 +2479,8 @@ void MPU6050::setWakeCycleEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_TEMP_DIS_BIT */ -bool MPU6050::getTempSensorEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer); +bool MPU6050_Base::getTempSensorEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0] == 0; // 1 is actually disabled here } /** Set temperature sensor enabled status. @@ -2429,9 +2493,9 @@ bool MPU6050::getTempSensorEnabled() { * @see MPU6050_RA_PWR_MGMT_1 * @see MPU6050_PWR1_TEMP_DIS_BIT */ -void MPU6050::setTempSensorEnabled(bool enabled) { +void MPU6050_Base::setTempSensorEnabled(bool enabled) { // 1 is actually disabled here - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled); + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, !enabled, wireObj); } /** Get clock source setting. * @return Current clock source setting @@ -2439,8 +2503,8 @@ void MPU6050::setTempSensorEnabled(bool enabled) { * @see MPU6050_PWR1_CLKSEL_BIT * @see MPU6050_PWR1_CLKSEL_LENGTH */ -uint8_t MPU6050::getClockSource() { - I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer); +uint8_t MPU6050_Base::getClockSource() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set clock source setting. @@ -2473,8 +2537,8 @@ uint8_t MPU6050::getClockSource() { * @see MPU6050_PWR1_CLKSEL_BIT * @see MPU6050_PWR1_CLKSEL_LENGTH */ -void MPU6050::setClockSource(uint8_t source) { - I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source); +void MPU6050_Base::setClockSource(uint8_t source) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source, wireObj); } // PWR_MGMT_2 register @@ -2492,9 +2556,9 @@ void MPU6050::setClockSource(uint8_t source) { * -------------+------------------ * 0 | 1.25 Hz * 1 | 2.5 Hz - * 2 | 5 Hz - * 3 | 10 Hz - * 
+ * 2            | 20 Hz
+ * 3            | 40 Hz
+ *
* * For further information regarding the MPU-60X0's power modes, please refer to * Register 107. @@ -2502,16 +2566,16 @@ void MPU6050::setClockSource(uint8_t source) { * @return Current wake frequency * @see MPU6050_RA_PWR_MGMT_2 */ -uint8_t MPU6050::getWakeFrequency() { - I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer); +uint8_t MPU6050_Base::getWakeFrequency() { + I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set wake frequency in Accel-Only Low Power Mode. * @param frequency New wake frequency * @see MPU6050_RA_PWR_MGMT_2 */ -void MPU6050::setWakeFrequency(uint8_t frequency) { - I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency); +void MPU6050_Base::setWakeFrequency(uint8_t frequency) { + I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency, wireObj); } /** Get X-axis accelerometer standby enabled status. @@ -2520,8 +2584,8 @@ void MPU6050::setWakeFrequency(uint8_t frequency) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_XA_BIT */ -bool MPU6050::getStandbyXAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer); +bool MPU6050_Base::getStandbyXAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set X-axis accelerometer standby enabled status. @@ -2530,8 +2594,8 @@ bool MPU6050::getStandbyXAccelEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_XA_BIT */ -void MPU6050::setStandbyXAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled); +void MPU6050_Base::setStandbyXAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, enabled, wireObj); } /** Get Y-axis accelerometer standby enabled status. * If enabled, the Y-axis will not gather or report data (or use power). @@ -2539,8 +2603,8 @@ void MPU6050::setStandbyXAccelEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_YA_BIT */ -bool MPU6050::getStandbyYAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer); +bool MPU6050_Base::getStandbyYAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Y-axis accelerometer standby enabled status. @@ -2549,8 +2613,8 @@ bool MPU6050::getStandbyYAccelEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_YA_BIT */ -void MPU6050::setStandbyYAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled); +void MPU6050_Base::setStandbyYAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, enabled, wireObj); } /** Get Z-axis accelerometer standby enabled status. * If enabled, the Z-axis will not gather or report data (or use power). @@ -2558,8 +2622,8 @@ void MPU6050::setStandbyYAccelEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_ZA_BIT */ -bool MPU6050::getStandbyZAccelEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer); +bool MPU6050_Base::getStandbyZAccelEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Z-axis accelerometer standby enabled status. @@ -2568,8 +2632,8 @@ bool MPU6050::getStandbyZAccelEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_ZA_BIT */ -void MPU6050::setStandbyZAccelEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled); +void MPU6050_Base::setStandbyZAccelEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, enabled, wireObj); } /** Get X-axis gyroscope standby enabled status. * If enabled, the X-axis will not gather or report data (or use power). @@ -2577,8 +2641,8 @@ void MPU6050::setStandbyZAccelEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_XG_BIT */ -bool MPU6050::getStandbyXGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer); +bool MPU6050_Base::getStandbyXGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set X-axis gyroscope standby enabled status. @@ -2587,8 +2651,8 @@ bool MPU6050::getStandbyXGyroEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_XG_BIT */ -void MPU6050::setStandbyXGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled); +void MPU6050_Base::setStandbyXGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, enabled, wireObj); } /** Get Y-axis gyroscope standby enabled status. * If enabled, the Y-axis will not gather or report data (or use power). @@ -2596,8 +2660,8 @@ void MPU6050::setStandbyXGyroEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_YG_BIT */ -bool MPU6050::getStandbyYGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer); +bool MPU6050_Base::getStandbyYGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Y-axis gyroscope standby enabled status. @@ -2606,8 +2670,8 @@ bool MPU6050::getStandbyYGyroEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_YG_BIT */ -void MPU6050::setStandbyYGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled); +void MPU6050_Base::setStandbyYGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, enabled, wireObj); } /** Get Z-axis gyroscope standby enabled status. * If enabled, the Z-axis will not gather or report data (or use power). @@ -2615,8 +2679,8 @@ void MPU6050::setStandbyYGyroEnabled(bool enabled) { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_ZG_BIT */ -bool MPU6050::getStandbyZGyroEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer); +bool MPU6050_Base::getStandbyZGyroEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Z-axis gyroscope standby enabled status. @@ -2625,8 +2689,8 @@ bool MPU6050::getStandbyZGyroEnabled() { * @see MPU6050_RA_PWR_MGMT_2 * @see MPU6050_PWR2_STBY_ZG_BIT */ -void MPU6050::setStandbyZGyroEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled); +void MPU6050_Base::setStandbyZGyroEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled, wireObj); } // FIFO_COUNT* registers @@ -2638,8 +2702,8 @@ void MPU6050::setStandbyZGyroEnabled(bool enabled) { * set of sensor data bound to be stored in the FIFO (register 35 and 36). * @return Current FIFO buffer size */ -uint16_t MPU6050::getFIFOCount() { - I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer); +uint16_t MPU6050_Base::getFIFOCount() { + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer, I2Cdev::readTimeout, wireObj); return (((uint16_t)buffer[0]) << 8) | buffer[1]; } @@ -2670,19 +2734,82 @@ uint16_t MPU6050::getFIFOCount() { * * @return Byte from FIFO buffer */ -uint8_t MPU6050::getFIFOByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer); +uint8_t MPU6050_Base::getFIFOByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::getFIFOBytes(uint8_t *data, uint8_t length) { - I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data); +void MPU6050_Base::getFIFOBytes(uint8_t *data, uint8_t length) { + if(length > 0){ + I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data, I2Cdev::readTimeout, wireObj); + } else { + *data = 0; + } } + +/** Get timeout to get a packet from FIFO buffer. + * @return Current timeout to get a packet from FIFO buffer + * @see MPU6050_FIFO_DEFAULT_TIMEOUT + */ +uint32_t MPU6050_Base::getFIFOTimeout() { + return fifoTimeout; +} + +/** Set timeout to get a packet from FIFO buffer. + * @param New timeout to get a packet from FIFO buffer + * @see MPU6050_FIFO_DEFAULT_TIMEOUT + */ +void MPU6050_Base::setFIFOTimeout(uint32_t fifoTimeout) { + this->fifoTimeout = fifoTimeout; +} + +/** Get latest byte from FIFO buffer no matter how much time has passed. + * === GetCurrentFIFOPacket === + * ================================================================ + * Returns 1) when nothing special was done + * 2) when recovering from overflow + * 0) when no valid data is available + * ================================================================ */ + int8_t MPU6050_Base::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof + int16_t fifoC; + // This section of code is for when we allowed more than 1 packet to be acquired + uint32_t BreakTimer = micros(); + bool packetReceived = false; + do { + if ((fifoC = getFIFOCount()) > length) { + + if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to reset the buffer and wait for the next to arrive + resetFIFO(); // Fixes any overflow corruption + fifoC = 0; + while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (getFIFOTimeout()))); // Get Next New Packet + } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer + uint8_t Trash[I2CDEVLIB_WIRE_BUFFER_LENGTH]; + while ((fifoC = getFIFOCount()) > length) { // Test each time just in case the MPU is writing to the FIFO Buffer + fifoC = fifoC - length; // Save the last packet + uint16_t RemoveBytes; + while (fifoC) { // fifo count will reach zero so this is safe + RemoveBytes = (fifoC < I2CDEVLIB_WIRE_BUFFER_LENGTH) ? fifoC : I2CDEVLIB_WIRE_BUFFER_LENGTH; // Buffer Length is different than the packet length this will efficiently clear the buffer + getFIFOBytes(Trash, (uint8_t)RemoveBytes); + fifoC -= RemoveBytes; + } + } + } + } + if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset + // We have 1 packet + packetReceived = fifoC == length; + if (!packetReceived && (micros() - BreakTimer) > (getFIFOTimeout())) return 0; + } while (!packetReceived); + getFIFOBytes(data, length); //Get 1 packet + return 1; +} + + /** Write byte to FIFO buffer. * @see getFIFOByte() * @see MPU6050_RA_FIFO_R_W */ -void MPU6050::setFIFOByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data); +void MPU6050_Base::setFIFOByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data, wireObj); } // WHO_AM_I register @@ -2694,8 +2821,8 @@ void MPU6050::setFIFOByte(uint8_t data) { * @see MPU6050_WHO_AM_I_BIT * @see MPU6050_WHO_AM_I_LENGTH */ -uint8_t MPU6050::getDeviceID() { - I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer); +uint8_t MPU6050_Base::getDeviceID() { + I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } /** Set Device ID. @@ -2707,232 +2834,238 @@ uint8_t MPU6050::getDeviceID() { * @see MPU6050_WHO_AM_I_BIT * @see MPU6050_WHO_AM_I_LENGTH */ -void MPU6050::setDeviceID(uint8_t id) { - I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id); +void MPU6050_Base::setDeviceID(uint8_t id) { + I2Cdev::writeBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, id, wireObj); } // ======== UNDOCUMENTED/DMP REGISTERS/METHODS ======== // XG_OFFS_TC register -uint8_t MPU6050::getOTPBankValid() { - I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer); +uint8_t MPU6050_Base::getOTPBankValid() { + I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setOTPBankValid(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled); +void MPU6050_Base::setOTPBankValid(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled, wireObj); } -int8_t MPU6050::getXGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); +int8_t MPU6050_Base::getXGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setXGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +void MPU6050_Base::setXGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj); } // YG_OFFS_TC register -int8_t MPU6050::getYGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); +int8_t MPU6050_Base::getYGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setYGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +void MPU6050_Base::setYGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj); } // ZG_OFFS_TC register -int8_t MPU6050::getZGyroOffsetTC() { - I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); +int8_t MPU6050_Base::getZGyroOffsetTC() { + I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setZGyroOffsetTC(int8_t offset) { - I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset); +void MPU6050_Base::setZGyroOffsetTC(int8_t offset) { + I2Cdev::writeBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset, wireObj); } // X_FINE_GAIN register -int8_t MPU6050::getXFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer); +int8_t MPU6050_Base::getXFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setXFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain); +void MPU6050_Base::setXFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain, wireObj); } // Y_FINE_GAIN register -int8_t MPU6050::getYFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer); +int8_t MPU6050_Base::getYFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setYFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain); +void MPU6050_Base::setYFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain, wireObj); } // Z_FINE_GAIN register -int8_t MPU6050::getZFineGain() { - I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer); +int8_t MPU6050_Base::getZFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setZFineGain(int8_t gain) { - I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain); +void MPU6050_Base::setZFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain, wireObj); } // XA_OFFS_* registers -int16_t MPU6050::getXAccelOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_XA_OFFS_H, 2, buffer); - return (((int16_t)buffer[0]) << 8) | buffer[1]; +int16_t MPU6050_Base::getXAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj); + return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setXAccelOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_XA_OFFS_H, offset); +void MPU6050_Base::setXAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj); } // 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]; +int16_t MPU6050_Base::getYAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj); + return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setYAccelOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_YA_OFFS_H, offset); +void MPU6050_Base::setYAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj); } // 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]; +int16_t MPU6050_Base::getZAccelOffset() { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout, wireObj); + return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setZAccelOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_ZA_OFFS_H, offset); +void MPU6050_Base::setZAccelOffset(int16_t offset) { + uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250 + I2Cdev::writeWord(devAddr, SaveAddress, offset, wireObj); } // XG_OFFS_USR* registers -int16_t MPU6050::getXGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer); +int16_t MPU6050_Base::getXGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setXGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset); +void MPU6050_Base::setXGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset, wireObj); } // YG_OFFS_USR* register -int16_t MPU6050::getYGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer); +int16_t MPU6050_Base::getYGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setYGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset); +void MPU6050_Base::setYGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset, wireObj); } // ZG_OFFS_USR* register -int16_t MPU6050::getZGyroOffset() { - I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer); +int16_t MPU6050_Base::getZGyroOffset() { + I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout, wireObj); return (((int16_t)buffer[0]) << 8) | buffer[1]; } -void MPU6050::setZGyroOffset(int16_t offset) { - I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset); +void MPU6050_Base::setZGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset, wireObj); } // INT_ENABLE register (DMP functions) -bool MPU6050::getIntPLLReadyEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); +bool MPU6050_Base::getIntPLLReadyEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setIntPLLReadyEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled); +void MPU6050_Base::setIntPLLReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled, wireObj); } -bool MPU6050::getIntDMPEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); +bool MPU6050_Base::getIntDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setIntDMPEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled); +void MPU6050_Base::setIntDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled, wireObj); } // DMP_INT_STATUS -bool MPU6050::getDMPInt5Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer); +bool MPU6050_Base::getDMPInt5Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getDMPInt4Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer); +bool MPU6050_Base::getDMPInt4Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getDMPInt3Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer); +bool MPU6050_Base::getDMPInt3Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getDMPInt2Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer); +bool MPU6050_Base::getDMPInt2Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getDMPInt1Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer); +bool MPU6050_Base::getDMPInt1Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getDMPInt0Status() { - I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer); +bool MPU6050_Base::getDMPInt0Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } // INT_STATUS register (DMP functions) -bool MPU6050::getIntPLLReadyStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer); +bool MPU6050_Base::getIntPLLReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -bool MPU6050::getIntDMPStatus() { - I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer); +bool MPU6050_Base::getIntDMPStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } // USER_CTRL register (DMP functions) -bool MPU6050::getDMPEnabled() { - I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer); +bool MPU6050_Base::getDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setDMPEnabled(bool enabled) { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled); +void MPU6050_Base::setDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled, wireObj); } -void MPU6050::resetDMP() { - I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true); +void MPU6050_Base::resetDMP() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true, wireObj); } // BANK_SEL register -void MPU6050::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) { +void MPU6050_Base::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) { bank &= 0x1F; if (userBank) bank |= 0x20; if (prefetchEnabled) bank |= 0x40; - I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank); + I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank, wireObj); } // MEM_START_ADDR register -void MPU6050::setMemoryStartAddress(uint8_t address) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address); +void MPU6050_Base::setMemoryStartAddress(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address, wireObj); } // MEM_R_W register -uint8_t MPU6050::readMemoryByte() { - I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer); +uint8_t MPU6050_Base::readMemoryByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::writeMemoryByte(uint8_t data) { - I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data); +void MPU6050_Base::writeMemoryByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data, wireObj); } -void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { +void MPU6050_Base::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) { setMemoryBank(bank); setMemoryStartAddress(address); uint8_t chunkSize; @@ -2947,7 +3080,7 @@ void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, ui if (chunkSize > 256 - address) chunkSize = 256 - address; // read the chunk of data as specified - I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i); + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i, I2Cdev::readTimeout, wireObj); // increase byte index by [chunkSize] i += chunkSize; @@ -2963,12 +3096,12 @@ void MPU6050::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, ui } } } -bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) { +bool MPU6050_Base::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) { setMemoryBank(bank); setMemoryStartAddress(address); uint8_t chunkSize; - uint8_t *verifyBuffer; - uint8_t *progBuffer; + 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); @@ -2991,13 +3124,13 @@ bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t b progBuffer = (uint8_t *)data + i; } - I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); + I2Cdev::writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer, wireObj); // verify data if needed if (verify && verifyBuffer) { setMemoryBank(bank); setMemoryStartAddress(address); - I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); + I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer, I2Cdev::readTimeout, wireObj); if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { /*Serial.print("Block write verification error, bank "); Serial.print(bank, DEC); @@ -3039,11 +3172,12 @@ bool MPU6050::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t b if (useProgMem) free(progBuffer); return true; } -bool MPU6050::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) { +bool MPU6050_Base::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) { return writeMemoryBlock(data, dataSize, bank, address, verify, true); } -bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { - uint8_t *progBuffer, success, special; +bool MPU6050_Base::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) { + uint8_t *progBuffer = 0; + uint8_t success, special; uint16_t i, j; if (useProgMem) { progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary @@ -3100,7 +3234,7 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, b //setIntZeroMotionEnabled(true); //setIntFIFOBufferOverflowEnabled(true); //setIntDMPEnabled(true); - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_ENABLE, 0x32, wireObj); // single operation success = true; } else { @@ -3117,26 +3251,147 @@ bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, b if (useProgMem) free(progBuffer); return true; } -bool MPU6050::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) { +bool MPU6050_Base::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) { return writeDMPConfigurationSet(data, dataSize, true); } // DMP_CFG_1 register -uint8_t MPU6050::getDMPConfig1() { - I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer); +uint8_t MPU6050_Base::getDMPConfig1() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setDMPConfig1(uint8_t config) { - I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config); +void MPU6050_Base::setDMPConfig1(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config, wireObj); } // DMP_CFG_2 register -uint8_t MPU6050::getDMPConfig2() { - I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer); +uint8_t MPU6050_Base::getDMPConfig2() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer, I2Cdev::readTimeout, wireObj); return buffer[0]; } -void MPU6050::setDMPConfig2(uint8_t config) { - I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config); -} \ No newline at end of file +void MPU6050_Base::setDMPConfig2(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config, wireObj); +} + + +//*************************************************************************************** +//********************** Calibration Routines ********************** +//*************************************************************************************** +/** + @brief Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings +*/ +void MPU6050_Base::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_Base::CalibrateAccel(uint8_t Loops ) { + + float kP = 0.3; + float kI = 20; + float x; + x = (100 - map(Loops, 1, 5, 20, 0)) * .01; + kP *= x; + kI *= x; + PID( 0x3B, kP, kI, Loops); +} + +void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){ + uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13; + + int16_t Data; + float Reading; + int16_t BitZero[3]; + uint8_t shift =(SaveAddress == 0x77)?3:2; + float Error, PTerm, ITerm[3]; + int16_t eSample; + uint32_t eSum; + uint16_t gravity = 8192; // prevent uninitialized compiler warning + if (ReadAddress == 0x3B) gravity = 16384 >> getFullScaleAccelRange(); + Serial.write('>'); + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if(SaveAddress != 0x13){ + BitZero[i] = Data & 1; // Capture Bit Zero to properly handle Accelerometer calibration + ITerm[i] = ((float)Reading) * 8; + } else { + ITerm[i] = Reading * 4; + } + } + for (int L = 0; L < Loops; L++) { + eSample = 0; + for (int c = 0; c < 100; c++) {// 100 PI Calculations + eSum = 0; + for (int i = 0; i < 3; i++) { + I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data, I2Cdev::readTimeout, wireObj); // reads 1 or more 16 bit integers (Word) + Reading = Data; + if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= gravity; //remove Gravity + Error = -Reading; + eSum += abs(Reading); + PTerm = kP * Error; + ITerm[i] += (Error * 0.001) * kI; // Integral term 1000 Calculations a second = 0.001 + if(SaveAddress != 0x13){ + Data = round((PTerm + ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((PTerm + ITerm[i] ) / 4); //Compute PID Output + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj); + } + if((c == 99) && eSum > 1000){ // Error is still to great to continue + c = 0; + Serial.write('*'); + } + if((eSum * ((ReadAddress == 0x3B)?.05: 1)) < 5) eSample++; // Successfully found offsets prepare to advance + if((eSum < 100) && (c > 10) && (eSample >= 10)) break; // Advance to next Loop + delay(1); + } + Serial.write('.'); + kP *= .75; + kI *= .75; + for (int i = 0; i < 3; i++){ + if(SaveAddress != 0x13) { + Data = round((ITerm[i] ) / 8); //Compute PID Output + Data = ((Data)&0xFFFE) |BitZero[i]; // Insert Bit0 Saved at beginning + } else Data = round((ITerm[i]) / 4); + I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, wireObj); + } + } + resetFIFO(); + resetDMP(); +} + +int16_t * MPU6050_Base::GetActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)offsets, I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(offsets+1), I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(offsets+2), I2Cdev::readTimeout, wireObj); + } + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)(offsets+3), I2Cdev::readTimeout, wireObj); + return offsets; +} + +void MPU6050_Base::PrintActiveOffsets() { + GetActiveOffsets(); + // A_OFFSET_H_READ_A_OFFS(Data); + Serial.print((float)offsets[0], 5); Serial.print(",\t"); + Serial.print((float)offsets[1], 5); Serial.print(",\t"); + Serial.print((float)offsets[2], 5); Serial.print(",\t"); + + // XG_OFFSET_H_READ_OFFS_USR(Data); + Serial.print((float)offsets[3], 5); Serial.print(",\t"); + Serial.print((float)offsets[4], 5); Serial.print(",\t"); + Serial.print((float)offsets[5], 5); Serial.print("\n\n"); +} diff --git a/Arduino/MPU6050/MPU6050.h b/Arduino/MPU6050/MPU6050.h index adfbb5d5..7cbbb9dd 100644 --- a/Arduino/MPU6050/MPU6050.h +++ b/Arduino/MPU6050/MPU6050.h @@ -4,6 +4,7 @@ // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2021/09/27 - split implementations out of header files, finally // ... - ongoing debug release // NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE @@ -38,8 +39,22 @@ THE SOFTWARE. #define _MPU6050_H_ #include "I2Cdev.h" -//#include +#include "helper_3dmath.h" +// supporting link: http://forum.arduino.cc/index.php?&topic=143444.msg1079517#msg1079517 +// also: http://forum.arduino.cc/index.php?&topic=141571.msg1062899#msg1062899s + +#ifdef __AVR__ +#include +#elif defined(ESP32) + #include +#else +//#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 +#endif #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board @@ -58,6 +73,10 @@ THE SOFTWARE. #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 @@ -157,6 +176,26 @@ THE SOFTWARE. #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 @@ -396,12 +435,11 @@ THE SOFTWARE. #define MPU6050_DMP_MEMORY_BANK_SIZE 256 #define MPU6050_DMP_MEMORY_CHUNK_SIZE 16 -// note: DMP code memory blocks defined at end of header file +#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000 -class MPU6050 { +class MPU6050_Base { public: - MPU6050(); - MPU6050(uint8_t address); + MPU6050_Base(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0); void initialize(); bool testConnection(); @@ -424,6 +462,15 @@ class MPU6050 { 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); @@ -598,6 +645,7 @@ class MPU6050 { uint32_t getExternalSensorDWord(int position); // MOT_DETECT_STATUS register + uint8_t getMotionStatus(); bool getXNegMotionDetected(); bool getXPosMotionDetected(); bool getYNegMotionDetected(); @@ -670,8 +718,11 @@ class MPU6050 { // FIFO_R_W register uint8_t getFIFOByte(); + int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length); void setFIFOByte(uint8_t data); void getFIFOBytes(uint8_t *data, uint8_t length); + void setFIFOTimeout(uint32_t fifoTimeout); + uint32_t getFIFOTimeout(); // WHO_AM_I register uint8_t getDeviceID(); @@ -776,212 +827,26 @@ 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 + // Calibration Routines + 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 math + void PrintActiveOffsets(); // See the results of the Calibration + int16_t * GetActiveOffsets(); - private: + protected: uint8_t devAddr; + void *wireObj; uint8_t buffer[14]; + uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT; + + private: + int16_t offsets[6]; }; +#ifndef I2CDEVLIB_MPU6050_TYPEDEF +#define I2CDEVLIB_MPU6050_TYPEDEF +typedef MPU6050_Base MPU6050; +#endif + #endif /* _MPU6050_H_ */ diff --git a/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.cpp b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.cpp new file mode 100644 index 00000000..3a896745 --- /dev/null +++ b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.cpp @@ -0,0 +1,616 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array +// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2021 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. +=============================================== +*/ + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU6050_6Axis_MotionApps20.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// I Only Changed this by applying all the configuration data and capturing it before startup: +// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it +static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// I Simplified this: +uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + setSleepEnabled(false); + + // get MPU hardware revision + setMemoryBank(0x10, true, true); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLN(readMemoryByte()); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1<= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetFIFORate(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleFrequency(); +// int32_t MPU6050_6Axis_MotionApps20::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050_6Axis_MotionApps20::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050_6Axis_MotionApps20::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050_6Axis_MotionApps20::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); + data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); + data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[32] << 8) | packet[33]; + data[2] = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) | packet[29]; + v -> y = (packet[32] << 8) | packet[33]; + v -> z = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[20] << 8) | packet[21]; + v -> z = (packet[24] << 8) | packet[25]; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + return status; +} + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050_6Axis_MotionApps20::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // unused parameter + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050_6Axis_MotionApps20::dmpInitFIFOParam(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpCloseFIFO(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050_6Axis_MotionApps20::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050_6Axis_MotionApps20::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050_6Axis_MotionApps20::dmpGetAccelSumOfSquare(); +// void MPU6050_6Axis_MotionApps20::dmpOverrideQuaternion(long *q); +uint16_t MPU6050_6Axis_MotionApps20::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + + + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof + return(GetCurrentFIFOPacket(data, dmpPacketSize)); +} diff --git a/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h index 0e699c47..0dc9379e 100644 --- a/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h +++ b/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h @@ -1,741 +1,153 @@ -// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation -// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 5/20/2013 by Jeff Rowberg -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -/* ============================================ -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_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 - -#include "MPU6050.h" - -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifndef __arm__ - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include - - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x - - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; - - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) - - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) - - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -/* Source is from the InvenSense MotionApps v2 demo code. Original source is - * unavailable, unless you happen to be amazing as decompiling binary by - * hand (in which case, please contact me, and I'm totally serious). - * - * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the - * DMP reverse-engineering he did to help make this bit of wizardry - * possible. - */ - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v2.0 42-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - // bank 0, 256 bytes - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - - // bank 1, 256 bytes - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - - // bank 2, 256 bytes - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, - 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, - 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, - 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, - 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, - 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, - 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, - 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, - 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, - 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, - 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, - 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, - 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - - // bank 4, 256 bytes - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - - // bank 5, 256 bytes - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, - 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, - 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, - 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, - 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, - 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, - 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, - 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, - - // bank 6, 256 bytes - 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, - 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, - 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, - 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, - 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, - 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, - 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, - 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, - 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, - 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, - 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, - 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, - 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, - 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, - 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, - 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, - - // bank 7, 138 bytes (remainder) - 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, - 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, - 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, - 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, - 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, - 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, - 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, - 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF -}; - -// thanks to Noah Zerkin for piecing this stuff together! -const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration - 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration - 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration - 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration - 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 - 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 - 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 - 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 - 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 - 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION - 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt - 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate - - // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, - // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. - // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) - - // It is important to make sure the host processor can keep up with reading and processing - // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. -}; - -const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xFF, - 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x01, 0x62, 0x02, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // enable sleep mode and wake cycle - /*Serial.println(F("Enabling sleep mode...")); - setSleepEnabled(true); - Serial.println(F("Enabling wake cycle...")); - setWakeCycleEnabled(true);*/ - - // disable sleep mode - DEBUG_PRINTLN(F("Disabling sleep mode...")); - setSleepEnabled(false); - - // get MPU hardware revision - DEBUG_PRINTLN(F("Selecting user bank 16...")); - setMemoryBank(0x10, true, true); - DEBUG_PRINTLN(F("Selecting memory byte 6...")); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - uint8_t hwRevision = readMemoryByte(); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - uint8_t otpValid = getOTPBankValid(); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); - - // get X/Y/Z gyro offsets - DEBUG_PRINTLN(F("Reading gyro offset TC values...")); - int8_t xgOffsetTC = getXGyroOffsetTC(); - int8_t ygOffsetTC = getYGyroOffsetTC(); - int8_t zgOffsetTC = getZGyroOffsetTC(); - DEBUG_PRINT(F("X gyro offset = ")); - DEBUG_PRINTLN(xgOffset); - DEBUG_PRINT(F("Y gyro offset = ")); - DEBUG_PRINTLN(ygOffset); - DEBUG_PRINT(F("Z gyro offset = ")); - DEBUG_PRINTLN(zgOffset); - - // setup weird slave stuff (?) - DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); - setSlaveAddress(0, 0x7F); - DEBUG_PRINTLN(F("Disabling I2C Master mode...")); - setI2CMasterModeEnabled(false); - DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); - setSlaveAddress(0, 0x68); - DEBUG_PRINTLN(F("Resetting I2C Master control...")); - resetI2CMaster(); - delay(20); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - // write DMP configuration - DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); - DEBUG_PRINTLN(F(" bytes in config def)")); - if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); - - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - setClockSource(MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(0x12); - - DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); - setRate(4); // 1khz / (1 + 4) = 200 Hz - - DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); - setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); - - DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); - setDLPFMode(MPU6050_DLPF_BW_42); - - DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); - setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - - DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); - setDMPConfig1(0x03); - setDMPConfig2(0x00); - - DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); - setOTPBankValid(false); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); - setXGyroOffsetTC(xgOffsetTC); - setYGyroOffsetTC(ygOffsetTC); - setZGyroOffsetTC(zgOffsetTC); - - //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); - //setXGyroOffset(0); - //setYGyroOffset(0); - //setZGyroOffset(0); - - DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); - uint8_t dmpUpdate[16], j; - uint16_t pos = 0; - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - resetFIFO(); - - DEBUG_PRINTLN(F("Reading FIFO count...")); - uint16_t fifoCount = getFIFOCount(); - uint8_t fifoBuffer[128]; - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); - setMotionDetectionThreshold(2); - - DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); - setZeroMotionDetectionThreshold(156); - - DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); - setMotionDetectionDuration(80); - - DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); - setZeroMotionDetectionDuration(0); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - resetFIFO(); - - DEBUG_PRINTLN(F("Enabling FIFO...")); - setFIFOEnabled(true); - - DEBUG_PRINTLN(F("Enabling DMP...")); - setDMPEnabled(true); - - DEBUG_PRINTLN(F("Resetting DMP...")); - resetDMP(); - - DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - DEBUG_PRINTLN(F("Reading FIFO data...")); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - uint8_t mpuIntStatus = getIntStatus(); - - DEBUG_PRINT(F("Current interrupt status=")); - DEBUG_PRINTLNF(mpuIntStatus, HEX); - - DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - - DEBUG_PRINTLN(F("Reading FIFO data...")); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - mpuIntStatus = getIntStatus(); - - DEBUG_PRINT(F("Current interrupt status=")); - DEBUG_PRINTLNF(mpuIntStatus, HEX); - - DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("DMP is good to go! Finally.")); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); - dmpPacketSize = 42; - /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { - return 3; // TODO: proper error code for no memory - }*/ - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - } else { - DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); - return 2; // configuration block loading failed - } - } else { - DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); - return 1; // main binary block loading failed - } - return 0; // success -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); - data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); - data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) + packet[29]; - data[1] = (packet[32] << 8) + packet[33]; - data[2] = (packet[36] << 8) + packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) + packet[29]; - v -> y = (packet[32] << 8) + packet[33]; - v -> z = (packet[36] << 8) + packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); - data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); - data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); - data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) + packet[1]); - data[1] = ((packet[4] << 8) + packet[5]); - data[2] = ((packet[8] << 8) + packet[9]); - data[3] = ((packet[12] << 8) + packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); - data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); - data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) + packet[17]; - data[1] = (packet[20] << 8) + packet[21]; - data[2] = (packet[24] << 8) + packet[25]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi - return 0; -} -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) *processed++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ +// 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) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// 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_6AXIS_MOTIONAPPS20_H_ +#define _MPU6050_6AXIS_MOTIONAPPS20_H_ + +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF + +#include "MPU6050.h" + +class MPU6050_6Axis_MotionApps20 : public MPU6050_Base { + public: + MPU6050_6Axis_MotionApps20(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; +}; + +typedef MPU6050_6Axis_MotionApps20 MPU6050; + +#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.cpp b/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.cpp new file mode 100644 index 00000000..2cd11134 --- /dev/null +++ b/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.cpp @@ -0,0 +1,617 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 6.12 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// 2019/07/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. +// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present +// - Run-time calibration routine is enabled which calibrates after no motion state is detected +// - once no motion state is detected Calibration completes within 0.5 seconds +// - The Drawback is that the firmware image is larger. +// ... - ongoing debug release + +/* ============================================ +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. +=============================================== +*/ + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS612 + +#include "MPU6050_6Axis_MotionApps612.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 3062 // dmpMemory[] + +/* ================================================================ * + | Default MotionApps v6.12 28-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 | + | | + | [GYRO X][GYRO Y][GYRO Z][ACC X ][ACC Y ][ACC Z ] | + | 16 17 18 19 20 21 22 23 24 25 26 27 | + * ================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made so we can just load it +const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { +/* bank # 0 */ +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00, +0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, +0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02, +0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, +0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1, +0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2, +0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, +0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, +0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76, +0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82, +0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21, +0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06, +0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34, +0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE, +/* bank # 1 */ +0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00, +0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00, +0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00, +0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83, +0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28, +0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00, +/* bank # 2 */ +0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, +0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, +0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00, +0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, +0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E, +0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C, +0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64, +0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 3 */ +0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3, +0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D, +0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00, +0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19, +0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D, +0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88, +0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, +/* bank # 4 */ +0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E, +0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, +0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9, +0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96, +0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83, +0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1, +0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2, +0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA, +0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF, +0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8, +0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, +0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35, +0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, +0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2, +0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55, +0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78, +/* bank # 5 */ +0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA, +0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8, +0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88, +0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7, +0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1, +0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1, +0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29, +0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A, +0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7, +0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E, +0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C, +0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8, +0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88, +0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94, +0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2, +0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82, +/* bank # 6 */ +0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1, +0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98, +0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88, +0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02, +0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF, +0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1, +0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39, +0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9, +0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9, +0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE, +0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0, +0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8, +0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2, +0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3, +0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB, +0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8, +/* bank # 7 */ +0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB, +0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28, +0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2, +0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A, +0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB, +0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6, +0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8, +0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1, +0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99, +0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB, +0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9, +0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9, +0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC, +0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6, +0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0, +0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF, +/* bank # 8 */ +0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30, +0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9, +0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0, +0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1, +0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6, +0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, +0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA, +0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE, +0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE, +0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88, +0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF, +0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8, +0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5, +0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8, +0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7, +0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8, +/* bank # 9 */ +0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, +0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B, +0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB, +0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0, +0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0, +0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59, +0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31, +0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28, +0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0, +0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9, +0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C, +0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E, +0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0, +0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, +0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2, +0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83, +/* bank # 10 */ +0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6, +0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, +0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB, +0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C, +0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8, +0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9, +0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2, +0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84, +0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, +0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, +0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, +0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, //Reverted back as packet size changes causing isues... TODO:change 2742 from 0xD8 to 0x20 Including the DMP_FEATURE_TAP -- known issue in which if you do not enable DMP_FEATURE_TAP then the interrupts will be at 200Hz even if fifo rate +0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, +0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, +0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, +0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29, +/* bank # 11 */ +0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83, +0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19, +0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39, +0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7, +0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80, +0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9, +0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26, +0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89, +0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, +0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, +0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22, +0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2, +0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00, +0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10, +0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88, +0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF, +}; + +// this divisor is pre configured into the above image and can't be modified at this time. +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done. +// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions" +uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely + uint8_t val; + uint16_t ival; + // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41 + I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1), wireObj); //PWR_MGMT_1: reset with 100ms delay + delay(100); + I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111), wireObj); // full SIGNAL_PATH_RESET: with another 100ms delay + delay(100); + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01), wireObj); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00), wireObj); // 0000 0000 INT_ENABLE: no Interrupt + I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00), wireObj); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead + I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00), wireObj); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g + I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80), wireObj); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01), wireObj); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04), wireObj); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01), wireObj); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat + if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail + I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400), wireObj); // DMP Program Start Address + I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18), wireObj); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec + I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0), wireObj); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02), wireObj); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on + I2Cdev::writeBit(devAddr,0x6A, 2, 1, wireObj); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) + + setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library +/* + dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL + dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO +*/ + dmpPacketSize = 28; + return 0; +} + +bool MPU6050::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050::dmpGetFIFORate(); +// uint8_t MPU6050::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050::dmpGetSampleFrequency(); +// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 8) | packet[17]); + data[1] = (((uint32_t)packet[18] << 8) | packet[19]); + data[2] = (((uint32_t)packet[20] << 8) | packet[21]); + return 0; +} +uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[18] << 8) | packet[19]; + data[2] = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[18] << 8) | packet[19]; + v -> z = (packet[20] << 8) | packet[21]; + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[22] << 8) | packet[23]); + data[1] = (((uint32_t)packet[24] << 8) | packet[25]); + data[2] = (((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[22] << 8) | packet[23]; + data[1] = (packet[24] << 8) | packet[25]; + data[2] = (packet[26] << 8) | packet[27]; + return 0; +} +uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[22] << 8) | packet[23]; + v -> y = (packet[24] << 8) | packet[25]; + v -> z = (packet[26] << 8) | packet[27]; + return 0; +} +// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +16384 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*16384; + v -> y = vRaw -> y - gravity -> y*16384; + v -> z = vRaw -> z - gravity -> z*16384; + return 0; +} +// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +16384, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + return status; +} + +uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // unused parameter + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050::dmpInitFIFOParam(); +// uint8_t MPU6050::dmpCloseFIFO(); +// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050::dmpGetAccelSumOfSquare(); +// void MPU6050::dmpOverrideQuaternion(long *q); +uint16_t MPU6050::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + + + +uint8_t MPU6050::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof + return(GetCurrentFIFOPacket(data, dmpPacketSize)); +} diff --git a/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.h b/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.h new file mode 100644 index 00000000..56e3bb35 --- /dev/null +++ b/Arduino/MPU6050/MPU6050_6Axis_MotionApps612.h @@ -0,0 +1,153 @@ +// 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) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// 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_6AXIS_MOTIONAPPS612_H_ +#define _MPU6050_6AXIS_MOTIONAPPS612_H_ + +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF + +#include "MPU6050.h" + +class MPU6050_6Axis_MotionApps612 : public MPU6050_Base { + public: + MPU6050_6Axis_MotionApps612(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } + + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; +}; + +typedef MPU6050_6Axis_MotionApps612 MPU6050; + +#endif /* _MPU6050_6AXIS_MOTIONAPPS612_H_ */ diff --git a/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.cpp b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.cpp new file mode 100644 index 00000000..d85d74a5 --- /dev/null +++ b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.cpp @@ -0,0 +1,887 @@ +// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 6/18/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2021 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. +=============================================== +*/ + +// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +#include "MPU6050_9Axis_MotionApps41.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + //typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v4.1 48-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) +static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, + 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, + 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, + 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, + 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, + 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, + 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, + 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, + 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, + 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, + 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, + 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, + 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, + 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, + 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, + 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, + + // bank 4, 256 bytes + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + + // bank 5, 256 bytes + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, + 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, + 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, + 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, + 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, + 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, + + // bank 6, 256 bytes + 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, + 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, + 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, + 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, + 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, + 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, + 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, + 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, + 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, + 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, + 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, + 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, + 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, + 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, + 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, + 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, + + // bank 7, 170 bytes (remainder) + 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, + 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, + 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, + 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, + 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, + 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, + 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, + 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, + 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, + 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 +#endif + +static const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? + 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration + 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration + + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 + + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x00, 0xA3, 0x01, 0x00, // ? + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? + 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION + 0x07, 0xA7, 0x01, 0xFE, // ? + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x07, 0x67, 0x01, 0x9A, // ? + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate + + // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xF5, + 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, + 0x00, 0xA3, 0x01, 0x00, + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x08, 0x02, 0x01, 0x20, + 0x01, 0x0A, 0x02, 0x00, 0x4E, + 0x01, 0x02, 0x02, 0xFE, 0xB3, + 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ + 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, + 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + +uint8_t MPU6050_9Axis_MotionApps41::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // disable sleep mode + DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); + + // get MPU product ID + DEBUG_PRINTLN(F("Getting product ID...")); + //uint8_t productID = 0; //getProductID(); + DEBUG_PRINT(F("Product ID = ")); + DEBUG_PRINT(productID); + + // get MPU hardware revision + DEBUG_PRINTLN(F("Selecting user bank 16...")); + setMemoryBank(0x10, true, true); + DEBUG_PRINTLN(F("Selecting memory byte 6...")); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + uint8_t hwRevision = readMemoryByte(); + (void)hwRevision; // suppress unused variable compile warning + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLNF(hwRevision, HEX); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + uint8_t otpValid = getOTPBankValid(); + (void)otpValid; // suppress unused variable compile warning + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + DEBUG_PRINTLN(F("Reading gyro offset values...")); + int8_t xgOffset = getXGyroOffset(); + int8_t ygOffset = getYGyroOffset(); + int8_t zgOffset = getZGyroOffset(); + DEBUG_PRINT(F("X gyro offset = ")); + DEBUG_PRINTLN(xgOffset); + DEBUG_PRINT(F("Y gyro offset = ")); + DEBUG_PRINTLN(ygOffset); + DEBUG_PRINT(F("Z gyro offset = ")); + DEBUG_PRINTLN(zgOffset); + + I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer, I2Cdev::readTimeout, wireObj); // ? + + DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32, wireObj); + + // enable MPU AUX I2C bypass mode + //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); + //setI2CBypassEnabled(true); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00, wireObj); + + DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); + //mag -> setMode(0x0F); + I2Cdev::writeByte(0x0E, 0x0A, 0x0F, wireObj); + + DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); + int8_t asax, asay, asaz; + //mag -> getAdjustment(&asax, &asay, &asaz); + I2Cdev::readBytes(0x0E, 0x10, 3, buffer, I2Cdev::readTimeout, wireObj); + asax = (int8_t)buffer[0]; + asay = (int8_t)buffer[1]; + asaz = (int8_t)buffer[2]; + (void)asax; // suppress unused variable compiler warning + (void)asay; // suppress unused variable compiler warning + (void)asaz; // suppress unused variable compiler warning + DEBUG_PRINT(F("Adjustment X/Y/Z = ")); + DEBUG_PRINT(asax); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINT(asay); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINTLN(asaz); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00, wireObj); + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + DEBUG_PRINTLN(F("Configuring DMP and related settings...")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1< setMode(1); + I2Cdev::writeByte(0x0E, 0x0A, 0x01, wireObj); + + // setup AK8975 (0x0E) as Slave 0 in read mode + DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA, wireObj); + + // setup AK8975 (0x0E) as Slave 2 in write mode + DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01, wireObj); + + // setup I2C timing/delay control + DEBUG_PRINTLN(F("Setting up slave access delay...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05, wireObj); + + // enable interrupts + DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); + I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00, wireObj); + + // enable I2C master mode and reset DMP/FIFO + DEBUG_PRINTLN(F("Enabling I2C master mode...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20, wireObj); + DEBUG_PRINTLN(F("Resetting FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24, wireObj); + DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20, wireObj); + DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8, wireObj); + + DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + #ifdef DEBUG + DEBUG_PRINT(F("Read bytes: ")); + for (j = 0; j < 4; j++) { + DEBUG_PRINTF(dmpUpdate[3 + j], HEX); + DEBUG_PRINT(" "); + } + DEBUG_PRINTLN(""); + #endif + + DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); + while ((fifoCount = getFIFOCount()) < 46); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); + dmpPacketSize = 48; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} + +bool MPU6050_9Axis_MotionApps41::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetFIFORate(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetSampleFrequency(); +// int32_t MPU6050_9Axis_MotionApps41::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050_9Axis_MotionApps41::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050_9Axis_MotionApps41::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050_9Axis_MotionApps41::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); + data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); + data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[34] << 8) | packet[35]; + data[1] = (packet[38] << 8) | packet[39]; + data[2] = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[34] << 8) | packet[35]; + v -> y = (packet[38] << 8) | packet[39]; + v -> z = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetMag(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[30] << 8) | packet[31]; + data[2] = (packet[32] << 8) | packet[33]; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) + v -> x = vRaw -> x - gravity -> x*4096; + v -> y = vRaw -> y - gravity -> y*4096; + v -> z = vRaw -> z - gravity -> z*4096; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + return status; +} + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetEuler(float *data, Quaternion *q) { + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050_9Axis_MotionApps41::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050_9Axis_MotionApps41::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if(gravity->z<0) { + if(data[1]>0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050_9Axis_MotionApps41::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // suppress unused variable compiler warning + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050_9Axis_MotionApps41::dmpInitFIFOParam(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpCloseFIFO(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050_9Axis_MotionApps41::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050_9Axis_MotionApps41::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050_9Axis_MotionApps41::dmpGetAccelSumOfSquare(); +// void MPU6050_9Axis_MotionApps41::dmpOverrideQuaternion(long *q); +uint16_t MPU6050_9Axis_MotionApps41::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} diff --git a/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h index 14e280c0..6d3576e9 100644 --- a/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h +++ b/Arduino/MPU6050/MPU6050_9Axis_MotionApps41.h @@ -1,11 +1,16 @@ -// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// 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) -// 6/18/2012 by Jeff Rowberg +// 10/3/2011 by Jeff Rowberg // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2021/09/27 - split implementations out of header files, finally // ... - ongoing debug release +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// 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 @@ -30,823 +35,119 @@ THE SOFTWARE. =============================================== */ -#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ -#define _MPU6050_9AXIS_MOTIONAPPS41_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" +#ifndef _MPU6050_6AXIS_MOTIONAPPS41_H_ +#define _MPU6050_6AXIS_MOTIONAPPS41_H_ -// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF #include "MPU6050.h" -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifndef __arm__ - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include +class MPU6050_9Axis_MotionApps41 : public MPU6050_Base { + public: + MPU6050_9Axis_MotionApps41(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) + // 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(); - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) + // 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); - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v4.1 48-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) -const prog_uchar dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - // bank 0, 256 bytes - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - - // bank 1, 256 bytes - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - - // bank 2, 256 bytes - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, - 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, - 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, - 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, - 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, - 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, - 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, - 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, - 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, - 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, - 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, - 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, - 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, - 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, - 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, - 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, - - // bank 4, 256 bytes - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - - // bank 5, 256 bytes - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, - 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, - 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, - 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, - 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, - 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, - - // bank 6, 256 bytes - 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, - 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, - 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, - 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, - 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, - 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, - 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, - 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, - 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, - 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, - 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, - 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, - 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, - 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, - 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, - 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, - - // bank 7, 170 bytes (remainder) - 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, - 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, - 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, - 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, - 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, - 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, - 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, - 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, - 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, - 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF -}; - -const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? - 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration - 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration - - 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration - 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 - 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 - 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 - 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 - 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 - 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 - - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x00, 0xA3, 0x01, 0x00, // ? - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? - 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION - 0x07, 0xA7, 0x01, 0xFE, // ? - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x07, 0x67, 0x01, 0x9A, // ? - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate - - // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, - // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. - // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) - - // It is important to make sure the host processor can keep up with reading and processing - // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. + 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(); + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; }; -const prog_uchar dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xF5, - 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, - 0x00, 0xA3, 0x01, 0x00, - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x01, 0x08, 0x02, 0x01, 0x20, - 0x01, 0x0A, 0x02, 0x00, 0x4E, - 0x01, 0x02, 0x02, 0xFE, 0xB3, - 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ - 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, - 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // disable sleep mode - DEBUG_PRINTLN(F("Disabling sleep mode...")); - setSleepEnabled(false); - - // get MPU product ID - DEBUG_PRINTLN(F("Getting product ID...")); - //uint8_t productID = 0; //getProductID(); - DEBUG_PRINT(F("Product ID = ")); - DEBUG_PRINT(productID); - - // get MPU hardware revision - DEBUG_PRINTLN(F("Selecting user bank 16...")); - setMemoryBank(0x10, true, true); - DEBUG_PRINTLN(F("Selecting memory byte 6...")); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - uint8_t hwRevision = readMemoryByte(); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - uint8_t otpValid = getOTPBankValid(); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); - - // get X/Y/Z gyro offsets - DEBUG_PRINTLN(F("Reading gyro offset values...")); - int8_t xgOffset = getXGyroOffset(); - int8_t ygOffset = getYGyroOffset(); - int8_t zgOffset = getZGyroOffset(); - DEBUG_PRINT(F("X gyro offset = ")); - DEBUG_PRINTLN(xgOffset); - DEBUG_PRINT(F("Y gyro offset = ")); - DEBUG_PRINTLN(ygOffset); - DEBUG_PRINT(F("Z gyro offset = ")); - DEBUG_PRINTLN(zgOffset); - - I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? - - DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); - - // enable MPU AUX I2C bypass mode - //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); - //setI2CBypassEnabled(true); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); - //mag -> setMode(0x0F); - I2Cdev::writeByte(0x0E, 0x0A, 0x0F); - - DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); - int8_t asax, asay, asaz; - //mag -> getAdjustment(&asax, &asay, &asaz); - I2Cdev::readBytes(0x0E, 0x10, 3, buffer); - asax = (int8_t)buffer[0]; - asay = (int8_t)buffer[1]; - asaz = (int8_t)buffer[2]; - DEBUG_PRINT(F("Adjustment X/Y/Z = ")); - DEBUG_PRINT(asax); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINT(asay); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINTLN(asaz); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - DEBUG_PRINTLN(F("Configuring DMP and related settings...")); - - // write DMP configuration - DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); - DEBUG_PRINTLN(F(" bytes in config def)")); - if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(0x12); - - DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); - setRate(4); // 1khz / (1 + 4) = 200 Hz - - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - setClockSource(MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); - setDLPFMode(MPU6050_DLPF_BW_42); - - DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); - setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); - - DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); - setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - - DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); - setDMPConfig1(0x03); - setDMPConfig2(0x00); - - DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); - setOTPBankValid(false); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values...")); - setXGyroOffset(xgOffset); - setYGyroOffset(ygOffset); - setZGyroOffset(zgOffset); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); - setXGyroOffsetUser(0); - setYGyroOffsetUser(0); - setZGyroOffsetUser(0); - - DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)...")); - uint8_t dmpUpdate[16], j; - uint16_t pos = 0; - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - resetFIFO(); - - DEBUG_PRINTLN(F("Reading FIFO count...")); - uint8_t fifoCount = getFIFOCount(); - - DEBUG_PRINT(F("Current FIFO count=")); - DEBUG_PRINTLN(fifoCount); - uint8_t fifoBuffer[128]; - //getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Disabling all standby flags...")); - I2Cdev::writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00); - - DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g...")); - I2Cdev::writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00); - - DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); - setMotionDetectionThreshold(2); - - DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); - setZeroMotionDetectionThreshold(156); - - DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); - setMotionDetectionDuration(80); - - DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); - setZeroMotionDetectionDuration(0); - - DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode...")); - //mag -> setMode(1); - I2Cdev::writeByte(0x0E, 0x0A, 0x01); - - // setup AK8975 (0x0E) as Slave 0 in read mode - DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); - - // setup AK8975 (0x0E) as Slave 2 in write mode - DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); - - // setup I2C timing/delay control - DEBUG_PRINTLN(F("Setting up slave access delay...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); - - // enable interrupts - DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); - I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); - - // enable I2C master mode and reset DMP/FIFO - DEBUG_PRINTLN(F("Enabling I2C master mode...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Resetting FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); - DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); - - DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - #ifdef DEBUG - DEBUG_PRINT(F("Read bytes: ")); - for (j = 0; j < 4; j++) { - DEBUG_PRINTF(dmpUpdate[3 + j], HEX); - DEBUG_PRINT(" "); - } - DEBUG_PRINTLN(""); - #endif - - DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); - while ((fifoCount = getFIFOCount()) < 46); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); - dmpPacketSize = 48; - /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { - return 3; // TODO: proper error code for no memory - }*/ - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - } else { - DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); - return 2; // configuration block loading failed - } - } else { - DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); - return 1; // main binary block loading failed - } - return 0; // success -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]); - data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]); - data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[34] << 8) + packet[35]; - data[1] = (packet[38] << 8) + packet[39]; - data[2] = (packet[42] << 8) + packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[34] << 8) + packet[35]; - v -> y = (packet[38] << 8) + packet[39]; - v -> z = (packet[42] << 8) + packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); - data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); - data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); - data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) + packet[1]); - data[1] = ((packet[4] << 8) + packet[5]); - data[2] = ((packet[8] << 8) + packet[9]); - data[3] = ((packet[12] << 8) + packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); - data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); - data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) + packet[17]; - data[1] = (packet[20] << 8) + packet[21]; - data[2] = (packet[24] << 8) + packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) + packet[29]; - data[1] = (packet[30] << 8) + packet[31]; - data[2] = (packet[32] << 8) + packet[33]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) - v -> x = vRaw -> x - gravity -> x*4096; - v -> y = vRaw -> y - gravity -> y*4096; - v -> z = vRaw -> z - gravity -> z*4096; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi - return 0; -} -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) *processed++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} +typedef MPU6050_9Axis_MotionApps41 MPU6050; -#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ +#endif /* _MPU6050_6AXIS_MOTIONAPPS41_H_ */ diff --git a/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino b/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino new file mode 100644 index 00000000..a9f46ea9 --- /dev/null +++ b/Arduino/MPU6050/examples/IMU_Zero/IMU_Zero.ino @@ -0,0 +1,358 @@ +// MPU6050 offset-finder, based on Jeff Rowberg's MPU6050_RAW +// 2016-10-19 by Robert R. Fenichel (bob@fenichel.net) + +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class +// 10/7/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-11 - added PID offset generation at begninning Generates first offsets +// - in @ 6 seconds and completes with 4 more sets @ 10 seconds +// - then continues with origional 2016 calibration code. +// 2016-11-25 - added delays to reduce sampling rate to ~200 Hz +// added temporizing printing during long computations +// 2016-10-25 - requires inequality (Low < Target, High > Target) during expansion +// dynamic speed change when closing in +// 2016-10-22 - cosmetic changes +// 2016-10-19 - initial release of IMU_Zero +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - initial release of MPU6050_RAW + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2011 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. + + If an MPU6050 + * is an ideal member of its tribe, + * is properly warmed up, + * is at rest in a neutral position, + * is in a location where the pull of gravity is exactly 1g, and + * has been loaded with the best possible offsets, +then it will report 0 for all accelerations and displacements, except for +Z acceleration, for which it will report 16384 (that is, 2^14). Your device +probably won't do quite this well, but good offsets will all get the baseline +outputs close to these target values. + + Put the MPU6050 on a flat and horizontal surface, and leave it operating for +5-10 minutes so its temperature gets stabilized. + + Run this program. A "----- done -----" line will indicate that it has done its best. +With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take +a few minutes to get there. + + Along the way, it will generate a dozen or so lines of output, showing that for each +of the 6 desired offsets, it is + * first, trying to find two estimates, one too low and one too high, and + * then, closing in until the bracket can't be made smaller. + + The line just above the "done" line will look something like + [567,567] --> [-1,2] [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1] [-25,-24] --> [0,3] [5,6] --> [0,4] +As will have been shown in interspersed header lines, the six groups making up this +line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration, +X gyro, Y gyro, and Z gyro, respectively. In the sample shown just above, the trial showed +that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, +and so on. + + The need for the delay between readings (usDelay) was brought to my attention by Nikolaus Doppelhammer. +=============================================== +*/ + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" +#include "MPU6050.h" + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 accelgyro; +//MPU6050 accelgyro(0x69); // <-- use for AD0 high + + +const char LBRACKET = '['; +const char RBRACKET = ']'; +const char COMMA = ','; +const char BLANK = ' '; +const char PERIOD = '.'; + +const int iAx = 0; +const int iAy = 1; +const int iAz = 2; +const int iGx = 3; +const int iGy = 4; +const int iGz = 5; + +const int usDelay = 3150; // empirical, to hold sampling to 200 Hz +const int NFast = 1000; // the bigger, the better (but slower) +const int NSlow = 10000; // .. +const int LinesBetweenHeaders = 5; + int LowValue[6]; + int HighValue[6]; + int Smoothed[6]; + int LowOffset[6]; + int HighOffset[6]; + int Target[6]; + int LinesOut; + int N; + +void ForceHeader() + { LinesOut = 99; } + +void GetSmoothed() + { int16_t RawValue[6]; + int i; + long Sums[6]; + for (i = iAx; i <= iGz; i++) + { Sums[i] = 0; } +// unsigned long Start = micros(); + + for (i = 1; i <= N; i++) + { // get sums + accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], + &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]); + if ((i % 500) == 0) + Serial.print(PERIOD); + delayMicroseconds(usDelay); + for (int j = iAx; j <= iGz; j++) + Sums[j] = Sums[j] + RawValue[j]; + } // get sums +// unsigned long usForN = micros() - Start; +// Serial.print(" reading at "); +// Serial.print(1000000/((usForN+N/2)/N)); +// Serial.println(" Hz"); + for (i = iAx; i <= iGz; i++) + { Smoothed[i] = (Sums[i] + N/2) / N ; } + } // GetSmoothed + +void Initialize() + { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + Serial.begin(9600); + + // initialize device + Serial.println("Initializing I2C devices..."); + accelgyro.initialize(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + Serial.println("PID tuning Each Dot = 100 readings"); + /*A tidbit on how PID (PI actually) tuning works. + When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and + integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral + uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it + to the integral value. Each reading narrows the error down to the desired offset. The greater the error from + set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the + integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the + noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 + readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to + the fact it reacts to any noise. + */ + accelgyro.CalibrateAccel(6); + accelgyro.CalibrateGyro(6); + Serial.println("\nat 600 Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("700 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("800 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("900 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println(); + accelgyro.CalibrateAccel(1); + accelgyro.CalibrateGyro(1); + Serial.println("1000 Total Readings"); + accelgyro.PrintActiveOffsets(); + Serial.println("\n\n Any of the above offsets will work nice \n\n Lets proof the PID tuning using another method:"); + } // Initialize + +void SetOffsets(int TheOffsets[6]) + { accelgyro.setXAccelOffset(TheOffsets [iAx]); + accelgyro.setYAccelOffset(TheOffsets [iAy]); + accelgyro.setZAccelOffset(TheOffsets [iAz]); + accelgyro.setXGyroOffset (TheOffsets [iGx]); + accelgyro.setYGyroOffset (TheOffsets [iGy]); + accelgyro.setZGyroOffset (TheOffsets [iGz]); + } // SetOffsets + +void ShowProgress() + { if (LinesOut >= LinesBetweenHeaders) + { // show header + Serial.println("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro"); + LinesOut = 0; + } // show header + Serial.print(BLANK); + for (int i = iAx; i <= iGz; i++) + { Serial.print(LBRACKET); + Serial.print(LowOffset[i]), + Serial.print(COMMA); + Serial.print(HighOffset[i]); + Serial.print("] --> ["); + Serial.print(LowValue[i]); + Serial.print(COMMA); + Serial.print(HighValue[i]); + if (i == iGz) + { Serial.println(RBRACKET); } + else + { Serial.print("]\t"); } + } + LinesOut++; + } // ShowProgress + +void PullBracketsIn() + { boolean AllBracketsNarrow; + boolean StillWorking; + int NewOffset[6]; + + Serial.println("\nclosing in:"); + AllBracketsNarrow = false; + ForceHeader(); + StillWorking = true; + while (StillWorking) + { StillWorking = false; + if (AllBracketsNarrow && (N == NFast)) + { SetAveraging(NSlow); } + else + { AllBracketsNarrow = true; }// tentative + for (int i = iAx; i <= iGz; i++) + { if (HighOffset[i] <= (LowOffset[i]+1)) + { NewOffset[i] = LowOffset[i]; } + else + { // binary search + StillWorking = true; + NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2; + if (HighOffset[i] > (LowOffset[i] + 10)) + { AllBracketsNarrow = false; } + } // binary search + } + SetOffsets(NewOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // closing in + if (Smoothed[i] > Target[i]) + { // use lower half + HighOffset[i] = NewOffset[i]; + HighValue[i] = Smoothed[i]; + } // use lower half + else + { // use upper half + LowOffset[i] = NewOffset[i]; + LowValue[i] = Smoothed[i]; + } // use upper half + } // closing in + ShowProgress(); + } // still working + + } // PullBracketsIn + +void PullBracketsOut() + { boolean Done = false; + int NextLowOffset[6]; + int NextHighOffset[6]; + + Serial.println("expanding:"); + ForceHeader(); + + while (!Done) + { Done = true; + SetOffsets(LowOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got low values + LowValue[i] = Smoothed[i]; + if (LowValue[i] >= Target[i]) + { Done = false; + NextLowOffset[i] = LowOffset[i] - 1000; + } + else + { NextLowOffset[i] = LowOffset[i]; } + } // got low values + + SetOffsets(HighOffset); + GetSmoothed(); + for (int i = iAx; i <= iGz; i++) + { // got high values + HighValue[i] = Smoothed[i]; + if (HighValue[i] <= Target[i]) + { Done = false; + NextHighOffset[i] = HighOffset[i] + 1000; + } + else + { NextHighOffset[i] = HighOffset[i]; } + } // got high values + ShowProgress(); + for (int i = iAx; i <= iGz; i++) + { LowOffset[i] = NextLowOffset[i]; // had to wait until ShowProgress done + HighOffset[i] = NextHighOffset[i]; // .. + } + } // keep going + } // PullBracketsOut + +void SetAveraging(int NewN) + { N = NewN; + Serial.print("averaging "); + Serial.print(N); + Serial.println(" readings each time"); + } // SetAveraging + +void setup() + { Initialize(); + for (int i = iAx; i <= iGz; i++) + { // set targets and initial guesses + Target[i] = 0; // must fix for ZAccel + HighOffset[i] = 0; + LowOffset[i] = 0; + } // set targets and initial guesses + Target[iAz] = 16384; + SetAveraging(NFast); + + PullBracketsOut(); + PullBracketsIn(); + + Serial.println("-------------- done --------------"); + } // setup + +void loop() + { + } // loop diff --git a/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino b/Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino similarity index 89% rename from Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino rename to Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino index be24ffd5..3ebe2ee6 100644 --- a/Arduino/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino +++ b/Arduino/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino @@ -3,6 +3,9 @@ // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2019-07-08 - Added Auto Calibration and offset generator +// - and altered FIFO retrieval sequence to avoid using blocking code +// 2016-04-18 - Eliminated a potential infinite loop // 2013-05-08 - added seamless Fastwire support // - added note about gyro calibration // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error @@ -118,6 +121,7 @@ MPU6050 mpu; +#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) bool blinkState = false; @@ -162,7 +166,7 @@ void setup() { // join I2C bus (I2Cdev library doesn't do this automatically) #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); - TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif @@ -173,8 +177,8 @@ void setup() { Serial.begin(115200); while (!Serial); // wait for Leonardo enumeration, others continue immediately - // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio - // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino + // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to // the baud timing being too misaligned with processor ticks. You must use // 38400 or slower in these cases, or use some kind of external separate // crystal solution for the UART timer. @@ -182,6 +186,7 @@ void setup() { // initialize device Serial.println(F("Initializing I2C devices...")); mpu.initialize(); + pinMode(INTERRUPT_PIN, INPUT); // verify connection Serial.println(F("Testing device connections...")); @@ -205,13 +210,19 @@ void setup() { // make sure it worked (returns 0 if so) if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + mpu.PrintActiveOffsets(); // turn on the DMP, now that it's ready Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); // enable Arduino interrupt detection - Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); - attachInterrupt(0, dmpDataReady, RISING); + Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); + Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); + Serial.println(F(")...")); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); // set our DMP Ready flag so the main loop() function knows it's okay to use it @@ -243,46 +254,8 @@ void setup() { void loop() { // if programming failed, don't try to do anything if (!dmpReady) return; - - // wait for MPU interrupt or extra packet(s) available - while (!mpuInterrupt && fifoCount < packetSize) { - // other program behavior stuff here - // . - // . - // . - // if you are really paranoid you can frequently test in between other - // stuff to see if mpuInterrupt is true, and if so, "break;" from the - // while() loop to immediately process the MPU data - // . - // . - // . - } - - // reset interrupt flag and get INT_STATUS byte - mpuInterrupt = false; - mpuIntStatus = mpu.getIntStatus(); - - // get current FIFO count - fifoCount = mpu.getFIFOCount(); - - // check for overflow (this should never happen unless our code is too inefficient) - if ((mpuIntStatus & 0x10) || fifoCount == 1024) { - // reset so we can continue cleanly - mpu.resetFIFO(); - Serial.println(F("FIFO overflow!")); - - // otherwise, check for DMP data ready interrupt (this should happen frequently) - } else if (mpuIntStatus & 0x02) { - // wait for correct available data length, should be a VERY short wait - while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); - - // read a packet from FIFO - mpu.getFIFOBytes(fifoBuffer, packetSize); - - // track FIFO count here in case there is > 1 packet available - // (this lets us immediately read more without waiting for an interrupt) - fifoCount -= packetSize; - + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet #ifdef OUTPUT_READABLE_QUATERNION // display quaternion values in easy matrix form: w x y z mpu.dmpGetQuaternion(&q, fifoBuffer); diff --git a/Arduino/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot.pde b/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde similarity index 67% rename from Arduino/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot.pde rename to Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde index d4c32daf..130fc4dc 100644 --- a/Arduino/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot.pde +++ b/Arduino/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde @@ -45,7 +45,7 @@ ToxiclibsSupport gfx; Serial port; // The serial port char[] teapotPacket = new char[14]; // InvenSense Teapot packet int serialCount = 0; // current packet byte position -int aligned = 0; +int synced = 0; int interval = 0; float[] q = new float[4]; @@ -145,62 +145,57 @@ void serialEvent(Serial port) { interval = millis(); while (port.available() > 0) { int ch = port.read(); - print((char)ch); - if (ch == '$') {serialCount = 0;} // this will help with alignment - if (aligned < 4) { - // make sure we are properly aligned on a 14-byte packet - if (serialCount == 0) { - if (ch == '$') aligned++; else aligned = 0; - } else if (serialCount == 1) { - if (ch == 2) aligned++; else aligned = 0; - } else if (serialCount == 12) { - if (ch == '\r') aligned++; else aligned = 0; - } else if (serialCount == 13) { - if (ch == '\n') aligned++; else aligned = 0; - } - //println(ch + " " + aligned + " " + serialCount); - serialCount++; - if (serialCount == 14) serialCount = 0; - } else { - if (serialCount > 0 || ch == '$') { - teapotPacket[serialCount++] = (char)ch; - if (serialCount == 14) { - serialCount = 0; // restart packet byte position - - // get quaternion from data packet - q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; - q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; - q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; - q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; - for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; - - // set our toxilibs quaternion to new data - quat.set(q[0], q[1], q[2], q[3]); - - /* - // below calculations unnecessary for orientation only using toxilibs - - // calculate gravity vector - gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); - gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); - gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; - - // calculate Euler angles - euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); - euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); - euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); - - // calculate yaw/pitch/roll angles - ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); - ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); - ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); - - // output various components for debugging - //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); - //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); - //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); - */ - } + + if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed + synced = 1; + print ((char)ch); + + if ((serialCount == 1 && ch != 2) + || (serialCount == 12 && ch != '\r') + || (serialCount == 13 && ch != '\n')) { + serialCount = 0; + synced = 0; + return; + } + + if (serialCount > 0 || ch == '$') { + teapotPacket[serialCount++] = (char)ch; + if (serialCount == 14) { + serialCount = 0; // restart packet byte position + + // get quaternion from data packet + q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; + q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; + q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; + q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; + for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; + + // set our toxilibs quaternion to new data + quat.set(q[0], q[1], q[2], q[3]); + + /* + // below calculations unnecessary for orientation only using toxilibs + + // calculate gravity vector + gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); + gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); + gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; + + // calculate Euler angles + euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); + euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); + euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); + + // calculate yaw/pitch/roll angles + ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); + ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); + ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); + + // output various components for debugging + //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); + //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); + //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); + */ } } } diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/MPU6050_DMP6_ESPWiFi.ino b/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/MPU6050_DMP6_ESPWiFi.ino new file mode 100644 index 00000000..a080869c --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/MPU6050_DMP6_ESPWiFi.ino @@ -0,0 +1,373 @@ +/* ============================================ +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. +=============================================== +*/ + +/* This driver reads quaternion data from the MPU6060 and sends + Open Sound Control messages. + + GY-521 NodeMCU + MPU6050 devkit 1.0 + board Lolin Description + ======= ========== ==================================================== + VCC VU (5V USB) Not available on all boards so use 3.3V if needed. + GND G Ground + SCL D1 (GPIO05) I2C clock + SDA D2 (GPIO04) I2C data + XDA not connected + XCL not connected + AD0 not connected + INT D8 (GPIO15) Interrupt pin + +*/ + +#if defined(ESP8266) +#include +#else +#include +#endif +#include +#include +#include +#include +#include //https://github.com/tzapu/WiFiManager + +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" + +#include "MPU6050_6Axis_MotionApps20.h" +//#include "MPU6050.h" // not necessary if using MotionApps include file + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 mpu; +//MPU6050 mpu(0x69); // <-- use for AD0 high + +/* ========================================================================= + NOTE: In addition to connection 5/3.3v, GND, SDA, and SCL, this sketch + depends on the MPU-6050's INT pin being connected to the ESP8266 GPIO15 + pin. + * ========================================================================= */ + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector + + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT_OSC" if you want output that matches the +// format used for the InvenSense teapot demo +#define OUTPUT_TEAPOT_OSC + + +#ifdef OUTPUT_READABLE_EULER +float euler[3]; // [psi, theta, phi] Euler angle container +#endif +#ifdef OUTPUT_READABLE_YAWPITCHROLL +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector +#endif + +#define INTERRUPT_PIN 15 // use pin 15 on ESP8266 + +const char DEVICE_NAME[] = "mpu6050"; + +WiFiUDP Udp; // A UDP instance to let us send and receive packets over UDP +const IPAddress outIp(192, 168, 1, 11); // remote IP to receive OSC +const unsigned int outPort = 9999; // remote port to receive OSC + +// ================================================================ +// === INTERRUPT DETECTION ROUTINE === +// ================================================================ + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void ICACHE_RAM_ATTR dmpDataReady() { + mpuInterrupt = true; +} + +void mpu_setup() +{ + // join I2C bus (I2Cdev library doesn't do this automatically) +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); +#endif + + // initialize device + Serial.println(F("Initializing I2C devices...")); + mpu.initialize(); + pinMode(INTERRUPT_PIN, INPUT); + + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + + // load and configure the DMP + Serial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(220); + mpu.setYGyroOffset(76); + mpu.setZGyroOffset(-85); + mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection + Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } +} + +void setup(void) +{ + Serial.begin(115200); + Serial.println(F("\nOrientation Sensor OSC output")); Serial.println(); + + //WiFiManager + //Local intialization. Once its business is done, there is no need to keep it around + WiFiManager wifiManager; + //reset saved settings + //wifiManager.resetSettings(); + + //fetches ssid and pass from eeprom and tries to connect + //if it does not connect it starts an access point with the specified name + //and goes into a blocking loop awaiting configuration + wifiManager.autoConnect(DEVICE_NAME); + + Serial.print(F("WiFi connected! IP address: ")); + Serial.println(WiFi.localIP()); + + mpu_setup(); +} + +void mpu_loop() +{ + // if programming failed, don't try to do anything + if (!dmpReady) return; + + // wait for MPU interrupt or extra packet(s) available + if (!mpuInterrupt && fifoCount < packetSize) return; + + // reset interrupt flag and get INT_STATUS byte + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + +#ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); +#endif + +#ifdef OUTPUT_TEAPOT_OSC +#ifndef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); +#endif + // Send OSC message + OSCMessage msg("/imuquat"); + msg.add((float)q.w); + msg.add((float)q.x); + msg.add((float)q.y); + msg.add((float)q.z); + + Udp.beginPacket(outIp, outPort); + msg.send(Udp); + Udp.endPacket(); + + msg.empty(); +#endif + +#ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + Serial.print("euler\t"); + Serial.print(euler[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(euler[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(euler[2] * 180/M_PI); +#endif + +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(ypr[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(ypr[2] * 180/M_PI); +#endif + +#ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + Serial.print("areal\t"); + Serial.print(aaReal.x); + Serial.print("\t"); + Serial.print(aaReal.y); + Serial.print("\t"); + Serial.println(aaReal.z); +#endif + +#ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x); + Serial.print("\t"); + Serial.print(aaWorld.y); + Serial.print("\t"); + Serial.println(aaWorld.z); +#endif + } +} + +/**************************************************************************/ +/* + Arduino loop function, called once 'setup' is complete (your own code + should go here) +*/ +/**************************************************************************/ +void loop(void) +{ + if (WiFi.status() != WL_CONNECTED) { + Serial.println(); + Serial.println("*** Disconnected from AP so rebooting ***"); + Serial.println(); + ESP.reset(); + } + + mpu_loop(); +} diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/Processing/MPUOSCTeapot/MPUOSCTeapot.pde b/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/Processing/MPUOSCTeapot/MPUOSCTeapot.pde new file mode 100644 index 00000000..dfe65189 --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_ESPWiFi/Processing/MPUOSCTeapot/MPUOSCTeapot.pde @@ -0,0 +1,188 @@ +// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output +// 6/20/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2012-06-20 - initial release + +/* ============================================ +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. +=============================================== +*/ + +/** + * MPUOSCTeapot Processing demo for MPU6050 DMP modified for OSC + * https://gitub.com/jrowberg/i2cdevlib + * The original demo uses serial port I/O which has been replaced with + * OSC UDP messages in this sketch. + * + * The MPU6050 is connected to an ESP8266 with battery so it is completely + * wire free. + * + * Tested on Processing 3.3.5 running on Ubuntu Linux 14.04 + * + * Dependencies installed using Library Manager + * + * Open Sound Control library + * oscP5 website at http://www.sojamo.de/oscP5 + * ToxicLibs + * quaternion functions http://toxiclibs.org/ + * + */ + +// Install oscP5 using the IDE library manager. +// From the IDE menu bar, Sketch | Import Library | Add library. +// In the search box type "osc". +import oscP5.*; +import netP5.*; +// Install ToxicLibs using the IDE library manager +// From the IDE menu bar, Sketch | Import Library | Add library. +// In the search box type "toxic". +import toxi.geom.*; +import toxi.processing.*; + +ToxiclibsSupport gfx; + +Quaternion quat = new Quaternion(1, 0, 0, 0); + +OscP5 oscP5; + +void setup() { + // 300px square viewport using OpenGL rendering + size(300, 300, P3D); + gfx = new ToxiclibsSupport(this); + + // setup lights and antialiasing + lights(); + smooth(); + + /* start oscP5, listening for incoming messages at port 9999 */ + oscP5 = new OscP5(this, 9999); + + oscP5.plug(this, "imu", "/imuquat"); +} + +/* incoming osc message are forwarded to the oscEvent method. */ +void oscEvent(OscMessage theOscMessage) { + /* print the address pattern and the typetag of the received OscMessage */ + //print("### received an osc message."); + //print(" addrpattern: "+theOscMessage.addrPattern()); + //println(" typetag: "+theOscMessage.typetag()); +} + +public void imu(float quant_w, float quant_x, float quant_y, float quant_z) { + //println(quant_w, quant_x, quant_y, quant_z); + quat.set(quant_w, quant_x, quant_y, quant_z); +} + +void draw() { + // black background + background(0); + + // translate everything to the middle of the viewport + pushMatrix(); + translate(width / 2, height / 2); + + // 3-step rotation from yaw/pitch/roll angles (gimbal lock!) + // ...and other weirdness I haven't figured out yet + //rotateY(-ypr[0]); + //rotateZ(-ypr[1]); + //rotateX(-ypr[2]); + + // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) + // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of + // different coordinate system orientation assumptions between Processing + // and InvenSense DMP) + float[] axis = quat.toAxisAngle(); + rotate(axis[0], -axis[1], axis[3], axis[2]); + + // draw main body in red + fill(255, 0, 0, 200); + box(10, 10, 200); + + // draw front-facing tip in blue + fill(0, 0, 255, 200); + pushMatrix(); + translate(0, 0, -120); + rotateX(PI/2); + drawCylinder(0, 20, 20, 8); + popMatrix(); + + // draw wings and tail fin in green + fill(0, 255, 0, 200); + beginShape(TRIANGLES); + vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer + vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer + vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer + vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer + endShape(); + beginShape(QUADS); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); + vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); + vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); + endShape(); + + popMatrix(); +} + +void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { + float angle = 0; + float angleIncrement = TWO_PI / sides; + beginShape(QUAD_STRIP); + for (int i = 0; i < sides + 1; ++i) { + vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); + vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); + angle += angleIncrement; + } + endShape(); + + // If it is not a cone, draw the circular top cap + if (topRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + + // Center point + vertex(0, 0, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } + + // If it is not a cone, draw the circular bottom cap + if (bottomRadius != 0) { + angle = 0; + beginShape(TRIANGLE_FAN); + + // Center point + vertex(0, tall, 0); + for (int i = 0; i < sides + 1; i++) { + vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); + angle += angleIncrement; + } + endShape(); + } +} diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_Ethernet/MPU6050_DMP6_Ethernet.ino b/Arduino/MPU6050/examples/MPU6050_DMP6_Ethernet/MPU6050_DMP6_Ethernet.ino new file mode 100644 index 00000000..b2e2081d --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_Ethernet/MPU6050_DMP6_Ethernet.ino @@ -0,0 +1,545 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) over Ethernet +// 2/27/2016 by hellphoenix +// Based on I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) (6/21/2012 by Jeff Rowberg ) +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2016-04-18 - Eliminated a potential infinite loop +// 2016-02-28 - Cleaned up code to be in line with other example codes +// - Added Ethernet outputs for Quaternion, Euler, RealAccel, WorldAccel +// 2016-02-27 - Initial working code compiled +// Bugs: +// - There is still a hangup after some time, though it only occurs when you are reading data from the website. +// If you only read the data from the serial port, there are no hangups. +/* ============================================ +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 +// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" + +#include "MPU6050_6Axis_MotionApps20.h" +//#include "MPU6050.h" // not necessary if using MotionApps include file + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#include "Wire.h" +#include "avr/wdt.h"// Watchdog library + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 mpu; +//MPU6050 mpu(0x69); // <-- use for AD0 high + +// MAC address from Ethernet shield sticker under board +byte mac[] = { + 0x90, 0xA2, 0xDA, 0x10, 0x26, 0x82 +}; +// assign an IP address for the controller: +IPAddress ip(192,168,1,50); +// the router's gateway address: +byte gateway[] = { 192, 168, 1, 1 }; +// the subnet: +byte subnet[] = { 255, 255, 0, 0 }; + +// Initialize the Ethernet server library +// with the IP address and port you want to use +// (port 80 is default for HTTP): +EthernetServer server(80); + +String HTTP_req; // stores the HTTP request + +/* ========================================================================= + NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch + depends on the MPU-6050's INT pin being connected to the Arduino's + external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is + digital I/O pin 2. + * ========================================================================= */ + +/* ========================================================================= + NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error + when using Serial.write(buf, len). The Teapot output uses this method. + The solution requires a modification to the Arduino USBAPI.h file, which + is fortunately simple, but annoying. This will be fixed in the next IDE + release. For more info, see these links: + + http://arduino.cc/forum/index.php/topic,109987.0.html + http://code.google.com/p/arduino/issues/detail?id=958 + * ========================================================================= */ + + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT" if you want output that matches the +// format used for the InvenSense teapot demo +//#define OUTPUT_TEAPOT + +#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards +#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) +bool blinkState = false; + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +// packet structure for InvenSense teapot demo +uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; + + + +// ================================================================ +// === INTERRUPT DETECTION ROUTINE === +// ================================================================ + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void dmpDataReady() { + mpuInterrupt = true; +} +// ================================================================ +// === INITIAL SETUP === +// ================================================================ + +void setup() { + wdt_enable(WDTO_1S); //Watchdog enable. + //WDTO_1S sets the watchdog timer to 1 second. The time set here is approximate. + // You can find more time settings at http://www.nongnu.org/avr-libc/user-manual/group__avr__watchdog.html . + + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + // initialize serial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + Serial.begin(115200); + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino + // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to + // the baud timing being too misaligned with processor ticks. You must use + // 38400 or slower in these cases, or use some kind of external separate + // crystal solution for the UART timer. + + Ethernet.begin(mac, ip, gateway, subnet); + server.begin(); + Serial.print("server is at "); + Serial.println(Ethernet.localIP()); + while (!Serial); // wait for Leonardo enumeration, others continue immediately + + // initialize device + Serial.println(F("Initializing I2C devices...")); + mpu.initialize(); + pinMode(INTERRUPT_PIN, INPUT); + + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + + // load and configure the DMP + Serial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(220); + mpu.setYGyroOffset(76); + mpu.setZGyroOffset(-85); + mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection + Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); + Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); + Serial.println(F(")...")); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } + + // configure LED for output + pinMode(LED_PIN, OUTPUT); +} + + + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + + wdt_reset();//Resets the watchdog timer. If the timer is not reset, and the timer expires, a watchdog-initiated device reset will occur. + // wait for MPU interrupt or extra packet(s) available + while (!mpuInterrupt && fifoCount < packetSize) { + if (mpuInterrupt && fifoCount < packetSize) { + // try to get out of the infinite loop + fifoCount = mpu.getFIFOCount(); + } + // other program behavior stuff here + // . + // . + // if you are really paranoid you can frequently test in between other + // stuff to see if mpuInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the MPU data + // . + // . + // . + } + + // reset interrupt flag and get INT_STATUS byte + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & (1 << MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + fifoCount = mpu.getFIFOCount(); + Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & (1 << MPU6050_INTERRUPT_DMP_INT_BIT)) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO, then clear the buffer + mpu.getFIFOBytes(fifoBuffer, packetSize); + //mpu.resetFIFO(); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + + #ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); + #endif + #ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + Serial.print("euler\t"); + Serial.print(euler[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(euler[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(euler[2] * 180/M_PI); + #endif + #ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(ypr[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(ypr[2] * 180/M_PI); + #endif + #ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + Serial.print("areal\t"); + Serial.print(aaReal.x); + Serial.print("\t"); + Serial.print(aaReal.y); + Serial.print("\t"); + Serial.println(aaReal.z); + #endif + #ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x); + Serial.print("\t"); + Serial.print(aaWorld.y); + Serial.print("\t"); + Serial.println(aaWorld.z); + #endif + #ifdef OUTPUT_TEAPOT + // display quaternion values in InvenSense Teapot demo format: + teapotPacket[2] = fifoBuffer[0]; + teapotPacket[3] = fifoBuffer[1]; + teapotPacket[4] = fifoBuffer[4]; + teapotPacket[5] = fifoBuffer[5]; + teapotPacket[6] = fifoBuffer[8]; + teapotPacket[7] = fifoBuffer[9]; + teapotPacket[8] = fifoBuffer[12]; + teapotPacket[9] = fifoBuffer[13]; + Serial.write(teapotPacket, 14); + teapotPacket[11]++; // packetCount, loops at 0xFF on purpose + #endif + serversend(); + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + } +} + +void serversend(){ + + EthernetClient client = server.available(); // try to get client + + if (client) { // got client? + //boolean currentLineIsBlank = true; + while (client.connected()) { + if (client.available()) { // client data available to read + char c = client.read(); // read 1 byte (character) from client + HTTP_req += c; // save the HTTP request 1 char at a time + // last line of client request is blank and ends with \n + // respond to client only after last line received + if (c == '\n') { + // send a standard http response header + client.println("HTTP/1.1 200 OK"); + client.println("Content-Type: text/html"); + //client.println("Connection: keep-alive"); + client.println(); + // AJAX request for switch state + if (HTTP_req.indexOf("ajax_switch") > -1) { + // read switch state and analog input + GetAjaxData(client); + } + else { // HTTP request for web page + // send web page - contains JavaScript with AJAX calls + client.println(""); + client.println(""); + client.println(""); + client.println("Arduino Web Page"); + client.println(""); + client.println(""); + client.println(""); + client.println("

MPU6050 Output

"); + client.println("
"); + client.println("
"); + client.println(""); + client.println(""); + } + // display received HTTP request on serial port + Serial.print(HTTP_req); + HTTP_req = ""; // finished with request, empty string + client.stop(); // close the connection + break; + } + } + } + } +} + +void GetAjaxData(EthernetClient cl) +{ + #ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + cl.print("Quaternion Values:\t"); + cl.print("

w:"); + cl.print(q.w); + cl.print("\t"); + cl.println("

"); + cl.print("

x:"); + cl.print(q.x); + cl.print("\t"); + cl.println("

"); + cl.print("

y:"); + cl.print(q.y); + cl.print("\t"); + cl.println("

"); + cl.print("

z:"); + cl.print(q.z); + cl.print("\t"); + cl.println("

"); + #endif + #ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + cl.print("Euler Angles:\t"); + cl.print("

Yaw:"); + cl.print(euler[0] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + cl.print("

Pitch:"); + cl.print(euler[2] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + cl.print("

Roll:"); + cl.print(euler[1] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + #endif + #ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Yaw/Pitch/Roll values in degrees + cl.print("Yaw, Pitch, and Roll:\t"); + cl.print("

Yaw:"); + cl.print(ypr[0] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + cl.print("

Pitch:"); + cl.print(ypr[2] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + cl.print("

Roll:"); + cl.print(ypr[1] * 180/M_PI); + cl.print("\t"); + cl.println("

"); + #endif + #ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + cl.print("Real Accel:\t"); + cl.print("

Yaw:"); + cl.print(aaReal.x); + cl.print("\t"); + cl.println("

"); + cl.print("

Pitch:"); + cl.print(aaReal.z); + cl.print("\t"); + cl.println("

"); + cl.print("

Roll:"); + cl.print(aaReal.y); + cl.print("\t"); + cl.println("

"); + #endif + #ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + cl.print("World Accel:\t"); + cl.print("

Yaw:"); + cl.print(aaWorld.x); + cl.print("\t"); + cl.println("

"); + cl.print("

Pitch:"); + cl.print(aaWorld.z); + cl.print("\t"); + cl.println("

"); + cl.print("

Roll:"); + cl.print(aaWorld.y); + cl.print("\t"); + cl.println("

"); + #endif + #ifdef OUTPUT_TEAPOT + cl.print("

teapotpacket:"); + cl.write(teapotPacket, 14); + cl.print("\t"); + cl.println("

"); + #endif +} diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino new file mode 100644 index 00000000..fd7ebf96 --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino @@ -0,0 +1,368 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12) +// 6/21/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2019-07-10 - Uses the new version of the DMP Firmware V6.12 +// - Note: I believe the Teapot demo is broken with this versin as +// - the fifo buffer structure has changed +// 2016-04-18 - Eliminated a potential infinite loop +// 2013-05-08 - added seamless Fastwire support +// - added note about gyro calibration +// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error +// 2012-06-20 - improved FIFO overflow handling and simplified read process +// 2012-06-19 - completely rearranged DMP initialization code and simplification +// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly +// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING +// 2012-06-05 - add gravity-compensated initial reference frame acceleration output +// - add 3D math helper file to DMP6 example sketch +// - add Euler output and Yaw/Pitch/Roll output formats +// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) +// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 +// 2012-05-30 - basic DMP initialization working + +/* ============================================ + 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 and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" + +#include "MPU6050_6Axis_MotionApps612.h" +//#include "MPU6050.h" // not necessary if using MotionApps include file + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE +#include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) +// AD0 high = 0x69 +MPU6050 mpu; +//MPU6050 mpu(0x69); // <-- use for AD0 high + +/* ========================================================================= + NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch + depends on the MPU-6050's INT pin being connected to the Arduino's + external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is + digital I/O pin 2. + ========================================================================= */ + +/* ========================================================================= + NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error + when using Serial.write(buf, len). The Teapot output uses this method. + The solution requires a modification to the Arduino USBAPI.h file, which + is fortunately simple, but annoying. This will be fixed in the next IDE + release. For more info, see these links: + + http://arduino.cc/forum/index.php/topic,109987.0.html + http://code.google.com/p/arduino/issues/detail?id=958 + ========================================================================= */ + + + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT" if you want output that matches the +// format used for the InvenSense teapot demo +//#define OUTPUT_TEAPOT + + + +#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards +#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) +bool blinkState = false; + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 gy; // [x, y, z] gyro sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +// packet structure for InvenSense teapot demo +uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' }; + + + +// ================================================================ +// === INTERRUPT DETECTION ROUTINE === +// ================================================================ + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void dmpDataReady() { + mpuInterrupt = true; +} + + + +// ================================================================ +// === INITIAL SETUP === +// ================================================================ + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); +#endif + + // initialize serial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + Serial.begin(115200); + while (!Serial); // wait for Leonardo enumeration, others continue immediately + + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino + // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to + // the baud timing being too misaligned with processor ticks. You must use + // 38400 or slower in these cases, or use some kind of external separate + // crystal solution for the UART timer. + + // initialize device + Serial.println(F("Initializing I2C devices...")); + mpu.initialize(); + pinMode(INTERRUPT_PIN, INPUT); + + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + + // wait for ready + Serial.println(F("\nSend any character to begin DMP programming and demo: ")); + while (Serial.available() && Serial.read()); // empty buffer + while (!Serial.available()); // wait for data + while (Serial.available() && Serial.read()); // empty buffer again + + // load and configure the DMP + Serial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(51); + mpu.setYGyroOffset(8); + mpu.setZGyroOffset(21); + mpu.setXAccelOffset(1150); + mpu.setYAccelOffset(-50); + mpu.setZAccelOffset(1060); + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + Serial.println(); + mpu.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection + Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); + Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); + Serial.println(F(")...")); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } + + // configure LED for output + pinMode(LED_PIN, OUTPUT); +} + + + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + +#ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); +#endif + +#ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + Serial.print("euler\t"); + Serial.print(euler[0] * 180 / M_PI); + Serial.print("\t"); + Serial.print(euler[1] * 180 / M_PI); + Serial.print("\t"); + Serial.println(euler[2] * 180 / M_PI); +#endif + +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * 180 / M_PI); + Serial.print("\t"); + Serial.print(ypr[1] * 180 / M_PI); + Serial.print("\t"); + Serial.print(ypr[2] * 180 / M_PI); + /* + mpu.dmpGetAccel(&aa, fifoBuffer); + Serial.print("\tRaw Accl XYZ\t"); + Serial.print(aa.x); + Serial.print("\t"); + Serial.print(aa.y); + Serial.print("\t"); + Serial.print(aa.z); + mpu.dmpGetGyro(&gy, fifoBuffer); + Serial.print("\tRaw Gyro XYZ\t"); + Serial.print(gy.x); + Serial.print("\t"); + Serial.print(gy.y); + Serial.print("\t"); + Serial.print(gy.z); + */ + Serial.println(); + +#endif + +#ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + Serial.print("areal\t"); + Serial.print(aaReal.x); + Serial.print("\t"); + Serial.print(aaReal.y); + Serial.print("\t"); + Serial.println(aaReal.z); +#endif + +#ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x); + Serial.print("\t"); + Serial.print(aaWorld.y); + Serial.print("\t"); + Serial.println(aaWorld.z); +#endif + +#ifdef OUTPUT_TEAPOT + // display quaternion values in InvenSense Teapot demo format: + teapotPacket[2] = fifoBuffer[0]; + teapotPacket[3] = fifoBuffer[1]; + teapotPacket[4] = fifoBuffer[4]; + teapotPacket[5] = fifoBuffer[5]; + teapotPacket[6] = fifoBuffer[8]; + teapotPacket[7] = fifoBuffer[9]; + teapotPacket[8] = fifoBuffer[12]; + teapotPacket[9] = fifoBuffer[13]; + Serial.write(teapotPacket, 14); + teapotPacket[11]++; // packetCount, loops at 0xFF on purpose +#endif + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + } +} diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/MPUplane.pde b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/MPUplane.pde new file mode 100644 index 00000000..ce4a6fda --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/MPUplane.pde @@ -0,0 +1,189 @@ +// I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output +// 6/20/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2012-06-20 - initial release +// 2016-10-28 - Changed to bi-plane 3d model based on tutorial at +// https://forum.processing.org/two/discussion/24350/display-obj-file-in-3d +// https://opengameart.org/content/low-poly-biplane + +/* ============================================ +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. +=============================================== +*/ + +import processing.serial.*; +//import processing.opengl.*; +import toxi.geom.*; +import toxi.processing.*; + +// NOTE: requires ToxicLibs to be installed in order to run properly. +// 1. Download from http://toxiclibs.org/downloads +// 2. Extract into [userdir]/Processing/libraries +// (location may be different on Mac/Linux) +// 3. Run and bask in awesomeness + +ToxiclibsSupport gfx; + +Serial port; // The serial port +char[] teapotPacket = new char[14]; // InvenSense Teapot packet +int serialCount = 0; // current packet byte position +int synced = 0; +int interval = 0; + +float[] q = new float[4]; +Quaternion quat = new Quaternion(1, 0, 0, 0); + +float[] gravity = new float[3]; +float[] euler = new float[3]; +float[] ypr = new float[3]; + + +PShape plane; // 3d model + +void setup() { + // 640x480 px square viewport + size(640, 480, P3D); + gfx = new ToxiclibsSupport(this); + + // setup lights and antialiasing + lights(); + smooth(); + + // display serial port list for debugging/clarity + println(Serial.list()); + + // get a specific serial port + String portName = "COM12"; + + // open the serial port + port = new Serial(this, portName, 115200); + + // send single character to trigger DMP init/start + // (expected by MPU6050_DMP6 example Arduino sketch) + port.write('r'); + + // Load Plane object + // The file must be in the \data folder + // of the current sketch to load successfully + plane = loadShape("biplane.obj"); + + + // apply its texture and set orientation + PImage img1=loadImage("diffuse_512.png"); + plane.setTexture(img1); + plane.scale(30); + plane.rotateX(PI); + plane.rotateY(PI+HALF_PI); + + +} + +void draw() { + if (millis() - interval > 1000) { + // resend single character to trigger DMP init/start + // in case the MPU is halted/reset while applet is running + port.write('r'); + interval = millis(); + } + + // black background + background(0); + + + // translate everything to the middle of the viewport + pushMatrix(); + translate(width / 2, height / 2); + + // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) + // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of + // different coordinate system orientation assumptions between Processing + // and InvenSense DMP) + float[] axis = quat.toAxisAngle(); + rotate(axis[0], -axis[1], axis[3], axis[2]); + + // draw plane + shape(plane, 0, 0); + + popMatrix(); +} + +void serialEvent(Serial port) { + interval = millis(); + while (port.available() > 0) { + int ch = port.read(); + + if (synced == 0 && ch != '$') return; // initial synchronization - also used to resync/realign if needed + synced = 1; + print ((char)ch); + + if ((serialCount == 1 && ch != 2) + || (serialCount == 12 && ch != '\r') + || (serialCount == 13 && ch != '\n')) { + serialCount = 0; + synced = 0; + return; + } + + if (serialCount > 0 || ch == '$') { + teapotPacket[serialCount++] = (char)ch; + if (serialCount == 14) { + serialCount = 0; // restart packet byte position + + // get quaternion from data packet + q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; + q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; + q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; + q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; + for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; + + // set our toxilibs quaternion to new data + quat.set(q[0], q[1], q[2], q[3]); + + + // below calculations unnecessary for orientation only using toxilibs + + // calculate gravity vector + gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); + gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); + gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; + + // calculate Euler angles + euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); + euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); + euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); + + // calculate yaw/pitch/roll angles + ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); + ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); + ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); + + // output various components for debugging + println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); + println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); + println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); + + } + } + } +} diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/biplane.obj b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/biplane.obj new file mode 100644 index 00000000..6867928e --- /dev/null +++ b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/biplane.obj @@ -0,0 +1,2239 @@ +v 4.500000 0.176989 0.367522 +v 4.500000 0.318923 0.254332 +v 4.500000 0.397690 0.090770 +v 4.500000 0.397690 -0.090770 +v 4.500000 0.318923 -0.254333 +v 4.500000 0.176989 -0.367521 +v 4.500000 0.000000 -0.407918 +v 4.500000 -0.176989 -0.367521 +v 4.500000 -0.318923 -0.254332 +v 4.500000 -0.397691 -0.090770 +v 4.500000 -0.397691 0.090770 +v 4.500000 -0.318923 0.254333 +v 4.500001 -0.176989 0.367522 +v 4.500000 0.000000 0.407918 +v 2.700000 0.259478 0.538812 +v 2.700000 0.467563 0.372869 +v 2.700000 0.583042 0.133076 +v 2.700000 0.583042 -0.133076 +v 2.700000 0.467563 -0.372869 +v 2.700000 0.259478 -0.538812 +v 2.700000 0.000000 -0.598036 +v 2.700000 -0.259478 -0.538812 +v 2.700000 -0.467563 -0.372869 +v 2.700000 -0.583042 -0.133075 +v 2.700000 -0.583042 0.133075 +v 2.700000 -0.467563 0.372870 +v 2.700000 -0.259478 0.538812 +v 2.700000 0.000000 0.598036 +v 0.900000 0.323112 0.670950 +v 0.900000 0.582229 0.464312 +v 0.900000 0.726027 0.165711 +v 0.900000 0.726027 -0.165711 +v 0.900000 0.582229 -0.464312 +v 0.900000 0.323113 -0.670950 +v 0.900000 0.000000 -0.744698 +v 0.900000 -0.323112 -0.670950 +v 0.900000 -0.582229 -0.464312 +v 0.900000 -0.726027 -0.165711 +v 0.900000 -0.726027 0.165711 +v 0.900000 -0.582229 0.464312 +v 0.900000 -0.323113 0.670950 +v 0.900000 0.000000 0.744699 +v -0.900000 0.384390 0.798195 +v -0.900000 0.692647 0.552368 +v -0.475444 0.863717 0.197138 +v -0.475445 0.863717 -0.197138 +v -0.900000 0.692647 -0.552368 +v -0.900000 0.384390 -0.798194 +v -0.900000 0.000000 -0.885929 +v -0.900000 -0.384390 -0.798195 +v -0.900000 -0.692647 -0.552368 +v -0.900000 -0.863717 -0.197138 +v -0.900000 -0.863717 0.197138 +v -0.900000 -0.692647 0.552368 +v -0.900000 -0.384390 0.798195 +v -0.900000 0.000000 0.885929 +v -2.700000 0.433883 0.900969 +v -2.700000 0.781831 0.623490 +v -2.885778 0.974928 0.222521 +v -2.885778 0.974928 -0.222521 +v -2.700000 0.781832 -0.623490 +v -2.699999 0.433884 -0.900969 +v -2.699999 0.000000 -1.000000 +v -2.700000 -0.433883 -0.900969 +v -2.700000 -0.781831 -0.623490 +v -2.700000 -0.974928 -0.222521 +v -2.700000 -0.974928 0.222521 +v -2.700000 -0.781832 0.623489 +v -2.700000 -0.433884 0.900969 +v -2.700000 0.000000 1.000000 +v 4.500000 0.000000 -0.000000 +v -5.181942 0.000000 -0.000000 +v 5.269916 1.092397 0.090771 +v 5.269916 1.092397 -0.090770 +v 3.399458 1.972455 0.000001 +v 5.551387 0.000000 -0.000000 +v 4.141554 0.176989 1.784806 +v 4.500000 0.318923 1.235124 +v 2.700000 0.467563 1.342699 +v 3.058446 0.259478 1.802116 +v 4.500000 0.318923 -1.235123 +v 4.141554 0.176989 -1.784806 +v 3.058446 0.259478 -1.802116 +v 2.700000 0.467563 -1.342699 +v -1.300000 1.103771 0.487962 +v -1.125000 1.252653 0.195352 +v -2.475000 1.333997 0.213918 +v -2.300000 1.156989 0.519851 +v -1.125000 1.252653 -0.195351 +v -2.475000 1.333997 -0.213917 +v -1.300000 1.103772 -0.487962 +v -2.300000 1.156989 -0.519851 +v -1.799999 1.281724 0.398879 +v -1.125000 1.187470 0.380784 +v -1.800000 1.350005 0.204635 +v -2.475000 1.262618 0.416974 +v -1.800000 1.156763 0.540666 +v -1.800000 1.372766 0.000000 +v -1.125000 1.274381 0.000000 +v -1.800000 1.350006 -0.204634 +v -2.475000 1.357790 0.000000 +v -1.800000 1.281724 -0.398879 +v -1.125000 1.187470 -0.380784 +v -1.800000 1.156763 -0.540666 +v -2.475000 1.262618 -0.416974 +v -0.825593 1.004904 0.374753 +v -0.475445 0.778182 0.374753 +v -0.825593 1.069055 0.192257 +v -1.125000 0.951901 0.516854 +v -2.700000 1.105101 0.423005 +v -2.885779 0.878380 0.423006 +v -2.475000 1.021542 0.564478 +v -2.700000 1.177512 0.217012 +v -1.800000 0.963961 0.587929 +v -1.800000 0.737239 0.587929 +v -0.825593 1.090439 0.000000 +v -0.475445 0.863717 0.000000 +v -0.825593 1.069055 -0.192257 +v -2.700000 1.201650 0.000000 +v -2.885779 0.974928 0.000000 +v -2.700000 1.177513 -0.217012 +v -0.825592 1.004904 -0.374753 +v -0.475444 0.778182 -0.374753 +v -1.125000 0.951901 -0.516854 +v -1.800000 0.963961 -0.587929 +v -1.800000 0.737239 -0.587929 +v -2.475000 1.021542 -0.564478 +v -2.700000 1.105101 -0.423005 +v -2.885778 0.878380 -0.423005 +v 2.700000 0.583042 0.000000 +v 0.900000 0.726027 0.005801 +v -5.181943 -0.629401 -0.501931 +v -4.986976 -0.781831 -0.623490 +v -5.181942 -0.784850 -0.179137 +v -4.986976 -0.974928 -0.222521 +v -5.181942 -0.784850 0.179137 +v -4.986976 -0.974928 0.222521 +v -5.181942 -0.629401 0.501930 +v -4.986977 -0.781831 0.623490 +v -5.181942 -0.349291 0.725311 +v -4.986976 -0.433884 0.900969 +v -4.986976 0.000000 1.000000 +v -5.181942 0.000000 0.805034 +v -5.181942 0.629401 -0.501930 +v -4.987885 0.789991 -0.606546 +v -4.986976 0.781832 -0.623490 +v -5.181942 0.349291 -0.725310 +v -4.986977 0.433884 -0.900968 +v -4.986976 0.433884 0.900969 +v -5.181942 0.349291 0.725311 +v -4.986976 0.781831 0.623490 +v -4.987885 0.789991 0.606546 +v -5.181942 0.629401 0.501931 +v -4.986976 0.974928 0.222521 +v -5.181942 0.784301 0.179012 +v -5.181942 0.000000 -0.805034 +v -4.986976 0.000000 -1.000000 +v -5.181942 -0.349291 -0.725310 +v -4.986976 -0.433884 -0.900969 +v -5.181942 0.784301 -0.179011 +v -4.986976 0.974928 -0.222521 +v -5.181942 0.779962 0.000001 +v -4.986976 0.974928 0.000000 +vt 0.455704 0.153054 +vt 0.419776 0.128644 +vt 0.383319 0.143119 +vt 0.328896 0.143119 +vt 0.292439 0.128644 +vt 0.256511 0.153054 +vt 0.245563 0.201018 +vt 0.256511 0.248981 +vt 0.287184 0.287445 +vt 0.331509 0.308790 +vt 0.380706 0.308790 +vt 0.425031 0.287445 +vt 0.455704 0.248981 +vt 0.466652 0.201018 +vt 0.005173 0.359445 +vt 0.016095 0.354912 +vt 0.061664 0.354281 +vt 0.166522 0.327467 +vt 0.186034 0.323730 +vt 0.219371 0.310411 +vt 0.248725 0.289652 +vt 0.263886 0.273262 +vt 0.275993 0.321494 +vt 0.305445 0.321494 +vt 0.334898 0.321494 +vt 0.364351 0.321494 +vt 0.393803 0.321494 +vt 0.423256 0.321494 +vt 0.452709 0.321494 +vt 0.019054 0.449768 +vt 0.082140 0.442189 +vt 0.099276 0.445505 +vt 0.128729 0.445505 +vt 0.158182 0.445505 +vt 0.187634 0.445505 +vt 0.217087 0.445505 +vt 0.246540 0.445505 +vt 0.275993 0.445505 +vt 0.305445 0.445505 +vt 0.334898 0.445505 +vt 0.364351 0.445505 +vt 0.393803 0.445505 +vt 0.423256 0.445505 +vt 0.452709 0.445505 +vt 0.040371 0.569517 +vt 0.069824 0.569517 +vt 0.099276 0.569517 +vt 0.128729 0.569517 +vt 0.158182 0.569517 +vt 0.187634 0.569517 +vt 0.217087 0.569517 +vt 0.246540 0.569517 +vt 0.275993 0.569517 +vt 0.305445 0.569517 +vt 0.334898 0.569517 +vt 0.364351 0.569517 +vt 0.393803 0.569517 +vt 0.423256 0.569517 +vt 0.452709 0.569517 +vt 0.040371 0.693528 +vt 0.073505 0.709029 +vt 0.099276 0.685777 +vt 0.128729 0.685777 +vt 0.154500 0.709029 +vt 0.187634 0.693528 +vt 0.217087 0.693528 +vt 0.246540 0.693528 +vt 0.275993 0.693528 +vt 0.305445 0.693528 +vt 0.334898 0.693528 +vt 0.364351 0.693528 +vt 0.393803 0.693528 +vt 0.423256 0.693528 +vt 0.452709 0.693528 +vt 0.040371 0.817539 +vt 0.073505 0.802038 +vt 0.154500 0.802038 +vt 0.356107 0.180046 +vt 0.098756 0.319100 +vt 0.114003 0.445505 +vt 0.087917 0.323202 +vt 0.360587 0.109025 +vt 0.356108 0.136215 +vt 0.351628 0.109025 +vt 0.023534 0.441375 +vt 0.044323 0.425256 +vt 0.082140 0.442189 +vt 0.019054 0.449768 +vt 0.204695 0.353513 +vt 0.203181 0.378966 +vt 0.187634 0.445505 +vt 0.158182 0.445505 +vt 0.076369 0.721086 +vt 0.099276 0.709029 +vt 0.099276 0.802038 +vt 0.076369 0.789982 +vt 0.128729 0.709029 +vt 0.128729 0.802038 +vt 0.151637 0.721086 +vt 0.151637 0.789982 +vt 0.084550 0.755534 +vt 0.084550 0.709029 +vt 0.099276 0.755534 +vt 0.084550 0.802038 +vt 0.073505 0.755534 +vt 0.114003 0.755534 +vt 0.114003 0.709029 +vt 0.128729 0.755534 +vt 0.114003 0.802038 +vt 0.143455 0.755534 +vt 0.143455 0.709029 +vt 0.154500 0.755534 +vt 0.143455 0.802038 +vt 0.084550 0.693528 +vt 0.084550 0.693528 +vt 0.099276 0.693528 +vt 0.073505 0.709029 +vt 0.084550 0.817539 +vt 0.073505 0.802038 +vt 0.099276 0.817539 +vt 0.069824 0.755534 +vt 0.069824 0.755534 +vt 0.114003 0.693528 +vt 0.114003 0.693528 +vt 0.128729 0.693528 +vt 0.114003 0.817539 +vt 0.128729 0.817539 +vt 0.143455 0.693528 +vt 0.143455 0.693528 +vt 0.154500 0.709029 +vt 0.158182 0.755534 +vt 0.158182 0.755534 +vt 0.154500 0.802038 +vt 0.143455 0.817539 +vt 0.114003 0.445505 +vt 0.113487 0.569517 +vt 0.084550 0.817539 +vt 0.063450 0.938935 +vt 0.275993 0.817539 +vt 0.060733 0.120401 +vt 0.305445 0.817539 +vt 0.104650 0.099252 +vt 0.334898 0.817539 +vt 0.153395 0.099252 +vt 0.364351 0.817539 +vt 0.197313 0.120401 +vt 0.423256 0.925429 +vt 0.393803 0.817539 +vt 0.423256 0.817539 +vt 0.143455 0.817539 +vt 0.060733 0.291667 +vt 0.030341 0.253557 +vt 0.449688 0.935863 +vt 0.452709 0.817539 +vt 0.053984 0.934786 +vt 0.017895 0.932919 +vt 0.100219 0.950069 +vt 0.102123 0.931740 +vt 0.099276 0.825290 +vt 0.217087 0.817539 +vt 0.187634 0.817539 +vt 0.019494 0.206034 +vt 0.030341 0.158511 +vt 0.246540 0.817539 +vt 0.104667 0.312738 +vt 0.129023 0.201653 +vt 0.128729 0.825290 +vt 0.114003 0.817539 +vt 0.129023 0.312124 +vt 0.275993 0.931809 +vt 0.273121 0.941551 +vt 0.249411 0.941551 +vt 0.278864 0.941551 +vt 0.305445 0.931809 +vt 0.302574 0.941551 +vt 0.308316 0.941551 +vt 0.334898 0.931809 +vt 0.332027 0.941551 +vt 0.337769 0.941551 +vt 0.364351 0.931809 +vt 0.361480 0.941551 +vt 0.367222 0.941551 +vt 0.396310 0.932910 +vt 0.390932 0.941551 +vt 0.227704 0.158511 +vt 0.153378 0.312738 +vt 0.066951 0.949302 +vt 0.411698 0.969294 +vt 0.007495 0.983733 +vt 0.156937 0.931070 +vt 0.155311 0.941551 +vt 0.131609 0.941551 +vt 0.161053 0.941551 +vt 0.187634 0.931809 +vt 0.184763 0.941551 +vt 0.190506 0.941551 +vt 0.217087 0.931809 +vt 0.214216 0.941551 +vt 0.437979 0.994491 +vt 0.197313 0.291667 +vt 0.050289 0.993403 +vt 0.393883 0.992961 +vt 0.238551 0.206034 +vt 0.102156 0.941551 +vt 0.114003 0.931021 +vt 0.219958 0.941551 +vt 0.246540 0.931809 +vt 0.243669 0.941551 +vt 0.128729 0.931680 +vt 0.125850 0.941551 +vt 0.114003 0.941551 +vt 0.158299 0.939937 +vt 0.157893 0.930592 +vt 0.227704 0.253557 +vt 0.046355 0.992034 +vt 0.421247 0.969294 +vn 0.089573 0.940981 0.326392 +vn 0.089573 0.940981 0.326392 +vn 0.089573 0.940981 0.326392 +vn 0.089573 0.940981 0.326392 +vn 0.089574 0.940981 -0.326392 +vn 0.089574 0.940981 -0.326392 +vn 0.089574 0.940981 -0.326392 +vn 0.089574 0.940981 -0.326392 +vn 0.150361 -0.955640 -0.253265 +vn 0.180233 0.000001 -0.983624 +vn 0.091281 0.000000 -0.995825 +vn -0.150994 -0.690915 -0.706992 +vn 0.180233 -0.426779 -0.886214 +vn 0.091281 -0.432073 -0.897208 +vn 0.180233 -0.769028 -0.613280 +vn 0.091281 -0.778567 -0.620887 +vn 0.180233 -0.958962 -0.218877 +vn 0.091281 -0.970858 -0.221593 +vn 0.180233 -0.958962 0.218877 +vn 0.091281 -0.970858 0.221592 +vn 0.180233 -0.769028 0.613280 +vn 0.091281 -0.778567 0.620887 +vn 0.180234 -0.426778 0.886214 +vn 0.091281 -0.432072 0.897208 +vn 0.180233 -0.000000 0.983624 +vn 0.091281 0.000000 0.995825 +vn 0.150361 -0.955640 0.253265 +vn 0.016625 -0.653805 0.756481 +vn 0.079554 0.000000 -0.996831 +vn 0.079554 0.432509 -0.898113 +vn 0.079554 -0.432508 -0.898113 +vn 0.079554 -0.779353 -0.621514 +vn 0.079554 -0.971838 -0.221816 +vn 0.079554 -0.971838 0.221816 +vn 0.079554 -0.779354 0.621514 +vn 0.079554 -0.432509 0.898113 +vn 0.079554 0.000000 0.996831 +vn 0.070121 -0.432816 -0.898751 +vn 0.070121 0.000000 -0.997539 +vn 0.070121 -0.779907 -0.621955 +vn 0.070121 -0.972528 -0.221973 +vn 0.070121 -0.972528 0.221973 +vn 0.070121 -0.779907 0.621955 +vn 0.070121 -0.432816 0.898751 +vn 0.130833 0.712633 0.689229 +vn 0.130833 0.712633 0.689229 +vn 0.130833 0.712633 0.689229 +vn 0.130833 0.712633 0.689229 +vn 0.143365 0.983605 0.109401 +vn 0.143365 0.983605 0.109401 +vn 0.143365 0.983605 0.109401 +vn 0.143365 0.983605 0.109401 +vn 0.139254 0.934218 -0.328397 +vn 0.139254 0.934218 -0.328397 +vn 0.139254 0.934218 -0.328397 +vn 0.139254 0.934218 -0.328397 +vn 0.026848 -0.433727 -0.900644 +vn 0.026848 0.000000 -0.999640 +vn 0.026848 -0.781550 -0.623265 +vn 0.026848 -0.974576 -0.222441 +vn 0.026848 -0.974577 0.222441 +vn 0.026848 -0.781550 0.623265 +vn 0.026848 -0.433728 0.900644 +vn -0.064587 -0.972892 -0.222057 +vn -0.064587 -0.780199 -0.622188 +vn -0.064587 -0.972892 0.222056 +vn -0.064587 -0.780199 0.622188 +vn -0.064587 -0.432978 0.899088 +vn 0.870350 -0.485974 0.079502 +vn 1.000000 -0.000000 -0.000000 +vn 1.000000 0.000002 -0.000003 +vn 0.870349 -0.485975 -0.079503 +vn 1.000000 -0.000003 0.000000 +vn 0.733799 -0.670454 0.109683 +vn 0.733799 -0.670454 0.109683 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn -0.842065 -0.522167 0.135162 +vn -0.842065 -0.522167 0.135162 +vn -0.842065 -0.522167 0.135162 +vn -0.842065 -0.522167 0.135162 +vn -0.057129 -0.998367 0.000000 +vn -0.057129 -0.998367 0.000000 +vn 0.733799 -0.670455 -0.109683 +vn 0.733799 -0.670455 -0.109683 +vn -0.057129 -0.998367 -0.000000 +vn -0.057129 -0.998367 -0.000000 +vn -0.045744 0.737399 -0.673906 +vn -0.842065 -0.522167 -0.135162 +vn -0.842065 -0.522167 -0.135162 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn 0.082297 0.996608 0.000000 +vn 0.139254 0.934218 0.328398 +vn 0.139254 0.934218 0.328398 +vn 0.139254 0.934218 0.328398 +vn 0.139254 0.934218 0.328398 +vn -0.017811 0.943260 0.331576 +vn -0.017811 0.943260 0.331576 +vn -0.017811 0.943260 0.331576 +vn -0.017811 0.943260 0.331576 +vn -0.013215 0.722929 0.690796 +vn -0.013215 0.722929 0.690796 +vn -0.013215 0.722929 0.690796 +vn -0.013215 0.722929 0.690796 +vn 0.143365 0.983605 -0.109401 +vn 0.143365 0.983605 -0.109401 +vn 0.143365 0.983605 -0.109401 +vn 0.143365 0.983605 -0.109401 +vn -0.022046 0.993630 -0.110516 +vn -0.022046 0.993630 -0.110516 +vn -0.022046 0.993630 -0.110516 +vn -0.022046 0.993630 -0.110516 +vn -0.022046 0.993630 0.110516 +vn -0.022046 0.993630 0.110516 +vn -0.022046 0.993630 0.110516 +vn -0.022046 0.993630 0.110516 +vn 0.130833 0.712634 -0.689229 +vn 0.130833 0.712634 -0.689229 +vn 0.130833 0.712634 -0.689229 +vn 0.130833 0.712634 -0.689229 +vn -0.013215 0.722930 -0.690795 +vn -0.013215 0.722930 -0.690795 +vn -0.013215 0.722930 -0.690795 +vn -0.013215 0.722930 -0.690795 +vn -0.017811 0.943260 -0.331576 +vn -0.017811 0.943260 -0.331576 +vn -0.017811 0.943260 -0.331576 +vn -0.017811 0.943260 -0.331576 +vn -0.545911 0.790429 -0.277854 +vn -0.545911 0.790429 -0.277854 +vn -0.545911 0.790429 -0.277854 +vn -0.545911 0.790429 -0.277854 +vn -0.264463 0.503333 -0.822627 +vn -0.264463 0.503333 -0.822627 +vn -0.264463 0.503333 -0.822627 +vn -0.264463 0.503333 -0.822627 +vn 0.071428 0.391121 -0.917563 +vn -0.022348 0.280539 -0.959582 +vn -0.022348 0.280539 -0.959582 +vn -0.022348 0.280539 -0.959582 +vn -0.022348 0.280539 -0.959582 +vn -0.727792 0.633269 -0.263228 +vn -0.727792 0.633269 -0.263228 +vn -0.727792 0.633269 -0.263228 +vn -0.727792 0.633270 -0.263228 +vn 0.024936 0.422951 -0.905809 +vn 0.020457 0.652869 -0.757194 +vn 0.064463 0.552835 -0.830794 +vn -0.064587 0.432978 -0.899088 +vn -0.051952 0.691351 -0.720648 +vn -0.430065 0.475818 -0.767230 +vn -0.430065 0.475818 -0.767230 +vn -0.430065 0.475818 -0.767230 +vn -0.430065 0.475818 -0.767230 +vn 0.015304 0.118231 -0.992868 +vn 0.015304 0.118231 -0.992868 +vn 0.015304 0.118231 -0.992868 +vn 0.015304 0.118231 -0.992868 +vn -0.064587 -0.432978 -0.899088 +vn -0.064587 0.000000 -0.997912 +vn -0.567759 0.818150 -0.090998 +vn -0.567759 0.818150 -0.090998 +vn -0.567759 0.818150 -0.090998 +vn -0.567759 0.818150 -0.090998 +vn -0.755415 0.654260 -0.035929 +vn -0.755415 0.654260 -0.035929 +vn -0.755415 0.654260 -0.035929 +vn -0.755415 0.654260 -0.035929 +vn 0.000000 0.900969 -0.433884 +vn -0.072505 0.898614 -0.432707 +vn -0.082933 0.960669 -0.265023 +vn 0.000000 0.964593 -0.263742 +vn -0.924988 0.164874 0.342364 +vn -0.927519 0.290707 0.234942 +vn -1.000000 -0.000000 0.000000 +vn -0.925651 0.362381 0.108856 +vn -0.924989 -0.000000 -0.379995 +vn -0.924988 -0.164873 -0.342365 +vn -0.924989 -0.297092 -0.236923 +vn -0.924989 -0.370468 -0.084556 +vn -0.924989 -0.370467 0.084557 +vn -0.924989 -0.297092 0.236923 +vn -0.924989 -0.164874 0.342363 +vn -0.924989 0.000000 0.379995 +vn -0.727792 0.633269 0.263229 +vn -0.727792 0.633269 0.263229 +vn -0.727792 0.633269 0.263229 +vn -0.727792 0.633269 0.263229 +vn -0.545911 0.790429 0.277853 +vn -0.545911 0.790429 0.277853 +vn -0.545911 0.790429 0.277853 +vn -0.545911 0.790429 0.277853 +vn -0.755415 0.654260 0.035930 +vn -0.755415 0.654260 0.035930 +vn -0.755415 0.654260 0.035930 +vn -0.755415 0.654260 0.035930 +vn -0.567759 0.818150 0.090998 +vn -0.567759 0.818150 0.090998 +vn -0.567759 0.818150 0.090998 +vn -0.567759 0.818150 0.090998 +vn -0.076076 0.997102 0.000000 +vn -0.082933 0.960669 0.265024 +vn 0.000000 0.964593 0.263742 +vn 0.000000 1.000000 0.000000 +vn -0.942446 0.334359 0.000000 +vn -0.925651 0.362381 -0.108857 +vn -0.927519 0.290707 -0.234942 +vn -0.924989 0.164874 -0.342364 +vn -0.072505 0.898614 0.432707 +vn 0.000000 0.900969 0.433884 +vn 0.070121 -0.000000 0.997539 +vn 0.026848 -0.000000 0.999640 +vn -0.064587 0.000000 0.997912 +vn 0.015304 0.118232 0.992868 +vn 0.015304 0.118232 0.992868 +vn 0.015304 0.118232 0.992868 +vn 0.015304 0.118232 0.992868 +vn -0.022348 0.280539 0.959583 +vn -0.022348 0.280539 0.959583 +vn -0.022348 0.280539 0.959583 +vn -0.022348 0.280539 0.959583 +vn -0.051952 0.691351 0.720648 +vn -0.064587 0.432978 0.899088 +vn 0.024936 0.422951 0.905809 +vn 0.020457 0.652869 0.757194 +vn -0.430065 0.475818 0.767230 +vn -0.430065 0.475818 0.767230 +vn -0.430065 0.475818 0.767230 +vn -0.430065 0.475818 0.767230 +vn -0.264462 0.503332 0.822628 +vn -0.264462 0.503332 0.822628 +vn -0.264462 0.503332 0.822628 +vn -0.264462 0.503332 0.822628 +vn 0.079554 0.432509 0.898113 +vn 0.081210 0.779249 0.621431 +vn 0.083582 0.756709 0.648386 +vn 0.090884 0.864154 0.494952 +vn 0.074379 0.588458 0.805099 +vn 0.299258 0.416890 0.858282 +vn 0.299258 0.416890 0.858282 +vn 0.299258 0.416890 0.858282 +vn 0.299258 0.416890 0.858282 +vn 0.502967 0.815394 0.286628 +vn 0.502967 0.815394 0.286628 +vn 0.502967 0.815394 0.286628 +vn 0.502967 0.815394 0.286628 +vn 0.296862 0.477629 0.826888 +vn 0.296862 0.477629 0.826888 +vn 0.296862 0.477629 0.826888 +vn 0.296862 0.477629 0.826888 +vn 0.071428 0.391121 0.917564 +vn 0.064463 0.552834 0.830794 +vn 0.070946 0.105332 0.991903 +vn 0.070946 0.105332 0.991903 +vn 0.115643 0.269922 0.955913 +vn 0.115643 0.269922 0.955913 +vn 0.115643 0.269922 0.955913 +vn 0.115643 0.269922 0.955913 +vn 0.083582 0.756710 -0.648386 +vn 0.090884 0.864154 -0.494952 +vn 0.074379 0.588458 -0.805099 +vn 0.070946 0.105332 -0.991903 +vn 0.070946 0.105332 -0.991903 +vn 0.299258 0.416891 -0.858281 +vn 0.299258 0.416891 -0.858281 +vn 0.299258 0.416891 -0.858281 +vn 0.299258 0.416891 -0.858281 +vn 0.296862 0.477628 -0.826888 +vn 0.296862 0.477628 -0.826888 +vn 0.296862 0.477628 -0.826888 +vn 0.296862 0.477628 -0.826888 +vn 0.115643 0.269922 -0.955913 +vn 0.115643 0.269922 -0.955913 +vn 0.115643 0.269922 -0.955913 +vn 0.115643 0.269922 -0.955913 +vn 0.102431 0.896230 0.431601 +vn 0.102431 0.896230 0.431601 +vn 0.102431 0.896230 0.431601 +vn 0.102431 0.896230 0.431601 +vn 0.425735 0.904848 0.000000 +vn 0.425735 0.904848 0.000000 +vn 0.425735 0.904848 0.000000 +vn 0.102431 0.896230 -0.431601 +vn 0.102431 0.896230 -0.431601 +vn 0.102431 0.896230 -0.431601 +vn 0.102431 0.896230 -0.431601 +vn 0.968371 0.249514 0.000000 +vn 0.968371 0.249514 0.000000 +vn 0.968371 0.249514 0.000000 +vn 0.004416 0.060621 -0.998151 +vn 0.004416 0.060621 -0.998151 +vn 0.004416 0.060621 -0.998151 +vn 0.004416 0.060621 -0.998151 +vn -0.893202 0.449656 0.000001 +vn -0.893202 0.449656 0.000001 +vn -0.893202 0.449656 0.000001 +vn 0.004416 0.060620 0.998151 +vn 0.004416 0.060620 0.998151 +vn 0.004416 0.060620 0.998151 +vn 0.004416 0.060620 0.998151 +vn 0.043031 -0.112813 0.992684 +vn 0.043031 -0.112813 0.992684 +vn 0.043031 -0.112813 0.992684 +vn 0.043031 -0.112813 0.992684 +vn 0.043030 -0.112812 -0.992684 +vn 0.043030 -0.112812 -0.992684 +vn 0.043030 -0.112812 -0.992684 +vn 0.043030 -0.112812 -0.992684 +vn 0.524265 0.850274 0.046693 +vn 0.524265 0.850274 0.046693 +vn 0.524265 0.850274 0.046693 +vn 0.524265 0.850274 0.046693 +vn 0.521127 0.848248 -0.094347 +vn 0.521127 0.848248 -0.094347 +vn 0.521127 0.848248 -0.094347 +vn 0.521127 0.848248 -0.094347 +vn 0.521127 0.848248 0.094347 +vn 0.521127 0.848248 0.094347 +vn 0.521127 0.848248 0.094346 +vn 0.521127 0.848248 0.094347 +vn 0.090218 0.955033 0.282440 +vn 0.080965 0.951518 0.296745 +vn 0.493255 0.803255 0.333887 +vn 0.493255 0.803255 0.333887 +vn 0.493255 0.803255 0.333887 +vn 0.493255 0.803255 0.333887 +vn 0.102044 0.959117 0.263971 +vn 0.089030 0.996029 0.000000 +vn 0.079187 0.996860 0.000000 +vn 0.080989 0.952672 -0.293015 +vn 0.090239 0.956082 -0.278863 +vn 0.102059 0.960038 -0.260596 +vn 0.099608 0.995027 -0.000000 +vn 0.493255 0.803256 -0.333886 +vn 0.493255 0.803256 -0.333886 +vn 0.493255 0.803256 -0.333886 +vn 0.493255 0.803256 -0.333886 +vn 0.524265 0.850274 -0.046693 +vn 0.524265 0.850274 -0.046693 +vn 0.524265 0.850274 -0.046693 +vn 0.524265 0.850274 -0.046693 +vn 0.502968 0.815395 -0.286627 +vn 0.502968 0.815395 -0.286627 +vn 0.502968 0.815395 -0.286627 +vn 0.502968 0.815395 -0.286627 +vn -0.893202 0.449656 0.000002 +vn -0.893202 0.449656 0.000002 +vn -0.893202 0.449656 0.000002 +s off +f 77/85/1 78/86/2 79/87/3 80/88/4 +f 81/89/5 82/90/6 83/91/7 84/92/8 +s 1 +f 6/20/9 7/21/10 21/36/11 20/35/12 +f 7/21/10 8/22/13 22/37/14 21/36/11 +f 8/22/13 9/23/15 23/38/16 22/37/14 +f 9/23/15 10/24/17 24/39/18 23/38/16 +f 10/24/17 11/25/19 25/40/20 24/39/18 +f 11/25/19 12/26/21 26/41/22 25/40/20 +f 12/26/21 13/27/23 27/42/24 26/41/22 +f 13/27/23 14/28/25 28/43/26 27/42/24 +f 14/28/25 1/29/27 15/44/28 28/43/26 +f 20/35/12 21/36/11 35/51/29 34/50/30 +f 21/36/11 22/37/14 36/52/31 35/51/29 +f 22/37/14 23/38/16 37/53/32 36/52/31 +f 23/38/16 24/39/18 38/54/33 37/53/32 +f 24/39/18 25/40/20 39/55/34 38/54/33 +f 25/40/20 26/41/22 40/56/35 39/55/34 +f 26/41/22 27/42/24 41/57/36 40/56/35 +f 27/42/24 28/43/26 42/58/37 41/57/36 +f 35/51/29 36/52/31 50/67/38 49/66/39 +f 36/52/31 37/53/32 51/68/40 50/67/38 +f 37/53/32 38/54/33 52/69/41 51/68/40 +f 38/54/33 39/55/34 53/70/42 52/69/41 +f 39/55/34 40/56/35 54/71/43 53/70/42 +f 40/56/35 41/57/36 55/72/44 54/71/43 +s off +f 97/105/45 85/93/46 94/102/47 93/101/48 +f 95/103/49 86/94/50 99/107/51 98/106/52 +f 100/108/53 89/97/54 103/111/55 102/110/56 +s 1 +f 49/66/39 50/67/38 64/164/57 63/160/58 +f 50/67/38 51/68/40 65/139/59 64/164/57 +f 51/68/40 52/69/41 66/141/60 65/139/59 +f 52/69/41 53/70/42 67/143/61 66/141/60 +f 53/70/42 54/71/43 68/145/62 67/143/61 +f 54/71/43 55/72/44 69/148/63 68/145/62 +f 65/139/59 66/141/60 135/174/64 133/170/65 +f 66/141/60 67/143/61 137/177/66 135/174/64 +f 67/143/61 68/145/62 139/180/67 137/177/66 +f 68/145/62 69/148/63 141/183/68 139/180/67 +f 2/2/69 1/1/27 71/78/70 +f 3/3/71 2/2/69 71/78/70 +f 5/5/72 4/4/73 71/78/70 +f 6/6/9 5/5/72 71/78/70 +f 7/7/10 6/6/9 71/78/70 +f 8/8/13 7/7/10 71/78/70 +f 9/9/15 8/8/13 71/78/70 +f 10/10/17 9/9/15 71/78/70 +f 11/11/19 10/10/17 71/78/70 +f 12/12/21 11/11/19 71/78/70 +f 13/13/23 12/12/21 71/78/70 +f 14/14/25 13/13/23 71/78/70 +f 1/1/27 14/14/25 71/78/70 +f 1/15/27 2/16/69 78/86/74 77/85/75 +s off +f 2/16/76 16/31/77 79/87/78 78/86/79 +f 16/31/80 15/30/81 80/88/82 79/87/83 +s 1 +f 15/30/28 1/15/27 77/85/84 80/88/85 +f 5/19/72 6/20/9 82/90/86 81/89/87 +f 6/20/9 20/35/12 83/91/88 82/90/89 +f 20/35/12 19/34/90 84/92/91 83/91/92 +s off +f 19/34/93 5/19/94 81/89/95 84/92/96 +f 94/102/97 86/94/98 95/103/99 93/101/100 +f 95/103/101 87/95/102 96/104/103 93/101/104 +f 96/104/105 88/96/106 97/105/107 93/101/108 +f 99/107/109 89/97/110 100/108/111 98/106/112 +f 100/108/113 90/98/114 101/109/115 98/106/116 +f 101/109/117 87/95/118 95/103/119 98/106/120 +f 103/111/121 91/99/122 104/112/123 102/110/124 +f 104/112/125 92/100/126 105/113/127 102/110/128 +f 105/113/129 90/98/130 100/108/131 102/110/132 +f 121/127/133 90/98/134 105/113/135 128/134/136 +f 105/113/137 92/100/138 127/133/139 128/134/140 +s 1 +f 48/65/141 34/50/30 35/51/29 49/66/39 +s off +f 125/131/142 127/133/143 92/100/144 104/112/145 +f 128/134/146 129/150/147 60/167/148 121/127/149 +s 1 +f 62/161/150 61/77/151 126/132/152 48/65/141 +f 63/160/58 62/161/150 48/65/141 49/66/39 +f 148/194/153 146/213/154 61/77/151 62/161/150 +s off +f 128/134/155 127/133/156 61/77/157 129/150/158 +f 126/132/159 61/77/160 127/133/161 125/131/162 +s 1 +f 159/207/163 157/197/164 63/160/58 64/164/57 +f 133/170/65 159/207/163 64/164/57 65/139/59 +s off +f 119/126/165 101/109/166 90/98/167 121/127/168 +f 121/127/169 60/167/170 120/168/171 119/126/172 +s 1 +f 63/160/58 157/197/164 148/194/153 62/161/150 +f 129/150/173 145/190/174 161/209/175 60/167/176 +f 150/214/177 153/200/178 72/166/179 +f 153/200/178 155/186/180 72/166/179 +f 156/162/181 158/163/182 72/166/179 +f 158/163/182 132/140/183 72/166/179 +f 132/140/183 134/142/184 72/166/179 +f 134/142/184 136/144/185 72/166/179 +f 136/144/185 138/146/186 72/166/179 +f 138/146/186 140/185/187 72/166/179 +f 140/185/187 143/203/188 72/166/179 +f 143/203/188 150/214/177 72/166/179 +s off +f 110/118/189 113/120/190 59/159/191 111/137/192 +f 96/104/193 87/95/194 113/120/195 110/118/196 +f 120/168/197 59/159/198 113/120/199 119/126/200 +f 119/126/201 113/120/202 87/95/203 101/109/204 +s 1 +f 163/205/205 154/158/206 59/159/207 120/168/208 +f 155/186/180 162/169/209 72/166/179 +f 160/165/210 144/151/211 72/166/179 +f 144/151/211 147/152/212 72/166/179 +f 147/152/212 156/162/181 72/166/179 +f 154/158/206 152/138/213 111/137/214 59/159/207 +f 161/209/175 163/205/205 120/168/208 60/167/176 +f 72/166/179 162/169/209 160/165/210 +f 55/72/44 41/57/36 42/58/37 56/73/215 +f 69/148/63 55/72/44 56/73/215 70/149/216 +f 142/147/217 141/183/68 69/148/63 70/149/216 +s off +f 112/119/218 58/76/219 115/122/220 114/121/221 +f 114/121/222 97/105/223 88/96/224 112/119/225 +s 1 +f 151/155/226 149/156/227 57/75/228 58/76/229 +f 111/137/214 152/138/213 151/155/226 58/76/229 +f 149/153/227 142/147/217 70/149/216 57/154/228 +s off +f 110/118/230 111/137/231 58/76/232 112/119/233 +f 112/119/234 88/96/235 96/104/236 110/118/237 +s 1 +f 29/45/238 15/30/28 16/31/239 30/46/240 +f 42/58/37 28/43/26 15/44/28 29/59/238 +f 30/46/240 107/115/241 44/61/242 +s off +f 106/114/243 109/117/244 44/61/245 107/115/246 +f 108/116/247 86/94/248 94/102/249 106/114/250 +f 94/102/251 85/93/252 109/117/253 106/114/254 +s 1 +f 44/61/242 43/60/255 29/45/238 30/46/240 +f 43/74/255 56/73/215 42/58/37 29/59/238 +f 115/122/256 58/76/229 57/75/228 43/60/255 +f 57/154/228 70/149/216 56/73/215 43/74/255 +f 115/122/256 44/61/242 109/117/257 114/121/258 +s off +f 114/121/259 109/117/260 85/93/261 97/105/262 +s 1 +f 115/122/256 43/60/255 44/61/242 +f 33/49/263 19/34/90 20/35/12 34/50/30 +f 123/129/264 33/49/263 47/64/265 +f 48/65/141 47/64/265 33/49/263 34/50/30 +f 124/130/266 47/64/265 126/132/152 125/131/267 +s off +f 122/128/268 123/129/269 47/64/270 124/130/271 +f 124/130/272 91/99/273 103/111/274 122/128/275 +f 125/131/276 104/112/277 91/99/278 124/130/279 +s 1 +f 48/65/141 126/132/152 47/64/265 +s off +f 16/31/280 2/16/281 3/17/282 17/32/283 +f 73/81/284 74/79/285 75/80/286 +f 18/33/287 4/18/288 5/19/289 19/34/290 +f 74/84/291 73/82/292 76/83/293 +f 4/18/294 18/33/295 75/80/296 74/79/297 +f 130/135/298 75/80/299 18/33/300 +f 75/80/301 17/32/302 3/17/303 73/81/304 +f 3/3/305 71/78/306 76/83/307 73/82/308 +f 76/83/309 71/78/310 4/4/311 74/84/312 +f 108/116/313 45/62/314 117/124/315 116/123/316 +f 116/123/317 118/125/318 89/97/319 99/107/320 +f 116/123/321 99/107/322 86/94/323 108/116/324 +s 1 +f 31/47/325 30/46/240 16/31/239 17/32/326 +s off +f 106/114/327 107/115/328 45/62/329 108/116/330 +s 1 +f 107/115/241 30/46/240 31/47/325 45/62/331 +f 131/136/332 130/135/333 18/33/334 32/48/335 +f 33/49/263 32/48/335 18/33/334 19/34/90 +f 32/48/335 46/63/336 117/124/337 131/136/332 +s off +f 122/128/338 118/125/339 46/63/340 123/129/341 +f 117/124/342 46/63/343 118/125/344 116/123/345 +f 103/111/346 89/97/347 118/125/348 122/128/349 +s 1 +f 32/48/335 33/49/263 123/129/264 46/63/336 +f 131/136/332 117/124/337 45/62/331 31/47/325 +f 130/135/333 131/136/332 31/47/325 17/32/326 +s off +f 75/80/350 130/135/351 17/32/352 +s 1 +f 146/213/154 145/190/174 129/150/173 61/77/151 +f 133/170/65 132/171/183 158/172/182 159/207/163 +f 132/173/183 133/170/65 135/174/64 134/175/184 +f 134/176/184 135/174/64 137/177/66 136/178/185 +f 136/179/185 137/177/66 139/180/67 138/181/186 +f 138/182/186 139/180/67 141/183/68 140/184/187 +f 140/202/187 141/183/68 142/147/217 143/188/188 +f 143/216/188 142/147/217 149/153/227 150/199/177 +f 145/190/174 144/191/211 160/192/210 161/209/175 +f 144/193/211 146/213/154 148/194/153 147/195/212 +f 147/196/212 148/194/153 157/197/164 156/198/181 +f 150/189/177 149/156/227 151/155/226 153/215/178 +f 153/187/178 152/138/213 154/158/206 155/157/180 +f 155/204/180 154/158/206 163/205/205 162/211/209 +f 156/206/181 157/197/164 159/207/163 158/208/182 +f 161/209/175 160/210/210 162/211/209 163/205/205 +f 144/212/211 145/190/174 146/213/154 +f 151/155/226 152/138/213 153/201/178 +v -4.526886 2.025292 -6.500000 +v -4.526886 2.025292 6.500000 +v -2.453123 1.990687 -6.229743 +v -2.453123 1.990687 6.229743 +v -4.716847 1.835833 -6.500000 +v -4.716847 1.835833 6.500000 +v -2.453123 1.835833 6.229743 +v -2.453123 1.835833 -6.229743 +v -4.526886 2.025292 5.055555 +v -2.453123 1.835833 5.055555 +v -4.853122 1.835833 5.055555 +v -4.526886 2.025292 3.611111 +v -2.453123 1.990687 3.611111 +v -2.453123 1.835833 3.611111 +v -4.853122 1.835833 3.611111 +v -4.526886 2.025292 2.166667 +v -2.453123 1.835833 2.166667 +v -4.853123 1.835833 2.166667 +v -4.526886 2.025292 0.722222 +v -2.453123 1.990687 0.722222 +v -2.453123 1.835833 0.722222 +v -4.853123 1.835833 0.722222 +v -4.526886 2.025292 -0.722222 +v -2.453123 1.990687 -0.722222 +v -2.453123 1.835833 -0.722222 +v -4.853123 1.835833 -0.722222 +v -4.526886 2.025292 -2.166667 +v -2.453123 1.835833 -2.166667 +v -4.853123 1.835833 -2.166667 +v -4.526886 2.025292 -3.611112 +v -2.453123 1.990687 -3.611112 +v -2.453123 1.835833 -3.611112 +v -4.853123 1.835833 -3.611112 +v -4.526886 2.025292 -5.055555 +v -2.453123 1.835833 -5.055556 +v -4.853123 1.835833 -5.055556 +vt 0.616150 0.306151 +vt 0.610251 0.992489 +vt 0.731520 0.325838 +vt 0.725886 0.974777 +vt 0.591047 0.313683 +vt 0.585284 0.984520 +vt 0.476020 0.976054 +vt 0.481594 0.320254 +vt 0.605471 0.315123 +vt 0.599736 0.983336 +vt 0.717845 0.974434 +vt 0.723467 0.326044 +vt 0.579649 0.910291 +vt 0.724217 0.318026 +vt 0.720447 0.913504 +vt 0.472481 0.914051 +vt 0.598060 0.909419 +vt 0.578999 0.836511 +vt 0.718456 0.982458 +vt 0.730267 0.838710 +vt 0.471391 0.837979 +vt 0.722220 0.838546 +vt 0.598433 0.836444 +vt 0.579018 0.761838 +vt 0.602520 0.389018 +vt 0.723365 0.763360 +vt 0.470415 0.761959 +vt 0.598696 0.761928 +vt 0.579544 0.686680 +vt 0.476998 0.382224 +vt 0.732302 0.688095 +vt 0.471265 0.685937 +vt 0.724215 0.688011 +vt 0.599234 0.686836 +vt 0.580196 0.611444 +vt 0.725022 0.387043 +vt 0.732967 0.612676 +vt 0.471912 0.610366 +vt 0.724881 0.612612 +vt 0.599876 0.611634 +vt 0.580951 0.536288 +vt 0.584126 0.387830 +vt 0.725344 0.537251 +vt 0.472346 0.534318 +vt 0.600626 0.536533 +vt 0.582223 0.461596 +vt 0.601648 0.461993 +vt 0.733547 0.462028 +vt 0.474637 0.458282 +vt 0.725502 0.462044 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.050020 0.997007 0.058958 +vn 0.050020 0.997007 0.058958 +vn 0.050020 0.997007 0.058958 +vn 0.050020 0.997007 0.058958 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.122841 -0.067773 -0.990110 +vn 0.122841 -0.067773 -0.990110 +vn 0.122841 -0.067773 -0.990110 +vn 0.122841 -0.067773 -0.990110 +vn 0.050020 0.997006 -0.058958 +vn 0.050020 0.997007 -0.058958 +vn 0.050020 0.997007 -0.058958 +vn 0.050020 0.997007 -0.058958 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.591512 0.805814 0.027903 +vn -0.591512 0.805814 0.027903 +vn -0.591511 0.805814 0.027903 +vn -0.591512 0.805814 0.027903 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502197 0.864753 0.000000 +vn -0.502197 0.864753 0.000000 +vn -0.502197 0.864753 0.000000 +vn -0.502197 0.864753 0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +s off +f 168/225/353 171/228/354 198/252/355 199/241/356 +f 164/221/357 197/258/358 198/246/359 166/224/360 +f 164/221/361 168/225/362 199/241/363 197/258/364 +f 165/218/365 169/226/366 170/227/367 167/235/368 +f 198/252/369 171/228/370 166/219/371 +f 166/230/372 171/228/373 168/225/374 164/217/375 +f 172/229/376 165/222/377 167/223/378 173/232/379 +f 167/220/380 170/227/381 173/231/382 +f 174/233/383 173/231/384 170/227/385 169/226/386 +f 172/229/387 174/233/388 169/226/389 165/222/390 +f 175/234/391 172/229/392 173/232/393 176/237/394 +f 173/231/395 177/238/396 176/236/397 +f 178/239/398 177/238/399 173/231/400 174/233/401 +f 175/234/402 178/239/403 174/233/404 172/229/405 +f 179/240/406 175/234/407 176/237/408 180/243/409 +f 176/236/410 177/238/411 180/242/412 +f 181/244/413 180/242/414 177/238/415 178/239/416 +f 179/240/417 181/244/418 178/239/419 175/234/420 +f 182/245/421 179/240/422 180/243/423 183/248/424 +f 180/242/425 184/249/426 183/247/427 +f 185/250/428 184/249/429 180/242/430 181/244/431 +f 182/245/432 185/250/433 181/244/434 179/240/435 +f 186/251/436 182/245/437 183/248/438 187/254/439 +f 183/247/440 184/249/441 188/255/442 187/253/443 +f 189/256/444 188/255/445 184/249/446 185/250/447 +f 186/251/448 189/256/449 185/250/450 182/245/451 +f 190/257/452 186/251/453 187/254/454 191/260/455 +f 187/253/456 188/255/457 191/259/458 +f 192/261/459 191/259/460 188/255/461 189/256/462 +f 190/257/463 192/261/464 189/256/465 186/251/466 +f 193/262/467 190/257/468 191/260/469 194/265/470 +f 191/259/471 195/266/472 194/264/473 +f 196/263/474 195/266/475 191/259/476 192/261/477 +f 193/262/478 196/263/479 192/261/480 190/257/481 +f 197/258/482 193/262/483 194/265/484 198/246/485 +f 194/264/486 195/266/487 198/252/488 +f 199/241/489 198/252/490 195/266/491 196/263/492 +f 197/258/493 199/241/494 196/263/495 193/262/496 +v -3.559415 -0.491252 -6.500000 +v -3.559415 -0.491252 6.500000 +v -1.485652 -0.525856 -6.229743 +v -1.485653 -0.525856 6.229743 +v -3.749376 -0.680711 -6.500000 +v -3.749376 -0.680711 6.500000 +v -1.485653 -0.680711 6.229743 +v -1.485652 -0.680711 -6.229743 +v -3.559415 -0.491252 5.055555 +v -1.485652 -0.680711 5.055555 +v -3.885653 -0.680711 5.055555 +v -3.559415 -0.491252 3.611110 +v -1.485652 -0.525856 3.611110 +v -1.485652 -0.680711 3.611110 +v -3.885652 -0.680711 3.611110 +v -3.559415 -0.491252 2.166667 +v -1.485652 -0.680711 2.166667 +v -3.885653 -0.680711 2.166667 +v -3.559415 -0.491252 0.722222 +v -1.485652 -0.525856 0.722222 +v -1.485652 -0.680711 0.722222 +v -3.885652 -0.680711 0.722222 +v -3.559416 -0.491252 -0.722223 +v -1.485652 -0.525856 -0.722222 +v -1.485652 -0.680711 -0.722222 +v -3.885653 -0.680711 -0.722223 +v -3.559416 -0.491252 -2.166667 +v -1.485652 -0.680711 -2.166667 +v -3.885653 -0.680711 -2.166667 +v -3.559415 -0.491252 -3.611112 +v -1.485652 -0.525856 -3.611111 +v -1.485652 -0.680711 -3.611111 +v -3.885653 -0.680711 -3.611111 +v -3.559415 -0.491252 -5.055556 +v -1.485652 -0.680711 -5.055556 +v -3.885652 -0.680711 -5.055556 +vt 0.878803 0.306151 +vt 0.872904 0.992489 +vt 0.994173 0.325838 +vt 0.988539 0.974777 +vt 0.853700 0.313683 +vt 0.847937 0.984520 +vt 0.738673 0.976054 +vt 0.744247 0.320254 +vt 0.868124 0.315123 +vt 0.862389 0.983336 +vt 0.980498 0.974434 +vt 0.986120 0.326044 +vt 0.842302 0.910291 +vt 0.986870 0.318026 +vt 0.983099 0.913504 +vt 0.735134 0.914051 +vt 0.860713 0.909419 +vt 0.841652 0.836511 +vt 0.981109 0.982458 +vt 0.992920 0.838710 +vt 0.734044 0.837979 +vt 0.984873 0.838546 +vt 0.861086 0.836444 +vt 0.841671 0.761838 +vt 0.865172 0.389018 +vt 0.986018 0.763360 +vt 0.733068 0.761959 +vt 0.861349 0.761928 +vt 0.842197 0.686680 +vt 0.739650 0.382224 +vt 0.994955 0.688095 +vt 0.733917 0.685937 +vt 0.986868 0.688011 +vt 0.861887 0.686836 +vt 0.842849 0.611444 +vt 0.987675 0.387043 +vt 0.995620 0.612676 +vt 0.734565 0.610366 +vt 0.987534 0.612612 +vt 0.862529 0.611634 +vt 0.843604 0.536288 +vt 0.846779 0.387830 +vt 0.987996 0.537251 +vt 0.734999 0.534318 +vt 0.863279 0.536533 +vt 0.844876 0.461596 +vt 0.864301 0.461993 +vt 0.996200 0.462028 +vt 0.737290 0.458282 +vt 0.988155 0.462044 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.050020 0.997007 0.058959 +vn 0.050020 0.997007 0.058959 +vn 0.050020 0.997006 0.058959 +vn 0.050020 0.997007 0.058959 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn -0.591510 0.805815 -0.027903 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 0.122841 -0.067773 0.990110 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 0.122841 -0.067772 -0.990110 +vn 0.122841 -0.067772 -0.990110 +vn 0.122841 -0.067772 -0.990110 +vn 0.122841 -0.067772 -0.990110 +vn 0.050020 0.997007 -0.058959 +vn 0.050020 0.997007 -0.058959 +vn 0.050020 0.997007 -0.058959 +vn 0.050020 0.997007 -0.058959 +vn 1.000000 0.000000 0.000001 +vn 1.000000 0.000000 0.000001 +vn 1.000000 0.000000 0.000001 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.591510 0.805815 0.027903 +vn -0.591510 0.805815 0.027903 +vn -0.591510 0.805815 0.027903 +vn -0.591510 0.805815 0.027903 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.502195 0.864754 -0.000000 +vn -0.502195 0.864754 -0.000000 +vn -0.502195 0.864754 -0.000000 +vn -0.502195 0.864754 -0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 0.016684 0.999861 0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn -0.502196 0.864754 0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 0.053867 0.997117 0.053449 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 1.000000 0.000000 -0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 0.053867 0.997117 -0.053449 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 1.000000 0.000000 0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn 0.000000 -1.000000 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +vn -0.502196 0.864754 -0.000000 +s off +f 204/275/497 207/278/498 234/302/499 235/291/500 +f 200/271/501 233/308/502 234/296/503 202/274/504 +f 200/271/505 204/275/506 235/291/507 233/308/508 +f 201/268/509 205/276/510 206/277/511 203/285/512 +f 234/302/513 207/278/514 202/269/515 +f 202/280/516 207/278/517 204/275/518 200/267/519 +f 208/279/520 201/272/521 203/273/522 209/282/523 +f 203/270/524 206/277/525 209/281/526 +f 210/283/527 209/281/528 206/277/529 205/276/530 +f 208/279/531 210/283/532 205/276/533 201/272/534 +f 211/284/535 208/279/536 209/282/537 212/287/538 +f 209/281/539 213/288/540 212/286/541 +f 214/289/542 213/288/543 209/281/544 210/283/545 +f 211/284/546 214/289/547 210/283/548 208/279/549 +f 215/290/550 211/284/551 212/287/552 216/293/553 +f 212/286/554 213/288/555 216/292/556 +f 217/294/557 216/292/558 213/288/559 214/289/560 +f 215/290/561 217/294/562 214/289/563 211/284/564 +f 218/295/565 215/290/566 216/293/567 219/298/568 +f 216/292/569 220/299/570 219/297/571 +f 221/300/572 220/299/573 216/292/574 217/294/575 +f 218/295/576 221/300/577 217/294/578 215/290/579 +f 222/301/580 218/295/581 219/298/582 223/304/583 +f 219/297/584 220/299/585 224/305/586 223/303/587 +f 225/306/588 224/305/589 220/299/590 221/300/591 +f 222/301/592 225/306/593 221/300/594 218/295/595 +f 226/307/596 222/301/597 223/304/598 227/310/599 +f 223/303/600 224/305/601 227/309/602 +f 228/311/603 227/309/604 224/305/605 225/306/606 +f 226/307/607 228/311/608 225/306/609 222/301/610 +f 229/312/611 226/307/612 227/310/613 230/315/614 +f 227/309/615 231/316/616 230/314/617 +f 232/313/618 231/316/619 227/309/620 228/311/621 +f 229/312/622 232/313/623 228/311/624 226/307/625 +f 233/308/626 229/312/627 230/315/628 234/296/629 +f 230/314/630 231/316/631 234/302/632 +f 235/291/633 234/302/634 231/316/635 232/313/636 +f 233/308/637 235/291/638 232/313/639 229/312/640 +v -4.142218 0.492005 -0.689421 +v -4.278798 0.483220 -0.711072 +v -4.229631 0.565988 -0.611412 +v -4.039907 1.892682 -1.903144 +v -4.176487 1.883898 -1.924795 +v -4.127320 1.966665 -1.825134 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.159359 -0.653124 -0.740293 +vn 0.159359 -0.653124 -0.740293 +vn 0.159359 -0.653124 -0.740293 +vn 0.159359 -0.653124 -0.740293 +vn -0.933302 0.271664 0.234831 +vn -0.933302 0.271664 0.234831 +vn -0.933302 0.271664 0.234831 +vn -0.933302 0.271664 0.234831 +vn 0.773945 0.381464 0.505464 +vn 0.773945 0.381464 0.505464 +vn 0.773945 0.381464 0.505464 +vn 0.773945 0.381464 0.505464 +s off +f 236/317/641 237/318/642 240/322/643 239/321/644 +f 237/318/645 238/319/646 241/323/647 240/322/648 +f 238/319/649 236/320/650 239/324/651 241/323/652 +v -3.239272 0.526544 -0.759048 +v -3.375609 0.514178 -0.780479 +v -3.327950 0.606955 -0.689259 +v -3.143238 1.854496 -2.131890 +v -3.279576 1.842130 -2.153321 +v -3.231915 1.934906 -2.062101 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.171460 -0.714084 -0.678738 +vn 0.171460 -0.714084 -0.678738 +vn 0.171460 -0.714084 -0.678738 +vn 0.171460 -0.714084 -0.678738 +vn -0.937813 0.279982 0.205223 +vn -0.937813 0.279982 0.205223 +vn -0.937813 0.279982 0.205223 +vn -0.937813 0.279982 0.205223 +vn 0.766343 0.434134 0.473546 +vn 0.766343 0.434134 0.473546 +vn 0.766343 0.434134 0.473546 +vn 0.766343 0.434134 0.473546 +s off +f 242/325/653 243/326/654 246/330/655 245/329/656 +f 243/326/657 244/327/658 247/331/659 246/330/660 +f 244/327/661 242/328/662 245/332/663 247/331/664 +v -3.163166 -0.512404 -4.366673 +v -3.288760 -0.570938 -4.366673 +v -3.225963 -0.541671 -4.246673 +v -4.291037 1.907626 -4.366673 +v -4.416630 1.849092 -4.366673 +v -4.353833 1.878359 -4.246673 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn -0.784961 -0.365836 0.500000 +vn -0.784961 -0.365836 0.500000 +vn -0.784961 -0.365836 0.500000 +vn -0.784961 -0.365836 0.500000 +vn 0.784961 0.365837 0.500000 +vn 0.784961 0.365837 0.500000 +vn 0.784961 0.365837 0.500000 +vn 0.784961 0.365837 0.500000 +s off +f 248/333/665 249/334/666 252/338/667 251/337/668 +f 249/334/669 250/335/670 253/339/671 252/338/672 +f 250/335/673 248/336/674 251/340/675 253/339/676 +v -2.090913 -0.552991 -4.366673 +v -2.216506 -0.611525 -4.366673 +v -2.153710 -0.582258 -4.246673 +v -3.269892 1.976702 -4.366673 +v -3.395485 1.918168 -4.366673 +v -3.332688 1.947435 -4.246673 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn -0.000005 0.000000 -1.000000 +vn -0.000005 0.000000 -1.000000 +vn -0.000005 0.000000 -1.000000 +vn -0.000005 0.000000 -1.000000 +vn -0.784960 -0.365836 0.500001 +vn -0.784960 -0.365836 0.500001 +vn -0.784960 -0.365836 0.500001 +vn -0.784960 -0.365836 0.500001 +vn 0.784962 0.365835 0.499999 +vn 0.784962 0.365835 0.499999 +vn 0.784962 0.365835 0.499999 +vn 0.784962 0.365835 0.499999 +s off +f 254/341/677 255/342/678 258/346/679 257/345/680 +f 255/342/681 256/343/682 259/347/683 258/346/684 +f 256/343/685 254/344/686 257/348/687 259/347/688 +v -4.142218 0.492005 0.689421 +v -4.278798 0.483220 0.711072 +v -4.229631 0.565988 0.611412 +v -4.039907 1.892682 1.903144 +v -4.176487 1.883898 1.924795 +v -4.127320 1.966665 1.825134 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn -0.159359 0.653124 -0.740293 +vn -0.159359 0.653124 -0.740293 +vn -0.159359 0.653124 -0.740293 +vn -0.159359 0.653124 -0.740293 +vn 0.933302 -0.271664 0.234831 +vn 0.933302 -0.271664 0.234831 +vn 0.933302 -0.271664 0.234831 +vn 0.933302 -0.271664 0.234831 +vn -0.773945 -0.381464 0.505464 +vn -0.773945 -0.381464 0.505464 +vn -0.773945 -0.381464 0.505464 +vn -0.773945 -0.381464 0.505464 +s off +f 260/349/689 261/350/690 264/354/691 263/353/692 +f 261/350/693 262/351/694 265/355/695 264/354/696 +f 262/351/697 260/352/698 263/356/699 265/355/700 +v -3.239272 0.526544 0.759048 +v -3.375609 0.514178 0.780479 +v -3.327950 0.606955 0.689259 +v -3.143239 1.854496 2.131890 +v -3.279576 1.842130 2.153321 +v -3.231915 1.934906 2.062101 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn -0.171462 0.714082 -0.678739 +vn -0.171462 0.714082 -0.678739 +vn -0.171462 0.714082 -0.678739 +vn -0.171462 0.714082 -0.678739 +vn 0.937813 -0.279982 0.205223 +vn 0.937813 -0.279982 0.205223 +vn 0.937813 -0.279982 0.205223 +vn 0.937813 -0.279982 0.205223 +vn -0.766344 -0.434134 0.473546 +vn -0.766344 -0.434133 0.473546 +vn -0.766344 -0.434134 0.473546 +vn -0.766344 -0.434133 0.473546 +s off +f 266/357/701 267/358/702 270/362/703 269/361/704 +f 267/358/705 268/359/706 271/363/707 270/362/708 +f 268/359/709 266/360/710 269/364/711 271/363/712 +v -3.163167 -0.512404 4.366673 +v -3.288760 -0.570938 4.366673 +v -3.225963 -0.541671 4.246673 +v -4.291037 1.907626 4.366672 +v -4.416630 1.849092 4.366673 +v -4.353833 1.878359 4.246673 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn 0.000000 -0.000001 -1.000000 +vn 0.784961 0.365836 0.500000 +vn 0.784961 0.365836 0.500000 +vn 0.784961 0.365836 0.500000 +vn 0.784961 0.365836 0.500000 +vn -0.784963 -0.365836 0.499998 +vn -0.784963 -0.365836 0.499998 +vn -0.784963 -0.365836 0.499998 +vn -0.784963 -0.365836 0.499998 +s off +f 272/365/713 273/366/714 276/370/715 275/369/716 +f 273/366/717 274/367/718 277/371/719 276/370/720 +f 274/367/721 272/368/722 275/372/723 277/371/724 +v -2.090913 -0.552991 4.366673 +v -2.216506 -0.611525 4.366673 +v -2.153710 -0.582258 4.246673 +v -3.269892 1.976702 4.366673 +v -3.395485 1.918168 4.366673 +v -3.332688 1.947435 4.246673 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.000005 0.000000 -1.000000 +vn 0.000005 0.000000 -1.000000 +vn 0.000005 0.000000 -1.000000 +vn 0.000005 0.000000 -1.000000 +vn 0.784961 0.365837 0.499999 +vn 0.784961 0.365837 0.499999 +vn 0.784961 0.365837 0.499999 +vn 0.784961 0.365837 0.499999 +vn -0.784962 -0.365835 0.500000 +vn -0.784962 -0.365835 0.500000 +vn -0.784962 -0.365835 0.500000 +vn -0.784962 -0.365835 0.500000 +s off +f 278/373/725 279/374/726 282/378/727 281/377/728 +f 279/374/729 280/375/730 283/379/731 282/378/732 +f 280/375/733 278/376/734 281/380/735 283/379/736 +v -5.517155 0.803553 -1.563271 +v -5.487778 -0.252819 -0.145963 +v -5.274500 -0.974333 1.464671 +v -5.274499 0.952056 -1.477533 +v -5.487779 0.252819 0.145963 +v -5.517155 -0.781276 1.576133 +v -5.303876 0.252819 0.145963 +v -5.303876 -0.252819 -0.145963 +v -5.502548 0.333527 -0.821039 +v -5.374407 0.570041 -0.684488 +v -5.289107 0.544277 -0.699363 +v -5.417247 0.307763 -0.835914 +v -5.374407 -0.585636 0.675485 +v -5.502548 -0.320803 0.828386 +v -5.417247 -0.292169 0.844918 +v -5.289107 -0.557001 0.692017 +vt 0.751894 0.271249 +vt 0.688242 0.270269 +vt 0.624590 0.269288 +vt 0.742593 0.186217 +vt 0.689223 0.206617 +vt 0.636506 0.184582 +vt 0.690203 0.142965 +vt 0.723500 0.047977 +vt 0.691184 0.079313 +vt 0.659848 0.046996 +vt 0.692165 0.015661 +vt 0.715908 0.196417 +vt 0.720068 0.270759 +vt 0.716398 0.164591 +vt 0.707342 0.063645 +vt 0.707832 0.031819 +vt 0.662864 0.195599 +vt 0.656416 0.269778 +vt 0.663355 0.163773 +vt 0.675516 0.063154 +vt 0.676006 0.031328 +vn -0.990061 0.139469 -0.018114 +vn -0.822500 0.528726 0.209628 +vn 0.317694 0.344286 -0.883481 +vn -0.317702 0.592979 -0.739893 +vn 0.822500 -0.445906 -0.353075 +vn 0.990061 -0.054049 -0.129840 +vn -0.948960 0.273223 0.157558 +vn -0.948215 -0.273253 -0.161933 +vn -0.991413 -0.129753 0.016270 +vn -0.842918 -0.500011 -0.198694 +vn 0.260994 -0.414547 0.871799 +vn -0.260998 -0.547769 0.794877 +vn 0.948215 0.276863 0.155677 +vn 0.842917 0.422079 0.333676 +vn 0.948960 -0.273060 -0.157838 +vn 0.991413 0.050787 0.120506 +s 1 +f 292/393/737 293/392/738 287/384/739 284/381/740 +f 295/395/741 292/396/737 284/388/740 +f 294/394/742 295/395/741 284/388/740 287/384/739 +f 293/392/738 294/394/742 287/384/739 +f 288/385/743 293/392/738 292/393/737 285/382/744 +f 297/397/745 296/398/746 286/383/747 289/386/748 +f 290/387/749 294/394/742 293/392/738 288/385/743 +f 298/399/750 297/397/745 289/386/748 +f 291/389/751 295/395/741 294/394/742 290/387/749 +f 299/400/752 298/399/750 289/386/748 286/390/747 +f 285/391/744 292/396/737 295/395/741 291/389/751 +f 296/401/746 299/400/752 286/390/747 +f 296/398/746 297/397/745 288/385/743 285/382/744 +f 299/400/752 296/401/746 285/391/744 291/389/751 +f 298/399/750 299/400/752 291/389/751 290/387/749 +f 297/397/745 298/399/750 290/387/749 288/385/743 +v -4.728155 0.086603 0.050000 +v -4.728154 0.086603 -0.050000 +v -4.728155 0.000000 -0.100000 +v -4.728154 -0.086603 -0.050000 +v -4.728155 -0.086603 0.050000 +v -4.728154 0.000000 0.100000 +v -5.628155 0.086603 0.050000 +v -5.628154 0.086603 -0.050000 +v -5.628155 0.000000 -0.100000 +v -5.628154 -0.086603 -0.050000 +v -5.628155 -0.086603 0.050000 +v -5.628154 0.000000 0.100000 +v -5.628154 0.000000 -0.000000 +vt 0.988488 0.024988 +vt 0.988520 0.041334 +vt 0.988552 0.057679 +vt 0.988584 0.074025 +vt 0.988616 0.090371 +vt 0.988648 0.106717 +vt 0.988680 0.123062 +vt 0.841008 0.025277 +vt 0.841040 0.041623 +vt 0.841072 0.057968 +vt 0.841104 0.074314 +vt 0.841136 0.090659 +vt 0.841168 0.107005 +vt 0.841200 0.123351 +vt 0.830926 0.091661 +vt 0.830824 0.039274 +vt 0.785404 0.013169 +vt 0.740087 0.039452 +vt 0.740190 0.091839 +vt 0.785609 0.117943 +vt 0.787602 0.065552 +vn 0.000000 1.000000 0.000000 +vn 0.000000 1.000000 0.000000 +vn 0.000000 1.000000 0.000000 +vn 0.000000 1.000000 0.000000 +vn -0.000000 0.499997 -0.866027 +vn -0.000000 0.499997 -0.866027 +vn -0.000000 0.499997 -0.866027 +vn -0.000000 0.499997 -0.866027 +vn 0.000000 -0.499999 -0.866026 +vn 0.000000 -0.499999 -0.866026 +vn 0.000000 -0.499999 -0.866026 +vn 0.000000 -0.499999 -0.866026 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -1.000000 0.000000 +vn 0.000000 -0.499997 0.866027 +vn 0.000000 -0.499997 0.866027 +vn 0.000000 -0.499997 0.866027 +vn 0.000000 -0.499997 0.866027 +vn 0.000000 0.499999 0.866026 +vn 0.000000 0.499999 0.866026 +vn 0.000000 0.499999 0.866026 +vn 0.000000 0.499999 0.866026 +vn -1.000000 -0.000007 -0.000005 +vn -1.000000 0.000003 -0.000002 +vn -1.000000 0.000001 -0.000002 +vn -1.000000 0.000003 0.000005 +vn -1.000000 0.000000 -0.000002 +vn -1.000000 0.000007 -0.000005 +vn -1.000000 0.000000 0.000000 +s off +f 300/402/753 301/403/754 307/410/755 306/409/756 +f 301/403/757 302/404/758 308/411/759 307/410/760 +f 302/404/761 303/405/762 309/412/763 308/411/764 +f 303/405/765 304/406/766 310/413/767 309/412/768 +f 304/406/769 305/407/770 311/414/771 310/413/772 +f 305/407/773 300/408/774 306/415/775 311/414/776 +s 1 +f 306/420/777 307/419/778 312/422/779 +f 307/419/778 308/418/780 312/422/779 +f 308/418/780 309/417/781 312/422/779 +f 309/417/781 310/416/782 312/422/779 +f 310/416/782 311/421/783 312/422/779 +f 311/421/783 306/420/777 312/422/779 +v -3.373292 -2.034549 -1.257066 +v -3.511857 -2.034549 -1.257066 +v -3.442574 -2.094549 -1.153143 +v -3.373292 -0.302498 -0.257066 +v -3.511857 -0.302498 -0.257066 +v -3.442574 -0.362498 -0.153143 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn 0.866025 0.250000 -0.433013 +vn -0.866027 0.250000 -0.433011 +vn -0.866026 0.250000 -0.433011 +vn 0.866025 0.250001 -0.433013 +vn -0.000000 -0.499999 0.866026 +vn -0.000000 -0.499999 0.866026 +s 1 +f 313/423/784 314/424/785 317/428/786 316/427/787 +f 314/424/785 315/425/788 318/429/789 317/428/786 +f 315/425/788 313/426/784 316/430/787 318/429/789 +v -3.250543 -1.665037 -1.076869 +v -3.141562 -1.815037 -1.076869 +v -3.141562 -2.000447 -1.076869 +v -3.250544 -2.150447 -1.076869 +v -3.426880 -2.207742 -1.076869 +v -3.603215 -2.150447 -1.076869 +v -3.712196 -2.000447 -1.076869 +v -3.712196 -1.815037 -1.076869 +v -3.603215 -1.665037 -1.076869 +v -3.426879 -1.607742 -1.076869 +v -3.250543 -1.665037 -1.376869 +v -3.141562 -1.815037 -1.376869 +v -3.141562 -2.000447 -1.376869 +v -3.250544 -2.150447 -1.376869 +v -3.426880 -2.207742 -1.376870 +v -3.603215 -2.150447 -1.376869 +v -3.712196 -2.000447 -1.376870 +v -3.712196 -1.815037 -1.376869 +v -3.603215 -1.665037 -1.376869 +v -3.426879 -1.607742 -1.376869 +v -3.426879 -1.907742 -1.076869 +v -3.426879 -1.907742 -1.271261 +vt 0.252891 0.122706 +vt 0.283344 0.107201 +vt 0.298868 0.076756 +vt 0.293532 0.043002 +vt 0.269375 0.018830 +vt 0.235623 0.013474 +vt 0.205170 0.028980 +vt 0.189646 0.059424 +vt 0.194982 0.093179 +vt 0.219140 0.117351 +vt 0.987153 0.141536 +vt 0.986865 0.156219 +vt 0.986578 0.170901 +vt 0.986290 0.185584 +vt 0.986003 0.200266 +vt 0.985715 0.214949 +vt 0.985428 0.229631 +vt 0.985140 0.244313 +vt 0.984853 0.258995 +vt 0.984565 0.273678 +vt 0.984278 0.288360 +vt 0.766365 0.137213 +vt 0.766077 0.151896 +vt 0.765790 0.166578 +vt 0.765503 0.181260 +vt 0.765215 0.195943 +vt 0.764927 0.210625 +vt 0.764640 0.225307 +vt 0.764352 0.239990 +vt 0.764065 0.254672 +vt 0.763777 0.269355 +vt 0.763490 0.284037 +vt 0.498215 0.317674 +vt 0.521155 0.305994 +vt 0.532848 0.283062 +vt 0.528828 0.257636 +vt 0.510632 0.239429 +vt 0.485208 0.235394 +vt 0.462269 0.247074 +vt 0.450576 0.270006 +vt 0.454595 0.295432 +vt 0.472792 0.313640 +vt 0.246228 0.069095 +vt 0.493196 0.277291 +vn 0.545745 0.751153 0.371391 +vn 0.883034 0.286915 0.371391 +vn 0.859567 0.279290 -0.427950 +vn 0.531242 0.731191 -0.427951 +vn 0.883033 -0.286916 0.371391 +vn 0.859567 -0.279291 -0.427951 +vn 0.545744 -0.751154 0.371391 +vn 0.531242 -0.731191 -0.427951 +vn -0.000000 -0.928477 0.371390 +vn -0.000000 -0.903802 -0.427952 +vn -0.545746 -0.751153 0.371391 +vn -0.531243 -0.731191 -0.427950 +vn -0.883034 -0.286914 0.371391 +vn -0.859567 -0.279289 -0.427952 +vn -0.883034 0.286915 0.371390 +vn -0.859567 0.279290 -0.427951 +vn -0.545745 0.751153 0.371391 +vn -0.531242 0.731191 -0.427951 +vn 0.000000 0.928477 0.371390 +vn 0.000000 0.903802 -0.427951 +vn -0.000000 -0.000000 1.000000 +vn 0.000000 0.000000 -1.000000 +s 1 +f 319/441/790 320/442/791 330/453/792 329/452/793 +f 320/442/791 321/443/794 331/454/795 330/453/792 +f 321/443/794 322/444/796 332/455/797 331/454/795 +f 322/444/796 323/445/798 333/456/799 332/455/797 +f 323/445/798 324/446/800 334/457/801 333/456/799 +f 324/446/800 325/447/802 335/458/803 334/457/801 +f 325/447/802 326/448/804 336/459/805 335/458/803 +f 326/448/804 327/449/806 337/460/807 336/459/805 +f 327/449/806 328/450/808 338/461/809 337/460/807 +f 328/450/808 319/451/790 329/462/793 338/461/809 +f 320/432/791 319/431/790 339/473/810 +f 321/433/794 320/432/791 339/473/810 +f 322/434/796 321/433/794 339/473/810 +f 323/435/798 322/434/796 339/473/810 +f 324/436/800 323/435/798 339/473/810 +f 325/437/802 324/436/800 339/473/810 +f 326/438/804 325/437/802 339/473/810 +f 327/439/806 326/438/804 339/473/810 +f 328/440/808 327/439/806 339/473/810 +f 319/431/790 328/440/808 339/473/810 +f 329/471/793 330/470/792 340/474/811 +f 330/470/792 331/469/795 340/474/811 +f 331/469/795 332/468/797 340/474/811 +f 332/468/797 333/467/799 340/474/811 +f 333/467/799 334/466/801 340/474/811 +f 334/466/801 335/465/803 340/474/811 +f 335/465/803 336/464/805 340/474/811 +f 336/464/805 337/463/807 340/474/811 +f 337/463/807 338/472/809 340/474/811 +f 338/472/809 329/471/793 340/474/811 +v -3.373292 -2.034549 1.257066 +v -3.511857 -2.034549 1.257066 +v -3.442574 -2.094549 1.153143 +v -3.373292 -0.302498 0.257066 +v -3.511857 -0.302498 0.257066 +v -3.442574 -0.362498 0.153143 +vt 0.475306 0.012993 +vt 0.524059 0.012993 +vt 0.572811 0.012993 +vt 0.621564 0.012993 +vt 0.475306 0.232930 +vt 0.524059 0.232930 +vt 0.572811 0.232930 +vt 0.621564 0.232930 +vn -0.866025 -0.250000 -0.433013 +vn 0.866027 -0.249999 -0.433011 +vn 0.866026 -0.249999 -0.433011 +vn -0.866025 -0.250001 -0.433014 +vn 0.000000 0.500000 0.866025 +vn -0.000000 0.500000 0.866026 +s 1 +f 341/475/812 342/476/813 345/480/814 344/479/815 +f 342/476/813 343/477/816 346/481/817 345/480/814 +f 343/477/816 341/478/812 344/482/815 346/481/817 +v -3.250543 -1.665037 1.076869 +v -3.141562 -1.815037 1.076869 +v -3.141562 -2.000447 1.076869 +v -3.250543 -2.150447 1.076869 +v -3.426879 -2.207742 1.076869 +v -3.603214 -2.150447 1.076869 +v -3.712196 -2.000447 1.076869 +v -3.712196 -1.815037 1.076869 +v -3.603214 -1.665037 1.076869 +v -3.426879 -1.607742 1.076869 +v -3.250544 -1.665037 1.376869 +v -3.141562 -1.815037 1.376869 +v -3.141562 -2.000447 1.376869 +v -3.250544 -2.150447 1.376869 +v -3.426879 -2.207742 1.376869 +v -3.603215 -2.150447 1.376869 +v -3.712196 -2.000447 1.376869 +v -3.712196 -1.815037 1.376869 +v -3.603215 -1.665037 1.376869 +v -3.426879 -1.607742 1.376869 +v -3.426879 -1.907742 1.076869 +v -3.426879 -1.907742 1.271261 +vt 0.252891 0.122706 +vt 0.283344 0.107201 +vt 0.298868 0.076756 +vt 0.293532 0.043002 +vt 0.269375 0.018830 +vt 0.235623 0.013474 +vt 0.205170 0.028980 +vt 0.189646 0.059424 +vt 0.194982 0.093179 +vt 0.219140 0.117351 +vt 0.987153 0.141536 +vt 0.986865 0.156219 +vt 0.986578 0.170901 +vt 0.986290 0.185584 +vt 0.986003 0.200266 +vt 0.985715 0.214949 +vt 0.985428 0.229631 +vt 0.985140 0.244313 +vt 0.984853 0.258995 +vt 0.984565 0.273678 +vt 0.984278 0.288360 +vt 0.766365 0.137213 +vt 0.766077 0.151896 +vt 0.765790 0.166578 +vt 0.765503 0.181260 +vt 0.765215 0.195943 +vt 0.764927 0.210625 +vt 0.764640 0.225307 +vt 0.764352 0.239990 +vt 0.764065 0.254672 +vt 0.763777 0.269355 +vt 0.763490 0.284037 +vt 0.498215 0.317674 +vt 0.521155 0.305994 +vt 0.532848 0.283062 +vt 0.528828 0.257636 +vt 0.510632 0.239429 +vt 0.485208 0.235394 +vt 0.462269 0.247074 +vt 0.450576 0.270006 +vt 0.454595 0.295432 +vt 0.472792 0.313640 +vt 0.246228 0.069095 +vt 0.493196 0.277291 +vn -0.545745 -0.751153 0.371390 +vn -0.883034 -0.286915 0.371390 +vn -0.859567 -0.279290 -0.427951 +vn -0.531241 -0.731192 -0.427951 +vn -0.883034 0.286915 0.371390 +vn -0.859567 0.279290 -0.427952 +vn -0.545745 0.751154 0.371390 +vn -0.531240 0.731191 -0.427952 +vn 0.000000 0.928477 0.371391 +vn -0.000000 0.903802 -0.427952 +vn 0.545745 0.751153 0.371392 +vn 0.531242 0.731191 -0.427951 +vn 0.883034 0.286914 0.371391 +vn 0.859567 0.279290 -0.427951 +vn 0.883034 -0.286915 0.371391 +vn 0.859567 -0.279290 -0.427951 +vn 0.545745 -0.751154 0.371391 +vn 0.531242 -0.731191 -0.427950 +vn 0.000000 -0.928477 0.371391 +vn -0.000000 -0.903802 -0.427951 +vn -0.000000 0.000000 1.000000 +vn 0.000000 0.000000 -1.000000 +s 1 +f 347/493/818 348/494/819 358/505/820 357/504/821 +f 348/494/819 349/495/822 359/506/823 358/505/820 +f 349/495/822 350/496/824 360/507/825 359/506/823 +f 350/496/824 351/497/826 361/508/827 360/507/825 +f 351/497/826 352/498/828 362/509/829 361/508/827 +f 352/498/828 353/499/830 363/510/831 362/509/829 +f 353/499/830 354/500/832 364/511/833 363/510/831 +f 354/500/832 355/501/834 365/512/835 364/511/833 +f 355/501/834 356/502/836 366/513/837 365/512/835 +f 356/502/836 347/503/818 357/514/821 366/513/837 +f 348/484/819 347/483/818 367/525/838 +f 349/485/822 348/484/819 367/525/838 +f 350/486/824 349/485/822 367/525/838 +f 351/487/826 350/486/824 367/525/838 +f 352/488/828 351/487/826 367/525/838 +f 353/489/830 352/488/828 367/525/838 +f 354/490/832 353/489/830 367/525/838 +f 355/491/834 354/490/832 367/525/838 +f 356/492/836 355/491/834 367/525/838 +f 347/483/818 356/492/836 367/525/838 +f 357/523/821 358/522/820 368/526/839 +f 358/522/820 359/521/823 368/526/839 +f 359/521/823 360/520/825 368/526/839 +f 360/520/825 361/519/827 368/526/839 +f 361/519/827 362/518/829 368/526/839 +f 362/518/829 363/517/831 368/526/839 +f 363/517/831 364/516/833 368/526/839 +f 364/516/833 365/515/835 368/526/839 +f 365/515/835 366/524/837 368/526/839 +f 366/524/837 357/523/821 368/526/839 +v 3.068942 -0.472892 0.150000 +v 3.068942 -0.658302 0.150000 +v 2.959961 -0.808302 0.150000 +v 2.783625 -0.865597 0.150000 +v 2.607290 -0.808302 0.150000 +v 2.498308 -0.658302 0.150000 +v 2.498308 -0.472892 0.150000 +v 3.068942 -0.472892 -0.150000 +v 3.068942 -0.658302 -0.150000 +v 2.959961 -0.808302 -0.150000 +v 2.783625 -0.865597 -0.150000 +v 2.607290 -0.808302 -0.150000 +v 2.498308 -0.658302 -0.150000 +v 2.498308 -0.472892 -0.150000 +v 2.783625 -0.565597 0.150000 +v 2.783625 -0.565597 -0.044391 +vt 0.283344 0.107201 +vt 0.298868 0.076756 +vt 0.293532 0.043002 +vt 0.269375 0.018830 +vt 0.235623 0.013474 +vt 0.205170 0.028980 +vt 0.189646 0.059424 +vt 0.986865 0.156219 +vt 0.986578 0.170901 +vt 0.986290 0.185584 +vt 0.986003 0.200266 +vt 0.985715 0.214949 +vt 0.985428 0.229631 +vt 0.985140 0.244313 +vt 0.766077 0.151896 +vt 0.765790 0.166578 +vt 0.765503 0.181260 +vt 0.765215 0.195943 +vt 0.764927 0.210625 +vt 0.764640 0.225307 +vt 0.764352 0.239990 +vt 0.521155 0.305994 +vt 0.532848 0.283062 +vt 0.528828 0.257636 +vt 0.510632 0.239429 +vt 0.485208 0.235394 +vt 0.462269 0.247074 +vt 0.450576 0.270006 +vt 0.246228 0.069095 +vt 0.493196 0.277291 +vn 0.934652 0.000000 0.355563 +vn 0.883034 -0.286915 0.371391 +vn 0.859567 -0.279290 -0.427951 +vn 0.911810 0.000000 -0.410612 +vn 0.545745 -0.751153 0.371391 +vn 0.531242 -0.731191 -0.427951 +vn -0.000000 -0.928477 0.371391 +vn -0.000000 -0.903802 -0.427951 +vn -0.545745 -0.751154 0.371391 +vn -0.531241 -0.731191 -0.427951 +vn -0.883034 -0.286915 0.371391 +vn -0.859567 -0.279291 -0.427951 +vn -0.934652 0.000000 0.355563 +vn -0.911810 0.000000 -0.410612 +vn 0.000000 0.000000 1.000000 +vn 0.000000 0.186532 -0.982449 +s 1 +f 369/534/840 370/535/841 377/542/842 376/541/843 +f 370/535/841 371/536/844 378/543/845 377/542/842 +f 371/536/844 372/537/846 379/544/847 378/543/845 +f 372/537/846 373/538/848 380/545/849 379/544/847 +f 373/538/848 374/539/850 381/546/851 380/545/849 +f 374/539/850 375/540/852 382/547/853 381/546/851 +f 370/528/841 369/527/840 383/555/854 +f 371/529/844 370/528/841 383/555/854 +f 372/530/846 371/529/844 383/555/854 +f 373/531/848 372/530/846 383/555/854 +f 374/532/850 373/531/848 383/555/854 +f 375/533/852 374/532/850 383/555/854 +f 376/554/843 377/553/842 384/556/855 +f 377/553/842 378/552/845 384/556/855 +f 378/552/845 379/551/847 384/556/855 +f 379/551/847 380/550/849 384/556/855 +f 380/550/849 381/549/851 384/556/855 +f 381/549/851 382/548/853 384/556/855 diff --git a/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/diffuse_512.png b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/diffuse_512.png new file mode 100644 index 00000000..930d910b Binary files /dev/null and b/Arduino/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/diffuse_512.png differ diff --git a/Arduino/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino b/Arduino/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino similarity index 97% rename from Arduino/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino rename to Arduino/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino index 28418a96..596f0750 100644 --- a/Arduino/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino +++ b/Arduino/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino @@ -48,6 +48,7 @@ THE SOFTWARE. // AD0 high = 0x69 MPU6050 accelgyro; //MPU6050 accelgyro(0x69); // <-- use for AD0 high +//MPU6050 accelgyro(0x68, &Wire1); // <-- use for AD0 low, but 2nd Wire (TWI/I2C) object int16_t ax, ay, az; int16_t gx, gy, gz; @@ -113,7 +114,7 @@ void setup() { Serial.print("\n"); */ - // configure Arduino LED for + // configure Arduino LED pin for output pinMode(LED_PIN, OUTPUT); } @@ -148,4 +149,5 @@ void loop() { // blink LED to indicate activity blinkState = !blinkState; digitalWrite(LED_PIN, blinkState); + delay(100); } diff --git a/Arduino/MPU6050/library.json b/Arduino/MPU6050/library.json new file mode 100644 index 00000000..3b5ad90a --- /dev/null +++ b/Arduino/MPU6050/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-MPU6050", + "version": "1.0.0", + "keywords": "gyroscope, accelerometer, sensor, i2cdevlib, i2c", + "description": "The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms", + "include": "Arduino/MPU6050", + "repository": + { + "type": "git", + "url": "https://github.com/jrowberg/i2cdevlib.git" + }, + "dependencies": + { + "jrowberg/I2Cdevlib-Core": "*" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/Arduino/MPU9150/Examples/MPU9150_DMP9/MPU9150_DMP9.ino b/Arduino/MPU9150/Examples/MPU9150_DMP9/MPU9150_DMP9.ino new file mode 100644 index 00000000..34852fd4 --- /dev/null +++ b/Arduino/MPU9150/Examples/MPU9150_DMP9/MPU9150_DMP9.ino @@ -0,0 +1,372 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) +// 6/21/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2015-08-03 - added Arduino 1.6 support +// 2013-05-08 - added seamless Fastwire support +// - added note about gyro calibration +// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error +// 2012-06-20 - improved FIFO overflow handling and simplified read process +// 2012-06-19 - completely rearranged DMP initialization code and simplification +// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly +// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING +// 2012-06-05 - add gravity-compensated initial reference frame acceleration output +// - add 3D math helper file to DMP6 example sketch +// - add Euler output and Yaw/Pitch/Roll output formats +// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) +// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 +// 2012-05-30 - basic DMP initialization working + +/* ============================================ +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 and MPU6050 must be installed as libraries, or else the .cpp/.h files +// for both classes must be in the include path of your project +#include "I2Cdev.h" + +#include "MPU9150_9Axis_MotionApps41.h" +//#include "MPU6050.h" // not necessary if using MotionApps include file + +// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation +// is used in I2Cdev.h +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + #include "Wire.h" +#endif + +// class default I2C address is 0x68 +// specific I2C addresses may be passed as a parameter here +// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) +// AD0 high = 0x69 +MPU9150 mpu; +//MPU6050 mpu(0x69); // <-- use for AD0 high + +/* ========================================================================= + NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch + depends on the MPU-6050's INT pin being connected to the Arduino's + external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is + digital I/O pin 2. + * ========================================================================= */ + +/* ========================================================================= + NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error + when using Serial.write(buf, len). The Teapot output uses this method. + The solution requires a modification to the Arduino USBAPI.h file, which + is fortunately simple, but annoying. This will be fixed in the next IDE + release. For more info, see these links: + + http://arduino.cc/forum/index.php/topic,109987.0.html + http://code.google.com/p/arduino/issues/detail?id=958 + * ========================================================================= */ + + + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +//#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT" if you want output that matches the +// format used for the InvenSense teapot demo +//#define OUTPUT_TEAPOT + + +#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) +bool blinkState = false; + +// MPU control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU +uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) +uint16_t packetSize; // expected DMP packet size (default is 42 bytes) +uint16_t fifoCount; // count of all bytes currently in FIFO +uint8_t fifoBuffer[64]; // FIFO storage buffer + +// orientation/motion vars +Quaternion q; // [w, x, y, z] quaternion container +VectorInt16 aa; // [x, y, z] accel sensor measurements +VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements +VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements +VectorFloat gravity; // [x, y, z] gravity vector +float euler[3]; // [psi, theta, phi] Euler angle container +float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector + +// packet structure for InvenSense teapot demo +uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; + + + +// ================================================================ +// === INTERRUPT DETECTION ROUTINE === +// ================================================================ + +volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high +void dmpDataReady() { + mpuInterrupt = true; +} + + + +// ================================================================ +// === INITIAL SETUP === +// ================================================================ + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + // initialize serial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + Serial.begin(115200); + while (!Serial); // wait for Leonardo enumeration, others continue immediately + + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio + // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to + // the baud timing being too misaligned with processor ticks. You must use + // 38400 or slower in these cases, or use some kind of external separate + // crystal solution for the UART timer. + + // initialize device + Serial.println(F("Initializing I2C devices...")); + mpu.initialize(); + + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + + // wait for ready + Serial.println(F("\nSend any character to begin DMP programming and demo: ")); + // while (Serial.available() && Serial.read()); // empty buffer + // while (!Serial.available()); // wait for data + // while (Serial.available() && Serial.read()); // empty buffer again + + // load and configure the DMP + Serial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + // mpu.setXGyroOffset(220); + // mpu.setYGyroOffset(76); + // mpu.setZGyroOffset(-85); + // mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); + + // enable Arduino interrupt detection + Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); + attachInterrupt(0, dmpDataReady, RISING); + mpuIntStatus = mpu.getIntStatus(); + + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; + + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } + + // configure LED for output + pinMode(LED_PIN, OUTPUT); +} + + + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + + // wait for MPU interrupt or extra packet(s) available + while (!mpuInterrupt && fifoCount < packetSize) { + // other program behavior stuff here + // . + // . + // . + // if you are really paranoid you can frequently test in between other + // stuff to see if mpuInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the MPU data + // . + // . + // . + } + + // reset interrupt flag and get INT_STATUS byte + mpuInterrupt = false; + mpuIntStatus = mpu.getIntStatus(); + + // get current FIFO count + fifoCount = mpu.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mpuIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + mpu.resetFIFO(); + Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mpuIntStatus & 0x02) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); + + // read a packet from FIFO + mpu.getFIFOBytes(fifoBuffer, packetSize); + + // track FIFO count here in case there is > 1 packet available + // (this lets us immediately read more without waiting for an interrupt) + fifoCount -= packetSize; + + #ifdef OUTPUT_READABLE_QUATERNION + // display quaternion values in easy matrix form: w x y z + mpu.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print(q.w); + Serial.print("\t"); + Serial.print(q.x); + Serial.print("\t"); + Serial.print(q.y); + Serial.print("\t"); + Serial.println(q.z); + #endif + + #ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetEuler(euler, &q); + Serial.print("euler\t"); + Serial.print(euler[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(euler[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(euler[2] * 180/M_PI); + #endif + + #ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + Serial.print("ypr\t"); + Serial.print(ypr[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(ypr[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(ypr[2] * 180/M_PI); + #endif + + #ifdef OUTPUT_READABLE_REALACCEL + // display real acceleration, adjusted to remove gravity + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + Serial.print("areal\t"); + Serial.print(aaReal.x); + Serial.print("\t"); + Serial.print(aaReal.y); + Serial.print("\t"); + Serial.println(aaReal.z); + #endif + + #ifdef OUTPUT_READABLE_WORLDACCEL + // display initial world-frame acceleration, adjusted to remove gravity + // and rotated based on known orientation from quaternion + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetAccel(&aa, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); + mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); + Serial.print("aworld\t"); + Serial.print(aaWorld.x); + Serial.print("\t"); + Serial.print(aaWorld.y); + Serial.print("\t"); + Serial.println(aaWorld.z); + #endif + + #ifdef OUTPUT_TEAPOT + // display quaternion values in InvenSense Teapot demo format: + teapotPacket[2] = fifoBuffer[0]; + teapotPacket[3] = fifoBuffer[1]; + teapotPacket[4] = fifoBuffer[4]; + teapotPacket[5] = fifoBuffer[5]; + teapotPacket[6] = fifoBuffer[8]; + teapotPacket[7] = fifoBuffer[9]; + teapotPacket[8] = fifoBuffer[12]; + teapotPacket[9] = fifoBuffer[13]; + Serial.write(teapotPacket, 14); + teapotPacket[11]++; // packetCount, loops at 0xFF on purpose + #endif + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + } +} diff --git a/Arduino/MPU9150/MPU9150.cpp b/Arduino/MPU9150/MPU9150.cpp new file mode 100644 index 00000000..a5f25d9c --- /dev/null +++ b/Arduino/MPU9150/MPU9150.cpp @@ -0,0 +1,3157 @@ +// I2Cdev library collection - MPU9150 I2C device class +// Based on InvenSense MPU-9150 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 8/24/2011 by Jeff Rowberg +// 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 "MPU9150.h" + +/** Default constructor, uses default I2C address. + * @see MPU9150_DEFAULT_ADDRESS + */ +MPU9150::MPU9150() { + devAddr = MPU9150_DEFAULT_ADDRESS; +} + +/** Specific address constructor. + * @param address I2C address + * @see MPU9150_DEFAULT_ADDRESS + * @see MPU9150_ADDRESS_AD0_LOW + * @see MPU9150_ADDRESS_AD0_HIGH + */ +MPU9150::MPU9150(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 MPU9150::initialize() { + setClockSource(MPU9150_CLOCK_PLL_XGYRO); + setFullScaleGyroRange(MPU9150_GYRO_FS_250); + setFullScaleAccelRange(MPU9150_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 MPU9150::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 MPU9150::getAuxVDDIOLevel() { + I2Cdev::readBit(devAddr, MPU9150_RA_YG_OFFS_TC, MPU9150_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 MPU9150::setAuxVDDIOLevel(uint8_t level) { + I2Cdev::writeBit(devAddr, MPU9150_RA_YG_OFFS_TC, MPU9150_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-9150 Product Specification document. + * + * @return Current sample rate + * @see MPU9150_RA_SMPLRT_DIV + */ +uint8_t MPU9150::getRate() { + I2Cdev::readByte(devAddr, MPU9150_RA_SMPLRT_DIV, buffer); + return buffer[0]; +} +/** Set gyroscope sample rate divider. + * @param rate New sample rate divider + * @see getRate() + * @see MPU9150_RA_SMPLRT_DIV + */ +void MPU9150::setRate(uint8_t rate) { + I2Cdev::writeByte(devAddr, MPU9150_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. + * + * 
+ * 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]
+ *
+ * + * @return FSYNC configuration value + */ +uint8_t MPU9150::getExternalFrameSync() { + I2Cdev::readBits(devAddr, MPU9150_RA_CONFIG, MPU9150_CFG_EXT_SYNC_SET_BIT, MPU9150_CFG_EXT_SYNC_SET_LENGTH, buffer); + return buffer[0]; +} +/** Set external FSYNC configuration. + * @see getExternalFrameSync() + * @see MPU9150_RA_CONFIG + * @param sync New FSYNC configuration value + */ +void MPU9150::setExternalFrameSync(uint8_t sync) { + I2Cdev::writeBits(devAddr, MPU9150_RA_CONFIG, MPU9150_CFG_EXT_SYNC_SET_BIT, MPU9150_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. + * + * 
+ *          |   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
+ *
+ * + * @return DLFP configuration + * @see MPU9150_RA_CONFIG + * @see MPU9150_CFG_DLPF_CFG_BIT + * @see MPU9150_CFG_DLPF_CFG_LENGTH + */ +uint8_t MPU9150::getDLPFMode() { + I2Cdev::readBits(devAddr, MPU9150_RA_CONFIG, MPU9150_CFG_DLPF_CFG_BIT, MPU9150_CFG_DLPF_CFG_LENGTH, buffer); + return buffer[0]; +} +/** Set digital low-pass filter configuration. + * @param mode New DLFP configuration setting + * @see getDLPFBandwidth() + * @see MPU9150_DLPF_BW_256 + * @see MPU9150_RA_CONFIG + * @see MPU9150_CFG_DLPF_CFG_BIT + * @see MPU9150_CFG_DLPF_CFG_LENGTH + */ +void MPU9150::setDLPFMode(uint8_t mode) { + I2Cdev::writeBits(devAddr, MPU9150_RA_CONFIG, MPU9150_CFG_DLPF_CFG_BIT, MPU9150_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. + * + * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ *
+ * + * @return Current full-scale gyroscope range setting + * @see MPU9150_GYRO_FS_250 + * @see MPU9150_RA_GYRO_CONFIG + * @see MPU9150_GCONFIG_FS_SEL_BIT + * @see MPU9150_GCONFIG_FS_SEL_LENGTH + */ +uint8_t MPU9150::getFullScaleGyroRange() { + I2Cdev::readBits(devAddr, MPU9150_RA_GYRO_CONFIG, MPU9150_GCONFIG_FS_SEL_BIT, MPU9150_GCONFIG_FS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale gyroscope range. + * @param range New full-scale gyroscope range value + * @see getFullScaleRange() + * @see MPU9150_GYRO_FS_250 + * @see MPU9150_RA_GYRO_CONFIG + * @see MPU9150_GCONFIG_FS_SEL_BIT + * @see MPU9150_GCONFIG_FS_SEL_LENGTH + */ +void MPU9150::setFullScaleGyroRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU9150_RA_GYRO_CONFIG, MPU9150_GCONFIG_FS_SEL_BIT, MPU9150_GCONFIG_FS_SEL_LENGTH, range); +} + +// ACCEL_CONFIG register + +/** Get self-test enabled setting for accelerometer X axis. + * @return Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +bool MPU9150::getAccelXSelfTest() { + I2Cdev::readBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_XA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled setting for accelerometer X axis. + * @param enabled Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +void MPU9150::setAccelXSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_XA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Y axis. + * @return Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +bool MPU9150::getAccelYSelfTest() { + I2Cdev::readBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_YA_ST_BIT, buffer); + return buffer[0]; +} +/** Get self-test enabled value for accelerometer Y axis. + * @param enabled Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +void MPU9150::setAccelYSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_YA_ST_BIT, enabled); +} +/** Get self-test enabled value for accelerometer Z axis. + * @return Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +bool MPU9150::getAccelZSelfTest() { + I2Cdev::readBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_ZA_ST_BIT, buffer); + return buffer[0]; +} +/** Set self-test enabled value for accelerometer Z axis. + * @param enabled Self-test enabled value + * @see MPU9150_RA_ACCEL_CONFIG + */ +void MPU9150::setAccelZSelfTest(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_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. + * + * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ *
+ * + * @return Current full-scale accelerometer range setting + * @see MPU9150_ACCEL_FS_2 + * @see MPU9150_RA_ACCEL_CONFIG + * @see MPU9150_ACONFIG_AFS_SEL_BIT + * @see MPU9150_ACONFIG_AFS_SEL_LENGTH + */ +uint8_t MPU9150::getFullScaleAccelRange() { + I2Cdev::readBits(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_AFS_SEL_BIT, MPU9150_ACONFIG_AFS_SEL_LENGTH, buffer); + return buffer[0]; +} +/** Set full-scale accelerometer range. + * @param range New full-scale accelerometer range setting + * @see getFullScaleAccelRange() + */ +void MPU9150::setFullScaleAccelRange(uint8_t range) { + I2Cdev::writeBits(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_AFS_SEL_BIT, MPU9150_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-9150 Product Specification document). + * + * The high pass filter has three modes: + * + * 
+ *    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.
+ *
+ * + * 
+ * 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
+ *
+ * + * @return Current high-pass filter configuration + * @see MPU9150_DHPF_RESET + * @see MPU9150_RA_ACCEL_CONFIG + */ +uint8_t MPU9150::getDHPFMode() { + I2Cdev::readBits(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_ACCEL_HPF_BIT, MPU9150_ACONFIG_ACCEL_HPF_LENGTH, buffer); + return buffer[0]; +} +/** Set the high-pass filter configuration. + * @param bandwidth New high-pass filter configuration + * @see setDHPFMode() + * @see MPU9150_DHPF_RESET + * @see MPU9150_RA_ACCEL_CONFIG + */ +void MPU9150::setDHPFMode(uint8_t bandwidth) { + I2Cdev::writeBits(devAddr, MPU9150_RA_ACCEL_CONFIG, MPU9150_ACONFIG_ACCEL_HPF_BIT, MPU9150_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-9150 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current free-fall acceleration threshold value (LSB = 2mg) + * @see MPU9150_RA_FF_THR + */ +uint8_t MPU9150::getFreefallDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_FF_THR + */ +void MPU9150::setFreefallDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU9150_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-9150 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current free-fall duration threshold value (LSB = 1ms) + * @see MPU9150_RA_FF_DUR + */ +uint8_t MPU9150::getFreefallDetectionDuration() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_FF_DUR + */ +void MPU9150::setFreefallDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU9150_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-9150 Product Specification document as well as Registers 56 and + * 58 of this document. + * + * @return Current motion detection acceleration threshold value (LSB = 2mg) + * @see MPU9150_RA_MOT_THR + */ +uint8_t MPU9150::getMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU9150_RA_MOT_THR, buffer); + return buffer[0]; +} +/** Set free-fall event acceleration threshold. + * @param threshold New motion detection acceleration threshold value (LSB = 2mg) + * @see getMotionDetectionThreshold() + * @see MPU9150_RA_MOT_THR + */ +void MPU9150::setMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU9150_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-9150 Product Specification document. + * + * @return Current motion detection duration threshold value (LSB = 1ms) + * @see MPU9150_RA_MOT_DUR + */ +uint8_t MPU9150::getMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_MOT_DUR + */ +void MPU9150::setMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU9150_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-9150 Product Specification document as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection acceleration threshold value (LSB = 2mg) + * @see MPU9150_RA_ZRMOT_THR + */ +uint8_t MPU9150::getZeroMotionDetectionThreshold() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_ZRMOT_THR + */ +void MPU9150::setZeroMotionDetectionThreshold(uint8_t threshold) { + I2Cdev::writeByte(devAddr, MPU9150_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-9150 Product Specification document, as well as Registers 56 + * and 58 of this document. + * + * @return Current zero motion detection duration threshold value (LSB = 64ms) + * @see MPU9150_RA_ZRMOT_DUR + */ +uint8_t MPU9150::getZeroMotionDetectionDuration() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_ZRMOT_DUR + */ +void MPU9150::setZeroMotionDetectionDuration(uint8_t duration) { + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getTempFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_TEMP_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set temperature FIFO enabled value. + * @param enabled New temperature FIFO enabled value + * @see getTempFIFOEnabled() + * @see MPU9150_RA_FIFO_EN + */ +void MPU9150::setTempFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getXGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setXGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getYGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setYGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getZGyroFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setZGyroFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getAccelFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_ACCEL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set accelerometer FIFO enabled value. + * @param enabled New accelerometer FIFO enabled value + * @see getAccelFIFOEnabled() + * @see MPU9150_RA_FIFO_EN + */ +void MPU9150::setAccelFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getSlave2FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setSlave2FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getSlave1FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setSlave1FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +bool MPU9150::getSlave0FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_FIFO_EN + */ +void MPU9150::setSlave0FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_FIFO_EN, MPU9150_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 MPU9150_RA_I2C_MST_CTRL + */ +bool MPU9150::getMultiMasterEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_MULT_MST_EN_BIT, buffer); + return buffer[0]; +} +/** Set multi-master enabled value. + * @param enabled New multi-master enabled value + * @see getMultiMasterEnabled() + * @see MPU9150_RA_I2C_MST_CTRL + */ +void MPU9150::setMultiMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_CTRL + */ +bool MPU9150::getWaitForExternalSensorEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_CTRL + */ +void MPU9150::setWaitForExternalSensorEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_MST_CTRL + */ +bool MPU9150::getSlave3FIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_MST_CTRL + */ +void MPU9150::setSlave3FIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_CTRL + */ +bool MPU9150::getSlaveReadWriteTransitionEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_CTRL + */ +void MPU9150::setSlaveReadWriteTransitionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_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: + * + * 
+ * 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
+ *
+ * + * @return Current I2C master clock speed + * @see MPU9150_RA_I2C_MST_CTRL + */ +uint8_t MPU9150::getMasterClockSpeed() { + I2Cdev::readBits(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_I2C_MST_CLK_BIT, MPU9150_I2C_MST_CLK_LENGTH, buffer); + return buffer[0]; +} +/** Set I2C master clock speed. + * @reparam speed Current I2C master clock speed + * @see MPU9150_RA_I2C_MST_CTRL + */ +void MPU9150::setMasterClockSpeed(uint8_t speed) { + I2Cdev::writeBits(devAddr, MPU9150_RA_I2C_MST_CTRL, MPU9150_I2C_MST_CLK_BIT, MPU9150_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-9150 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 MPU9150_RA_I2C_SLV0_ADDR + */ +uint8_t MPU9150::getSlaveAddress(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV0_ADDR + */ +void MPU9150::setSlaveAddress(uint8_t num, uint8_t address) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU9150_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-9150 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 MPU9150_RA_I2C_SLV0_REG + */ +uint8_t MPU9150::getSlaveRegister(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV0_REG + */ +void MPU9150::setSlaveRegister(uint8_t num, uint8_t reg) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +bool MPU9150::getSlaveEnabled(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +void MPU9150::setSlaveEnabled(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +bool MPU9150::getSlaveWordByteSwap(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +void MPU9150::setSlaveWordByteSwap(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +bool MPU9150::getSlaveWriteMode(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +void MPU9150::setSlaveWriteMode(uint8_t num, bool mode) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +bool MPU9150::getSlaveWordGroupOffset(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +void MPU9150::setSlaveWordGroupOffset(uint8_t num, bool enabled) { + if (num > 3) return; + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +uint8_t MPU9150::getSlaveDataLength(uint8_t num) { + if (num > 3) return 0; + I2Cdev::readBits(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_I2C_SLV_LEN_BIT, MPU9150_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 MPU9150_RA_I2C_SLV0_CTRL + */ +void MPU9150::setSlaveDataLength(uint8_t num, uint8_t length) { + if (num > 3) return; + I2Cdev::writeBits(devAddr, MPU9150_RA_I2C_SLV0_CTRL + num*3, MPU9150_I2C_SLV_LEN_BIT, MPU9150_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 MPU9150_RA_I2C_SLV4_ADDR + */ +uint8_t MPU9150::getSlave4Address() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV4_ADDR + */ +void MPU9150::setSlave4Address(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV4_REG + */ +uint8_t MPU9150::getSlave4Register() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV4_REG + */ +void MPU9150::setSlave4Register(uint8_t reg) { + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV4_DO + */ +void MPU9150::setSlave4OutputByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +bool MPU9150::getSlave4Enabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +void MPU9150::setSlave4Enabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +bool MPU9150::getSlave4InterruptEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +void MPU9150::setSlave4InterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +bool MPU9150::getSlave4WriteMode() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +void MPU9150::setSlave4WriteMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +uint8_t MPU9150::getSlave4MasterDelay() { + I2Cdev::readBits(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_I2C_SLV4_MST_DLY_BIT, MPU9150_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 MPU9150_RA_I2C_SLV4_CTRL + */ +void MPU9150::setSlave4MasterDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU9150_RA_I2C_SLV4_CTRL, MPU9150_I2C_SLV4_MST_DLY_BIT, MPU9150_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 MPU9150_RA_I2C_SLV4_DI + */ +uint8_t MPU9150::getSlate4InputByte() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getPassthroughStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave4IsDone() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getLostArbitration() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave4Nack() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave3Nack() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave2Nack() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave1Nack() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_I2C_MST_STATUS + */ +bool MPU9150::getSlave0Nack() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_STATUS, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_LEVEL_BIT + */ +bool MPU9150::getInterruptMode() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_LEVEL_BIT + */ +void MPU9150::setInterruptMode(bool mode) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_OPEN_BIT + */ +bool MPU9150::getInterruptDrive() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_OPEN_BIT + */ +void MPU9150::setInterruptDrive(bool drive) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_LATCH_INT_EN_BIT + */ +bool MPU9150::getInterruptLatch() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_LATCH_INT_EN_BIT + */ +void MPU9150::setInterruptLatch(bool latch) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_RD_CLEAR_BIT + */ +bool MPU9150::getInterruptLatchClear() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_INT_RD_CLEAR_BIT + */ +void MPU9150::setInterruptLatchClear(bool clear) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_FSYNC_INT_LEVEL_BIT + */ +bool MPU9150::getFSyncInterruptLevel() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_FSYNC_INT_LEVEL_BIT + */ +void MPU9150::setFSyncInterruptLevel(bool level) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_FSYNC_INT_EN_BIT + */ +bool MPU9150::getFSyncInterruptEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_FSYNC_INT_EN_BIT + */ +void MPU9150::setFSyncInterruptEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_I2C_BYPASS_EN_BIT + */ +bool MPU9150::getI2CBypassEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_I2C_BYPASS_EN_BIT + */ +void MPU9150::setI2CBypassEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_CLKOUT_EN_BIT + */ +bool MPU9150::getClockOutputEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_PIN_CFG + * @see MPU9150_INTCFG_CLKOUT_EN_BIT + */ +void MPU9150::setClockOutputEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_PIN_CFG, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FF_BIT + **/ +uint8_t MPU9150::getIntEnabled() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FF_BIT + **/ +void MPU9150::setIntEnabled(uint8_t enabled) { + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FF_BIT + **/ +bool MPU9150::getIntFreefallEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_FF_BIT, buffer); + return buffer[0]; +} +/** Set Free Fall interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFreefallEnabled() + * @see MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FF_BIT + **/ +void MPU9150::setIntFreefallEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_MOT_BIT + **/ +bool MPU9150::getIntMotionEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_MOT_BIT, buffer); + return buffer[0]; +} +/** Set Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntMotionEnabled() + * @see MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_MOT_BIT + **/ +void MPU9150::setIntMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_ZMOT_BIT + **/ +bool MPU9150::getIntZeroMotionEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_ZMOT_BIT, buffer); + return buffer[0]; +} +/** Set Zero Motion Detection interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntZeroMotionEnabled() + * @see MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_ZMOT_BIT + **/ +void MPU9150::setIntZeroMotionEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FIFO_OFLOW_BIT + **/ +bool MPU9150::getIntFIFOBufferOverflowEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_FIFO_OFLOW_BIT, buffer); + return buffer[0]; +} +/** Set FIFO Buffer Overflow interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntFIFOBufferOverflowEnabled() + * @see MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_FIFO_OFLOW_BIT + **/ +void MPU9150::setIntFIFOBufferOverflowEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_I2C_MST_INT_BIT + **/ +bool MPU9150::getIntI2CMasterEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_I2C_MST_INT_BIT, buffer); + return buffer[0]; +} +/** Set I2C Master interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntI2CMasterEnabled() + * @see MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_I2C_MST_INT_BIT + **/ +void MPU9150::setIntI2CMasterEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_ENABLE + * @see MPU9150_INTERRUPT_DATA_RDY_BIT + */ +bool MPU9150::getIntDataReadyEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_DATA_RDY_BIT, buffer); + return buffer[0]; +} +/** Set Data Ready interrupt enabled status. + * @param enabled New interrupt enabled status + * @see getIntDataReadyEnabled() + * @see MPU9150_RA_INT_CFG + * @see MPU9150_INTERRUPT_DATA_RDY_BIT + */ +void MPU9150::setIntDataReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_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 MPU9150_RA_INT_STATUS + */ +uint8_t MPU9150::getIntStatus() { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_FF_BIT + */ +bool MPU9150::getIntFreefallStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_MOT_BIT + */ +bool MPU9150::getIntMotionStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_ZMOT_BIT + */ +bool MPU9150::getIntZeroMotionStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_FIFO_OFLOW_BIT + */ +bool MPU9150::getIntFIFOBufferOverflowStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_I2C_MST_INT_BIT + */ +bool MPU9150::getIntI2CMasterStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_INT_STATUS + * @see MPU9150_INTERRUPT_DATA_RDY_BIT + */ +bool MPU9150::getIntDataReadyStatus() { + I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_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 MPU9150_RA_ACCEL_XOUT_H + */ +void MPU9150::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) { + + //get accel and gyro + getMotion6(ax, ay, az, gx, gy, gz); + + //read mag + I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer + delay(10); + I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer + delay(10); + I2Cdev::readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer); + //Measurement data is stored in two's complement and Little Endian format. Measurement range of each axis is from -4096 to +4095 in decimal. + *mx = (int16_t)(((uint16_t)buffer[1] << 8) | (uint16_t)buffer[0]); + *my = (int16_t)(((uint16_t)buffer[3] << 8) | (uint16_t)buffer[2]); + *mz = (int16_t)(((uint16_t)buffer[5] << 8) | (uint16_t)buffer[4]); +} +/** 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 MPU9150_RA_ACCEL_XOUT_H + */ +void MPU9150::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) { + I2Cdev::readBytes(devAddr, MPU9150_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: + * + * 
+ * 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
+ *
+ * + * @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 MPU9150_RA_GYRO_XOUT_H + */ +void MPU9150::getAcceleration(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_ACCEL_XOUT_H + */ +int16_t MPU9150::getAccelerationX() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_ACCEL_YOUT_H + */ +int16_t MPU9150::getAccelerationY() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_ACCEL_ZOUT_H + */ +int16_t MPU9150::getAccelerationZ() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_TEMP_OUT_H + */ +int16_t MPU9150::getTemperature() { + I2Cdev::readBytes(devAddr, MPU9150_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: + * + * 
+ * 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
+ *
+ * + * @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 MPU9150_RA_GYRO_XOUT_H + */ +void MPU9150::getRotation(int16_t* x, int16_t* y, int16_t* z) { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_GYRO_XOUT_H + */ +int16_t MPU9150::getRotationX() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_GYRO_YOUT_H + */ +int16_t MPU9150::getRotationY() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150_RA_GYRO_ZOUT_H + */ +int16_t MPU9150::getRotationZ() { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150::getExternalSensorByte(int position) { + I2Cdev::readByte(devAddr, MPU9150_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 MPU9150::getExternalSensorWord(int position) { + I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150::getExternalSensorDWord(int position) { + I2Cdev::readBytes(devAddr, MPU9150_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 X-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_XNEG_BIT + */ +bool MPU9150::getXNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_XNEG_BIT, buffer); + return buffer[0]; +} +/** Get X-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_XPOS_BIT + */ +bool MPU9150::getXPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_XPOS_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_YNEG_BIT + */ +bool MPU9150::getYNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_YNEG_BIT, buffer); + return buffer[0]; +} +/** Get Y-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_YPOS_BIT + */ +bool MPU9150::getYPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_YPOS_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis negative motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_ZNEG_BIT + */ +bool MPU9150::getZNegMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_ZNEG_BIT, buffer); + return buffer[0]; +} +/** Get Z-axis positive motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_ZPOS_BIT + */ +bool MPU9150::getZPosMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_MOTION_MOT_ZPOS_BIT, buffer); + return buffer[0]; +} +/** Get zero motion detection interrupt status. + * @return Motion detection status + * @see MPU9150_RA_MOT_DETECT_STATUS + * @see MPU9150_MOTION_MOT_ZRMOT_BIT + */ +bool MPU9150::getZeroMotionDetected() { + I2Cdev::readBit(devAddr, MPU9150_RA_MOT_DETECT_STATUS, MPU9150_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 MPU9150_RA_I2C_SLV0_DO + */ +void MPU9150::setSlaveOutputByte(uint8_t num, uint8_t data) { + if (num > 3) return; + I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_I2C_MST_DELAY_CTRL + * @see MPU9150_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +bool MPU9150::getExternalShadowDelayEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_I2C_MST_DELAY_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_DELAY_CTRL + * @see MPU9150_DELAYCTRL_DELAY_ES_SHADOW_BIT + */ +void MPU9150::setExternalShadowDelayEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_I2C_MST_DELAY_CTRL, MPU9150_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 MPU9150_RA_I2C_MST_DELAY_CTRL + * @see MPU9150_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +bool MPU9150::getSlaveDelayEnabled(uint8_t num) { + // MPU9150_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc. + if (num > 4) return 0; + I2Cdev::readBit(devAddr, MPU9150_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 MPU9150_RA_I2C_MST_DELAY_CTRL + * @see MPU9150_DELAYCTRL_I2C_SLV0_DLY_EN_BIT + */ +void MPU9150::setSlaveDelayEnabled(uint8_t num, bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_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 MPU9150_RA_SIGNAL_PATH_RESET + * @see MPU9150_PATHRESET_GYRO_RESET_BIT + */ +void MPU9150::resetGyroscopePath() { + I2Cdev::writeBit(devAddr, MPU9150_RA_SIGNAL_PATH_RESET, MPU9150_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 MPU9150_RA_SIGNAL_PATH_RESET + * @see MPU9150_PATHRESET_ACCEL_RESET_BIT + */ +void MPU9150::resetAccelerometerPath() { + I2Cdev::writeBit(devAddr, MPU9150_RA_SIGNAL_PATH_RESET, MPU9150_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 MPU9150_RA_SIGNAL_PATH_RESET + * @see MPU9150_PATHRESET_TEMP_RESET_BIT + */ +void MPU9150::resetTemperaturePath() { + I2Cdev::writeBit(devAddr, MPU9150_RA_SIGNAL_PATH_RESET, MPU9150_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-9150 Product Specification document for + * further information regarding the detection modules. + * @return Current accelerometer power-on delay + * @see MPU9150_RA_MOT_DETECT_CTRL + * @see MPU9150_DETECT_ACCEL_ON_DELAY_BIT + */ +uint8_t MPU9150::getAccelerometerPowerOnDelay() { + I2Cdev::readBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_ACCEL_ON_DELAY_BIT, MPU9150_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 MPU9150_RA_MOT_DETECT_CTRL + * @see MPU9150_DETECT_ACCEL_ON_DELAY_BIT + */ +void MPU9150::setAccelerometerPowerOnDelay(uint8_t delay) { + I2Cdev::writeBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_ACCEL_ON_DELAY_BIT, MPU9150_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: + * + * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ *
+ * + * 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 MPU9150_RA_MOT_DETECT_CTRL + * @see MPU9150_DETECT_FF_COUNT_BIT + */ +uint8_t MPU9150::getFreefallDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_FF_COUNT_BIT, MPU9150_DETECT_FF_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Free Fall detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getFreefallDetectionCounterDecrement() + * @see MPU9150_RA_MOT_DETECT_CTRL + * @see MPU9150_DETECT_FF_COUNT_BIT + */ +void MPU9150::setFreefallDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_FF_COUNT_BIT, MPU9150_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: + * + * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ *
+ * + * 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 MPU9150::getMotionDetectionCounterDecrement() { + I2Cdev::readBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_MOT_COUNT_BIT, MPU9150_DETECT_MOT_COUNT_LENGTH, buffer); + return buffer[0]; +} +/** Set Motion detection counter decrement configuration. + * @param decrement New decrement configuration value + * @see getMotionDetectionCounterDecrement() + * @see MPU9150_RA_MOT_DETECT_CTRL + * @see MPU9150_DETECT_MOT_COUNT_BIT + */ +void MPU9150::setMotionDetectionCounterDecrement(uint8_t decrement) { + I2Cdev::writeBits(devAddr, MPU9150_RA_MOT_DETECT_CTRL, MPU9150_DETECT_MOT_COUNT_BIT, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_FIFO_EN_BIT + */ +bool MPU9150::getFIFOEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_USERCTRL_FIFO_EN_BIT, buffer); + return buffer[0]; +} +/** Set FIFO enabled status. + * @param enabled New FIFO enabled status + * @see getFIFOEnabled() + * @see MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_FIFO_EN_BIT + */ +void MPU9150::setFIFOEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_I2C_MST_EN_BIT + */ +bool MPU9150::getI2CMasterModeEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_I2C_MST_EN_BIT + */ +void MPU9150::setI2CMasterModeEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150::switchSPIEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_FIFO_RESET_BIT + */ +void MPU9150::resetFIFO() { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_I2C_MST_RESET_BIT + */ +void MPU9150::resetI2CMaster() { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_USER_CTRL + * @see MPU9150_USERCTRL_SIG_COND_RESET_BIT + */ +void MPU9150::resetSensors() { + I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_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 MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_DEVICE_RESET_BIT + */ +void MPU9150::reset() { + I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_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 MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_SLEEP_BIT + */ +bool MPU9150::getSleepEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_PWR1_SLEEP_BIT, buffer); + return buffer[0]; +} +/** Set sleep mode status. + * @param enabled New sleep mode enabled status + * @see getSleepEnabled() + * @see MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_SLEEP_BIT + */ +void MPU9150::setSleepEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_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 MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_CYCLE_BIT + */ +bool MPU9150::getWakeCycleEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_PWR1_CYCLE_BIT, buffer); + return buffer[0]; +} +/** Set wake cycle enabled status. + * @param enabled New sleep mode enabled status + * @see getWakeCycleEnabled() + * @see MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_CYCLE_BIT + */ +void MPU9150::setWakeCycleEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_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 MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_TEMP_DIS_BIT + */ +bool MPU9150::getTempSensorEnabled() { + I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_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 MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_TEMP_DIS_BIT + */ +void MPU9150::setTempSensorEnabled(bool enabled) { + // 1 is actually disabled here + I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_PWR1_TEMP_DIS_BIT, !enabled); +} +/** Get clock source setting. + * @return Current clock source setting + * @see MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_CLKSEL_BIT + * @see MPU9150_PWR1_CLKSEL_LENGTH + */ +uint8_t MPU9150::getClockSource() { + I2Cdev::readBits(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_PWR1_CLKSEL_BIT, MPU9150_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: + * + * 
+ * 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
+ *
+ * + * @param source New clock source setting + * @see getClockSource() + * @see MPU9150_RA_PWR_MGMT_1 + * @see MPU9150_PWR1_CLKSEL_BIT + * @see MPU9150_PWR1_CLKSEL_LENGTH + */ +void MPU9150::setClockSource(uint8_t source) { + I2Cdev::writeBits(devAddr, MPU9150_RA_PWR_MGMT_1, MPU9150_PWR1_CLKSEL_BIT, MPU9150_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: + * + * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * For further information regarding the MPU-60X0's power modes, please refer to
+ * Register 107.
+ *
+ * @return Current wake frequency
+ * @see MPU9150_RA_PWR_MGMT_2
+ */
+uint8_t MPU9150::getWakeFrequency() {
+    I2Cdev::readBits(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_PWR2_LP_WAKE_CTRL_BIT, MPU9150_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set wake frequency in Accel-Only Low Power Mode.
+ * @param frequency New wake frequency
+ * @see MPU9150_RA_PWR_MGMT_2
+ */
+void MPU9150::setWakeFrequency(uint8_t frequency) {
+    I2Cdev::writeBits(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_PWR2_LP_WAKE_CTRL_BIT, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_XA_BIT
+ */
+bool MPU9150::getStandbyXAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_XA_BIT
+ */
+void MPU9150::setStandbyXAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_YA_BIT
+ */
+bool MPU9150::getStandbyYAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_YA_BIT
+ */
+void MPU9150::setStandbyYAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_ZA_BIT
+ */
+bool MPU9150::getStandbyZAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_ZA_BIT
+ */
+void MPU9150::setStandbyZAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_XG_BIT
+ */
+bool MPU9150::getStandbyXGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_XG_BIT
+ */
+void MPU9150::setStandbyXGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_YG_BIT
+ */
+bool MPU9150::getStandbyYGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_YG_BIT
+ */
+void MPU9150::setStandbyYGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_ZG_BIT
+ */
+bool MPU9150::getStandbyZGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150_RA_PWR_MGMT_2
+ * @see MPU9150_PWR2_STBY_ZG_BIT
+ */
+void MPU9150::setStandbyZGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_PWR_MGMT_2, MPU9150_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 MPU9150::getFIFOCount() {
+    I2Cdev::readBytes(devAddr, MPU9150_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 MPU9150::getFIFOByte() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_FIFO_R_W, buffer);
+    return buffer[0];
+}
+void MPU9150::getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO buffer.
+ * @see getFIFOByte()
+ * @see MPU9150_RA_FIFO_R_W
+ */
+void MPU9150::setFIFOByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU9150_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 MPU9150_RA_WHO_AM_I
+ * @see MPU9150_WHO_AM_I_BIT
+ * @see MPU9150_WHO_AM_I_LENGTH
+ */
+uint8_t MPU9150::getDeviceID() {
+    I2Cdev::readBits(devAddr, MPU9150_RA_WHO_AM_I, MPU9150_WHO_AM_I_BIT, MPU9150_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 MPU9150_RA_WHO_AM_I
+ * @see MPU9150_WHO_AM_I_BIT
+ * @see MPU9150_WHO_AM_I_LENGTH
+ */
+void MPU9150::setDeviceID(uint8_t id) {
+    I2Cdev::writeBits(devAddr, MPU9150_RA_WHO_AM_I, MPU9150_WHO_AM_I_BIT, MPU9150_WHO_AM_I_LENGTH, id);
+}
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+
+uint8_t MPU9150::getOTPBankValid() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_XG_OFFS_TC, MPU9150_TC_OTP_BNK_VLD_BIT, buffer);
+    return buffer[0];
+}
+void MPU9150::setOTPBankValid(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_XG_OFFS_TC, MPU9150_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU9150::getXGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU9150_RA_XG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9150::setXGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9150_RA_XG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, offset);
+}
+
+// YG_OFFS_TC register
+
+int8_t MPU9150::getYGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU9150_RA_YG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9150::setYGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9150_RA_YG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, offset);
+}
+
+// ZG_OFFS_TC register
+
+int8_t MPU9150::getZGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU9150_RA_ZG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9150::setZGyroOffsetTC(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9150_RA_ZG_OFFS_TC, MPU9150_TC_OFFSET_BIT, MPU9150_TC_OFFSET_LENGTH, offset);
+}
+
+// X_FINE_GAIN register
+
+int8_t MPU9150::getXFineGain() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_X_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9150::setXFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU9150::getYFineGain() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_Y_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9150::setYFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU9150::getZFineGain() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_Z_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9150::setZFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU9150::getXAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_XA_OFFS_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setXAccelOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU9150::getYAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_YA_OFFS_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setYAccelOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU9150::getZAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_ZA_OFFS_H, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setZAccelOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU9150::getXGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_XG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setXGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU9150::getYGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_YG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setYGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU9150::getZGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU9150_RA_ZG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9150::setZGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9150_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU9150::getIntPLLReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+    return buffer[0];
+}
+void MPU9150::setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU9150::getIntDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_DMP_INT_BIT, buffer);
+    return buffer[0];
+}
+void MPU9150::setIntDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_INT_ENABLE, MPU9150_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU9150::getDMPInt5Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_5_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getDMPInt4Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_4_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getDMPInt3Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_3_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getDMPInt2Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_2_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getDMPInt1Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_1_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getDMPInt0Status() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_DMP_INT_STATUS, MPU9150_DMPINT_0_BIT, buffer);
+    return buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU9150::getIntPLLReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9150::getIntDMPStatus() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_INT_STATUS, MPU9150_INTERRUPT_DMP_INT_BIT, buffer);
+    return buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU9150::getDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_USERCTRL_DMP_EN_BIT, buffer);
+    return buffer[0];
+}
+void MPU9150::setDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU9150::resetDMP() {
+    I2Cdev::writeBit(devAddr, MPU9150_RA_USER_CTRL, MPU9150_USERCTRL_DMP_RESET_BIT, true);
+}
+
+// BANK_SEL register
+
+void MPU9150::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
+    bank &= 0x1F;
+    if (userBank) bank |= 0x20;
+    if (prefetchEnabled) bank |= 0x40;
+    I2Cdev::writeByte(devAddr, MPU9150_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU9150::setMemoryStartAddress(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU9150::readMemoryByte() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_MEM_R_W, buffer);
+    return buffer[0];
+}
+void MPU9150::writeMemoryByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_MEM_R_W, data);
+}
+void MPU9150::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 = MPU9150_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, MPU9150_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 MPU9150::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;
+    uint8_t *progBuffer;
+    uint16_t i;
+    uint8_t j;
+    if (verify) verifyBuffer = (uint8_t *)malloc(MPU9150_DMP_MEMORY_CHUNK_SIZE);
+    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU9150_DMP_MEMORY_CHUNK_SIZE);
+    else progBuffer = NULL;
+    for (i = 0; i < dataSize;) {
+        // determine correct chunk size according to bank position and data size
+        chunkSize = MPU9150_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, MPU9150_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            I2Cdev::readBytes(devAddr, MPU9150_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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 MPU9150::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
+    return writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU9150::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, success, special;
+    uint16_t i, j;
+    if (useProgMem) {
+        progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary
+    } else {
+        progBuffer = NULL;
+    }
+
+    // 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, MPU9150_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 MPU9150::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
+    return writeDMPConfigurationSet(data, dataSize, true);
+}
+
+// DMP_CFG_1 register
+
+uint8_t MPU9150::getDMPConfig1() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_DMP_CFG_1, buffer);
+    return buffer[0];
+}
+void MPU9150::setDMPConfig1(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU9150::getDMPConfig2() {
+    I2Cdev::readByte(devAddr, MPU9150_RA_DMP_CFG_2, buffer);
+    return buffer[0];
+}
+void MPU9150::setDMPConfig2(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU9150_RA_DMP_CFG_2, config);
+}
diff --git a/Arduino/MPU9150/MPU9150.h b/Arduino/MPU9150/MPU9150.h
new file mode 100644
index 00000000..90ee1cc4
--- /dev/null
+++ b/Arduino/MPU9150/MPU9150.h
@@ -0,0 +1,1038 @@
+// I2Cdev library collection - MPU9150 I2C device class
+// Based on InvenSense MPU-9150 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 10/3/2011 by Jeff Rowberg 
+// 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 _MPU9150_H_
+#define _MPU9150_H_
+
+#include "I2Cdev.h"
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifdef __AVR__
+    #include 
+#else
+    // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+    #ifndef __PGMSPACE_H_
+        #define __PGMSPACE_H_ 1
+        #include 
+
+        #define PROGMEM
+        #define PGM_P  const char *
+        #define PSTR(str) (str)
+        #define F(x) x
+
+        typedef void prog_void;
+        typedef char prog_char;
+        typedef unsigned char prog_uchar;
+        typedef int8_t prog_int8_t;
+        typedef uint8_t prog_uint8_t;
+        typedef int16_t prog_int16_t;
+        typedef uint16_t prog_uint16_t;
+        typedef int32_t prog_int32_t;
+        typedef uint32_t prog_uint32_t;
+
+        #define strcpy_P(dest, src) strcpy((dest), (src))
+        #define strcat_P(dest, src) strcat((dest), (src))
+        #define strcmp_P(a, b) strcmp((a), (b))
+
+        #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+        #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+        #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+        #define pgm_read_float(addr) (*(const float *)(addr))
+
+        #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+        #define pgm_read_word_near(addr) pgm_read_word(addr)
+        #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+        #define pgm_read_float_near(addr) pgm_read_float(addr)
+        #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+        #define pgm_read_word_far(addr) pgm_read_word(addr)
+        #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+        #define pgm_read_float_far(addr) pgm_read_float(addr)
+    #endif
+#endif
+
+//Magnetometer Registers
+#define MPU9150_RA_MAG_ADDRESS		0x0C
+#define MPU9150_RA_MAG_XOUT_L		0x03
+#define MPU9150_RA_MAG_XOUT_H		0x04
+#define MPU9150_RA_MAG_YOUT_L		0x05
+#define MPU9150_RA_MAG_YOUT_H		0x06
+#define MPU9150_RA_MAG_ZOUT_L		0x07
+#define MPU9150_RA_MAG_ZOUT_H		0x08
+
+#define MPU9150_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
+#define MPU9150_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
+#define MPU9150_DEFAULT_ADDRESS     MPU9150_ADDRESS_AD0_LOW
+
+#define MPU9150_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9150_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9150_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9150_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN
+#define MPU9150_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN
+#define MPU9150_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN
+#define MPU9150_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
+#define MPU9150_RA_XA_OFFS_L_TC     0x07
+#define MPU9150_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS
+#define MPU9150_RA_YA_OFFS_L_TC     0x09
+#define MPU9150_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
+#define MPU9150_RA_ZA_OFFS_L_TC     0x0B
+#define MPU9150_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
+#define MPU9150_RA_XG_OFFS_USRL     0x14
+#define MPU9150_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR
+#define MPU9150_RA_YG_OFFS_USRL     0x16
+#define MPU9150_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR
+#define MPU9150_RA_ZG_OFFS_USRL     0x18
+#define MPU9150_RA_SMPLRT_DIV       0x19
+#define MPU9150_RA_CONFIG           0x1A
+#define MPU9150_RA_GYRO_CONFIG      0x1B
+#define MPU9150_RA_ACCEL_CONFIG     0x1C
+#define MPU9150_RA_FF_THR           0x1D
+#define MPU9150_RA_FF_DUR           0x1E
+#define MPU9150_RA_MOT_THR          0x1F
+#define MPU9150_RA_MOT_DUR          0x20
+#define MPU9150_RA_ZRMOT_THR        0x21
+#define MPU9150_RA_ZRMOT_DUR        0x22
+#define MPU9150_RA_FIFO_EN          0x23
+#define MPU9150_RA_I2C_MST_CTRL     0x24
+#define MPU9150_RA_I2C_SLV0_ADDR    0x25
+#define MPU9150_RA_I2C_SLV0_REG     0x26
+#define MPU9150_RA_I2C_SLV0_CTRL    0x27
+#define MPU9150_RA_I2C_SLV1_ADDR    0x28
+#define MPU9150_RA_I2C_SLV1_REG     0x29
+#define MPU9150_RA_I2C_SLV1_CTRL    0x2A
+#define MPU9150_RA_I2C_SLV2_ADDR    0x2B
+#define MPU9150_RA_I2C_SLV2_REG     0x2C
+#define MPU9150_RA_I2C_SLV2_CTRL    0x2D
+#define MPU9150_RA_I2C_SLV3_ADDR    0x2E
+#define MPU9150_RA_I2C_SLV3_REG     0x2F
+#define MPU9150_RA_I2C_SLV3_CTRL    0x30
+#define MPU9150_RA_I2C_SLV4_ADDR    0x31
+#define MPU9150_RA_I2C_SLV4_REG     0x32
+#define MPU9150_RA_I2C_SLV4_DO      0x33
+#define MPU9150_RA_I2C_SLV4_CTRL    0x34
+#define MPU9150_RA_I2C_SLV4_DI      0x35
+#define MPU9150_RA_I2C_MST_STATUS   0x36
+#define MPU9150_RA_INT_PIN_CFG      0x37
+#define MPU9150_RA_INT_ENABLE       0x38
+#define MPU9150_RA_DMP_INT_STATUS   0x39
+#define MPU9150_RA_INT_STATUS       0x3A
+#define MPU9150_RA_ACCEL_XOUT_H     0x3B
+#define MPU9150_RA_ACCEL_XOUT_L     0x3C
+#define MPU9150_RA_ACCEL_YOUT_H     0x3D
+#define MPU9150_RA_ACCEL_YOUT_L     0x3E
+#define MPU9150_RA_ACCEL_ZOUT_H     0x3F
+#define MPU9150_RA_ACCEL_ZOUT_L     0x40
+#define MPU9150_RA_TEMP_OUT_H       0x41
+#define MPU9150_RA_TEMP_OUT_L       0x42
+#define MPU9150_RA_GYRO_XOUT_H      0x43
+#define MPU9150_RA_GYRO_XOUT_L      0x44
+#define MPU9150_RA_GYRO_YOUT_H      0x45
+#define MPU9150_RA_GYRO_YOUT_L      0x46
+#define MPU9150_RA_GYRO_ZOUT_H      0x47
+#define MPU9150_RA_GYRO_ZOUT_L      0x48
+#define MPU9150_RA_EXT_SENS_DATA_00 0x49
+#define MPU9150_RA_EXT_SENS_DATA_01 0x4A
+#define MPU9150_RA_EXT_SENS_DATA_02 0x4B
+#define MPU9150_RA_EXT_SENS_DATA_03 0x4C
+#define MPU9150_RA_EXT_SENS_DATA_04 0x4D
+#define MPU9150_RA_EXT_SENS_DATA_05 0x4E
+#define MPU9150_RA_EXT_SENS_DATA_06 0x4F
+#define MPU9150_RA_EXT_SENS_DATA_07 0x50
+#define MPU9150_RA_EXT_SENS_DATA_08 0x51
+#define MPU9150_RA_EXT_SENS_DATA_09 0x52
+#define MPU9150_RA_EXT_SENS_DATA_10 0x53
+#define MPU9150_RA_EXT_SENS_DATA_11 0x54
+#define MPU9150_RA_EXT_SENS_DATA_12 0x55
+#define MPU9150_RA_EXT_SENS_DATA_13 0x56
+#define MPU9150_RA_EXT_SENS_DATA_14 0x57
+#define MPU9150_RA_EXT_SENS_DATA_15 0x58
+#define MPU9150_RA_EXT_SENS_DATA_16 0x59
+#define MPU9150_RA_EXT_SENS_DATA_17 0x5A
+#define MPU9150_RA_EXT_SENS_DATA_18 0x5B
+#define MPU9150_RA_EXT_SENS_DATA_19 0x5C
+#define MPU9150_RA_EXT_SENS_DATA_20 0x5D
+#define MPU9150_RA_EXT_SENS_DATA_21 0x5E
+#define MPU9150_RA_EXT_SENS_DATA_22 0x5F
+#define MPU9150_RA_EXT_SENS_DATA_23 0x60
+#define MPU9150_RA_MOT_DETECT_STATUS    0x61
+#define MPU9150_RA_I2C_SLV0_DO      0x63
+#define MPU9150_RA_I2C_SLV1_DO      0x64
+#define MPU9150_RA_I2C_SLV2_DO      0x65
+#define MPU9150_RA_I2C_SLV3_DO      0x66
+#define MPU9150_RA_I2C_MST_DELAY_CTRL   0x67
+#define MPU9150_RA_SIGNAL_PATH_RESET    0x68
+#define MPU9150_RA_MOT_DETECT_CTRL      0x69
+#define MPU9150_RA_USER_CTRL        0x6A
+#define MPU9150_RA_PWR_MGMT_1       0x6B
+#define MPU9150_RA_PWR_MGMT_2       0x6C
+#define MPU9150_RA_BANK_SEL         0x6D
+#define MPU9150_RA_MEM_START_ADDR   0x6E
+#define MPU9150_RA_MEM_R_W          0x6F
+#define MPU9150_RA_DMP_CFG_1        0x70
+#define MPU9150_RA_DMP_CFG_2        0x71
+#define MPU9150_RA_FIFO_COUNTH      0x72
+#define MPU9150_RA_FIFO_COUNTL      0x73
+#define MPU9150_RA_FIFO_R_W         0x74
+#define MPU9150_RA_WHO_AM_I         0x75
+
+#define MPU9150_TC_PWR_MODE_BIT     7
+#define MPU9150_TC_OFFSET_BIT       6
+#define MPU9150_TC_OFFSET_LENGTH    6
+#define MPU9150_TC_OTP_BNK_VLD_BIT  0
+
+#define MPU9150_VDDIO_LEVEL_VLOGIC  0
+#define MPU9150_VDDIO_LEVEL_VDD     1
+
+#define MPU9150_CFG_EXT_SYNC_SET_BIT    5
+#define MPU9150_CFG_EXT_SYNC_SET_LENGTH 3
+#define MPU9150_CFG_DLPF_CFG_BIT    2
+#define MPU9150_CFG_DLPF_CFG_LENGTH 3
+
+#define MPU9150_EXT_SYNC_DISABLED       0x0
+#define MPU9150_EXT_SYNC_TEMP_OUT_L     0x1
+#define MPU9150_EXT_SYNC_GYRO_XOUT_L    0x2
+#define MPU9150_EXT_SYNC_GYRO_YOUT_L    0x3
+#define MPU9150_EXT_SYNC_GYRO_ZOUT_L    0x4
+#define MPU9150_EXT_SYNC_ACCEL_XOUT_L   0x5
+#define MPU9150_EXT_SYNC_ACCEL_YOUT_L   0x6
+#define MPU9150_EXT_SYNC_ACCEL_ZOUT_L   0x7
+
+#define MPU9150_DLPF_BW_256         0x00
+#define MPU9150_DLPF_BW_188         0x01
+#define MPU9150_DLPF_BW_98          0x02
+#define MPU9150_DLPF_BW_42          0x03
+#define MPU9150_DLPF_BW_20          0x04
+#define MPU9150_DLPF_BW_10          0x05
+#define MPU9150_DLPF_BW_5           0x06
+
+#define MPU9150_GCONFIG_FS_SEL_BIT      4
+#define MPU9150_GCONFIG_FS_SEL_LENGTH   2
+
+#define MPU9150_GYRO_FS_250         0x00
+#define MPU9150_GYRO_FS_500         0x01
+#define MPU9150_GYRO_FS_1000        0x02
+#define MPU9150_GYRO_FS_2000        0x03
+
+#define MPU9150_ACONFIG_XA_ST_BIT           7
+#define MPU9150_ACONFIG_YA_ST_BIT           6
+#define MPU9150_ACONFIG_ZA_ST_BIT           5
+#define MPU9150_ACONFIG_AFS_SEL_BIT         4
+#define MPU9150_ACONFIG_AFS_SEL_LENGTH      2
+#define MPU9150_ACONFIG_ACCEL_HPF_BIT       2
+#define MPU9150_ACONFIG_ACCEL_HPF_LENGTH    3
+
+#define MPU9150_ACCEL_FS_2          0x00
+#define MPU9150_ACCEL_FS_4          0x01
+#define MPU9150_ACCEL_FS_8          0x02
+#define MPU9150_ACCEL_FS_16         0x03
+
+#define MPU9150_DHPF_RESET          0x00
+#define MPU9150_DHPF_5              0x01
+#define MPU9150_DHPF_2P5            0x02
+#define MPU9150_DHPF_1P25           0x03
+#define MPU9150_DHPF_0P63           0x04
+#define MPU9150_DHPF_HOLD           0x07
+
+#define MPU9150_TEMP_FIFO_EN_BIT    7
+#define MPU9150_XG_FIFO_EN_BIT      6
+#define MPU9150_YG_FIFO_EN_BIT      5
+#define MPU9150_ZG_FIFO_EN_BIT      4
+#define MPU9150_ACCEL_FIFO_EN_BIT   3
+#define MPU9150_SLV2_FIFO_EN_BIT    2
+#define MPU9150_SLV1_FIFO_EN_BIT    1
+#define MPU9150_SLV0_FIFO_EN_BIT    0
+
+#define MPU9150_MULT_MST_EN_BIT     7
+#define MPU9150_WAIT_FOR_ES_BIT     6
+#define MPU9150_SLV_3_FIFO_EN_BIT   5
+#define MPU9150_I2C_MST_P_NSR_BIT   4
+#define MPU9150_I2C_MST_CLK_BIT     3
+#define MPU9150_I2C_MST_CLK_LENGTH  4
+
+#define MPU9150_CLOCK_DIV_348       0x0
+#define MPU9150_CLOCK_DIV_333       0x1
+#define MPU9150_CLOCK_DIV_320       0x2
+#define MPU9150_CLOCK_DIV_308       0x3
+#define MPU9150_CLOCK_DIV_296       0x4
+#define MPU9150_CLOCK_DIV_286       0x5
+#define MPU9150_CLOCK_DIV_276       0x6
+#define MPU9150_CLOCK_DIV_267       0x7
+#define MPU9150_CLOCK_DIV_258       0x8
+#define MPU9150_CLOCK_DIV_500       0x9
+#define MPU9150_CLOCK_DIV_471       0xA
+#define MPU9150_CLOCK_DIV_444       0xB
+#define MPU9150_CLOCK_DIV_421       0xC
+#define MPU9150_CLOCK_DIV_400       0xD
+#define MPU9150_CLOCK_DIV_381       0xE
+#define MPU9150_CLOCK_DIV_364       0xF
+
+#define MPU9150_I2C_SLV_RW_BIT      7
+#define MPU9150_I2C_SLV_ADDR_BIT    6
+#define MPU9150_I2C_SLV_ADDR_LENGTH 7
+#define MPU9150_I2C_SLV_EN_BIT      7
+#define MPU9150_I2C_SLV_BYTE_SW_BIT 6
+#define MPU9150_I2C_SLV_REG_DIS_BIT 5
+#define MPU9150_I2C_SLV_GRP_BIT     4
+#define MPU9150_I2C_SLV_LEN_BIT     3
+#define MPU9150_I2C_SLV_LEN_LENGTH  4
+
+#define MPU9150_I2C_SLV4_RW_BIT         7
+#define MPU9150_I2C_SLV4_ADDR_BIT       6
+#define MPU9150_I2C_SLV4_ADDR_LENGTH    7
+#define MPU9150_I2C_SLV4_EN_BIT         7
+#define MPU9150_I2C_SLV4_INT_EN_BIT     6
+#define MPU9150_I2C_SLV4_REG_DIS_BIT    5
+#define MPU9150_I2C_SLV4_MST_DLY_BIT    4
+#define MPU9150_I2C_SLV4_MST_DLY_LENGTH 5
+
+#define MPU9150_MST_PASS_THROUGH_BIT    7
+#define MPU9150_MST_I2C_SLV4_DONE_BIT   6
+#define MPU9150_MST_I2C_LOST_ARB_BIT    5
+#define MPU9150_MST_I2C_SLV4_NACK_BIT   4
+#define MPU9150_MST_I2C_SLV3_NACK_BIT   3
+#define MPU9150_MST_I2C_SLV2_NACK_BIT   2
+#define MPU9150_MST_I2C_SLV1_NACK_BIT   1
+#define MPU9150_MST_I2C_SLV0_NACK_BIT   0
+
+#define MPU9150_INTCFG_INT_LEVEL_BIT        7
+#define MPU9150_INTCFG_INT_OPEN_BIT         6
+#define MPU9150_INTCFG_LATCH_INT_EN_BIT     5
+#define MPU9150_INTCFG_INT_RD_CLEAR_BIT     4
+#define MPU9150_INTCFG_FSYNC_INT_LEVEL_BIT  3
+#define MPU9150_INTCFG_FSYNC_INT_EN_BIT     2
+#define MPU9150_INTCFG_I2C_BYPASS_EN_BIT    1
+#define MPU9150_INTCFG_CLKOUT_EN_BIT        0
+
+#define MPU9150_INTMODE_ACTIVEHIGH  0x00
+#define MPU9150_INTMODE_ACTIVELOW   0x01
+
+#define MPU9150_INTDRV_PUSHPULL     0x00
+#define MPU9150_INTDRV_OPENDRAIN    0x01
+
+#define MPU9150_INTLATCH_50USPULSE  0x00
+#define MPU9150_INTLATCH_WAITCLEAR  0x01
+
+#define MPU9150_INTCLEAR_STATUSREAD 0x00
+#define MPU9150_INTCLEAR_ANYREAD    0x01
+
+#define MPU9150_INTERRUPT_FF_BIT            7
+#define MPU9150_INTERRUPT_MOT_BIT           6
+#define MPU9150_INTERRUPT_ZMOT_BIT          5
+#define MPU9150_INTERRUPT_FIFO_OFLOW_BIT    4
+#define MPU9150_INTERRUPT_I2C_MST_INT_BIT   3
+#define MPU9150_INTERRUPT_PLL_RDY_INT_BIT   2
+#define MPU9150_INTERRUPT_DMP_INT_BIT       1
+#define MPU9150_INTERRUPT_DATA_RDY_BIT      0
+
+// TODO: figure out what these actually do
+// UMPL source code is not very obivous
+#define MPU9150_DMPINT_5_BIT            5
+#define MPU9150_DMPINT_4_BIT            4
+#define MPU9150_DMPINT_3_BIT            3
+#define MPU9150_DMPINT_2_BIT            2
+#define MPU9150_DMPINT_1_BIT            1
+#define MPU9150_DMPINT_0_BIT            0
+
+#define MPU9150_MOTION_MOT_XNEG_BIT     7
+#define MPU9150_MOTION_MOT_XPOS_BIT     6
+#define MPU9150_MOTION_MOT_YNEG_BIT     5
+#define MPU9150_MOTION_MOT_YPOS_BIT     4
+#define MPU9150_MOTION_MOT_ZNEG_BIT     3
+#define MPU9150_MOTION_MOT_ZPOS_BIT     2
+#define MPU9150_MOTION_MOT_ZRMOT_BIT    0
+
+#define MPU9150_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
+#define MPU9150_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
+#define MPU9150_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
+#define MPU9150_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
+#define MPU9150_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
+#define MPU9150_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0
+
+#define MPU9150_PATHRESET_GYRO_RESET_BIT    2
+#define MPU9150_PATHRESET_ACCEL_RESET_BIT   1
+#define MPU9150_PATHRESET_TEMP_RESET_BIT    0
+
+#define MPU9150_DETECT_ACCEL_ON_DELAY_BIT       5
+#define MPU9150_DETECT_ACCEL_ON_DELAY_LENGTH    2
+#define MPU9150_DETECT_FF_COUNT_BIT             3
+#define MPU9150_DETECT_FF_COUNT_LENGTH          2
+#define MPU9150_DETECT_MOT_COUNT_BIT            1
+#define MPU9150_DETECT_MOT_COUNT_LENGTH         2
+
+#define MPU9150_DETECT_DECREMENT_RESET  0x0
+#define MPU9150_DETECT_DECREMENT_1      0x1
+#define MPU9150_DETECT_DECREMENT_2      0x2
+#define MPU9150_DETECT_DECREMENT_4      0x3
+
+#define MPU9150_USERCTRL_DMP_EN_BIT             7
+#define MPU9150_USERCTRL_FIFO_EN_BIT            6
+#define MPU9150_USERCTRL_I2C_MST_EN_BIT         5
+#define MPU9150_USERCTRL_I2C_IF_DIS_BIT         4
+#define MPU9150_USERCTRL_DMP_RESET_BIT          3
+#define MPU9150_USERCTRL_FIFO_RESET_BIT         2
+#define MPU9150_USERCTRL_I2C_MST_RESET_BIT      1
+#define MPU9150_USERCTRL_SIG_COND_RESET_BIT     0
+
+#define MPU9150_PWR1_DEVICE_RESET_BIT   7
+#define MPU9150_PWR1_SLEEP_BIT          6
+#define MPU9150_PWR1_CYCLE_BIT          5
+#define MPU9150_PWR1_TEMP_DIS_BIT       3
+#define MPU9150_PWR1_CLKSEL_BIT         2
+#define MPU9150_PWR1_CLKSEL_LENGTH      3
+
+#define MPU9150_CLOCK_INTERNAL          0x00
+#define MPU9150_CLOCK_PLL_XGYRO         0x01
+#define MPU9150_CLOCK_PLL_YGYRO         0x02
+#define MPU9150_CLOCK_PLL_ZGYRO         0x03
+#define MPU9150_CLOCK_PLL_EXT32K        0x04
+#define MPU9150_CLOCK_PLL_EXT19M        0x05
+#define MPU9150_CLOCK_KEEP_RESET        0x07
+
+#define MPU9150_PWR2_LP_WAKE_CTRL_BIT       7
+#define MPU9150_PWR2_LP_WAKE_CTRL_LENGTH    2
+#define MPU9150_PWR2_STBY_XA_BIT            5
+#define MPU9150_PWR2_STBY_YA_BIT            4
+#define MPU9150_PWR2_STBY_ZA_BIT            3
+#define MPU9150_PWR2_STBY_XG_BIT            2
+#define MPU9150_PWR2_STBY_YG_BIT            1
+#define MPU9150_PWR2_STBY_ZG_BIT            0
+
+#define MPU9150_WAKE_FREQ_1P25      0x0
+#define MPU9150_WAKE_FREQ_2P5       0x1
+#define MPU9150_WAKE_FREQ_5         0x2
+#define MPU9150_WAKE_FREQ_10        0x3
+
+#define MPU9150_BANKSEL_PRFTCH_EN_BIT       6
+#define MPU9150_BANKSEL_CFG_USER_BANK_BIT   5
+#define MPU9150_BANKSEL_MEM_SEL_BIT         4
+#define MPU9150_BANKSEL_MEM_SEL_LENGTH      5
+
+#define MPU9150_WHO_AM_I_BIT        6
+#define MPU9150_WHO_AM_I_LENGTH     6
+
+#define MPU9150_DMP_MEMORY_BANKS        8
+#define MPU9150_DMP_MEMORY_BANK_SIZE    256
+#define MPU9150_DMP_MEMORY_CHUNK_SIZE   16
+
+// note: DMP code memory blocks defined at end of header file
+
+class MPU9150 {
+    public:
+        MPU9150();
+        MPU9150(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);
+
+        // 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
+        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 MPU9150_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 MPU9150_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
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[14];
+};
+
+#endif /* _MPU9150_H_ */
diff --git a/Arduino/MPU9150/MPU9150_9Axis_MotionApps41.h b/Arduino/MPU9150/MPU9150_9Axis_MotionApps41.h
new file mode 100644
index 00000000..ce8e57dc
--- /dev/null
+++ b/Arduino/MPU9150/MPU9150_9Axis_MotionApps41.h
@@ -0,0 +1,852 @@
+// I2Cdev library collection - MPU9150 I2C device class, 9-axis MotionApps 4.1 implementation
+// Based on InvenSense MPU-9150 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 6/18/2012 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ... - ongoing debug release
+
+/* ============================================
+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 _MPU9150_9AXIS_MOTIONAPPS41_H_
+#define _MPU9150_9AXIS_MOTIONAPPS41_H_
+
+#include "I2Cdev.h"
+#include "helper_3dmath.h"
+
+// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
+#define MPU9150_INCLUDE_DMP_MOTIONAPPS41
+
+#include "MPU9150.h"
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifdef __AVR__
+    #include 
+#else
+    // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+    #ifndef __PGMSPACE_H_
+        #define __PGMSPACE_H_ 1
+        #include 
+
+        #define PROGMEM
+        #define PGM_P  const char *
+        #define PSTR(str) (str)
+        #define F(x) x
+
+        typedef void prog_void;
+        typedef char prog_char;
+//        typedef unsigned char unsigned char;
+        typedef int8_t prog_int8_t;
+        typedef uint8_t prog_uint8_t;
+        typedef int16_t prog_int16_t;
+        typedef uint16_t prog_uint16_t;
+        typedef int32_t prog_int32_t;
+        typedef uint32_t prog_uint32_t;
+        
+        #define strcpy_P(dest, src) strcpy((dest), (src))
+        #define strcat_P(dest, src) strcat((dest), (src))
+        #define strcmp_P(a, b) strcmp((a), (b))
+        
+        #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+        #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+        #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+        #define pgm_read_float(addr) (*(const float *)(addr))
+        
+        #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+        #define pgm_read_word_near(addr) pgm_read_word(addr)
+        #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+        #define pgm_read_float_near(addr) pgm_read_float(addr)
+        #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+        #define pgm_read_word_far(addr) pgm_read_word(addr)
+        #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+        #define pgm_read_float_far(addr) pgm_read_float(addr)
+    #endif
+#endif
+
+// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
+// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
+// after moving string constants to flash memory storage using the F()
+// compiler macro (Arduino IDE 1.0+ required).
+
+//#define DEBUG
+#ifdef DEBUG
+    #define DEBUG_PRINT(x) Serial.print(x)
+    #define DEBUG_PRINTF(x, y) Serial.print(x, y)
+    #define DEBUG_PRINTLN(x) Serial.println(x)
+    #define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
+#else
+    #define DEBUG_PRINT(x)
+    #define DEBUG_PRINTF(x, y)
+    #define DEBUG_PRINTLN(x)
+    #define DEBUG_PRINTLNF(x, y)
+#endif
+
+#define MPU9150_DMP_CODE_SIZE       1962    // dmpMemory[]
+#define MPU9150_DMP_CONFIG_SIZE     232     // dmpConfig[]
+#define MPU9150_DMP_UPDATES_SIZE    140     // dmpUpdates[]
+
+/* ================================================================================================ *
+ | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
+ |                                                                                                  |
+ | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
+ |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
+ |                                                                                                  |
+ | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
+ |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
+ * ================================================================================================ */
+
+// this block of memory gets written to the MPU on start-up, and it seems
+// to be volatile memory, so it has to be done each time (it only takes ~1
+// second though)
+const unsigned char dmpMemory[MPU9150_DMP_CODE_SIZE] PROGMEM = {
+    // bank 0, 256 bytes
+    0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
+    0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
+    0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+    0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
+    0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
+    0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
+    0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
+
+    // bank 1, 256 bytes
+    0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
+    0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
+    0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
+    0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
+    
+    // bank 2, 256 bytes
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    
+    // bank 3, 256 bytes
+    0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
+    0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
+    0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
+    0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
+    0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
+    0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
+    0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
+    0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
+    0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
+    0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
+    0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
+    0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
+    0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
+    0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
+    0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
+    0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,
+    
+    // bank 4, 256 bytes
+    0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
+    0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
+    0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
+    0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
+    0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
+    0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
+    0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
+    0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
+    0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
+    0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
+    0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
+    0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
+    0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+    0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
+    0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+    0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
+    
+    // bank 5, 256 bytes
+    0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
+    0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
+    0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
+    0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
+    0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
+    0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
+    0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
+    0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
+    0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
+    0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
+    0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
+    0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
+    0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
+    0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
+    0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
+    0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,
+
+    // bank 6, 256 bytes
+    0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
+    0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
+    0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
+    0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
+    0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
+    0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
+    0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
+    0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
+    0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
+    0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
+    0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
+    0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
+    0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
+    0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
+    0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
+    0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,
+    
+    // bank 7, 170 bytes (remainder)
+    0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
+    0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
+    0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
+    0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
+    0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
+    0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
+    0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
+    0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+    0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
+    0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
+    0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
+};
+
+const unsigned char dmpConfig[MPU9150_DMP_CONFIG_SIZE] PROGMEM = {
+//  BANK    OFFSET  LENGTH  [DATA]
+    0x02,   0xEC,   0x04,   0x00, 0x47, 0x7D, 0x1A,   // ?
+    0x03,   0x82,   0x03,   0x4C, 0xCD, 0x6C,         // FCFG_1 inv_set_gyro_calibration
+    0x03,   0xB2,   0x03,   0x36, 0x56, 0x76,         // FCFG_3 inv_set_gyro_calibration
+    0x00,   0x68,   0x04,   0x02, 0xCA, 0xE3, 0x09,   // D_0_104 inv_set_gyro_calibration
+    0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // D_1_152 inv_set_accel_calibration
+    0x03,   0x86,   0x03,   0x0C, 0xC9, 0x2C,         // FCFG_2 inv_set_accel_calibration
+    0x03,   0x90,   0x03,   0x26, 0x46, 0x66,         //   (continued)...FCFG_2 inv_set_accel_calibration
+    0x00,   0x6C,   0x02,   0x40, 0x00,               // D_0_108 inv_set_accel_calibration
+
+    0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_00 inv_set_compass_calibration
+    0x02,   0x44,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_01
+    0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_02
+    0x02,   0x4C,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_10
+    0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_11
+    0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_12
+    0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_20
+    0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_21
+    0x02,   0xBC,   0x04,   0xC0, 0x00, 0x00, 0x00,   // CPASS_MTX_22
+
+    0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,   // D_1_236 inv_apply_endian_accel
+    0x03,   0x86,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
+    0x04,   0x22,   0x03,   0x0D, 0x35, 0x5D,         // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
+    0x00,   0xA3,   0x01,   0x00,                     // ?
+    0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,   // FCFG_5 inv_set_bias_update
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
+    0x07,   0x9F,   0x01,   0x30,                     // CFG_16 inv_set_footer
+    0x07,   0x67,   0x01,   0x9A,                     // CFG_GYRO_SOURCE inv_send_gyro
+    0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_9 inv_send_gyro -> inv_construct3_fifo
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
+    0x02,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // ?
+    0x07,   0x83,   0x06,   0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
+                 // SPECIAL 0x01 = enable interrupts
+    0x00,   0x00,   0x00,   0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
+    0x07,   0xA7,   0x01,   0xFE,                     // ?
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
+    0x07,   0x67,   0x01,   0x9A,                     // ?
+    0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
+    0x07,   0x8D,   0x04,   0xF1, 0x28, 0x30, 0x38,   // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
+    0x02,   0x16,   0x02,   0x00, 0x03                // D_0_22 inv_set_fifo_rate
+
+    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
+    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
+    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
+
+    // It is important to make sure the host processor can keep up with reading and processing
+    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
+};
+
+const unsigned char dmpUpdates[MPU9150_DMP_UPDATES_SIZE] PROGMEM = {
+    0x01,   0xB2,   0x02,   0xFF, 0xF5,
+    0x01,   0x90,   0x04,   0x0A, 0x0D, 0x97, 0xC0,
+    0x00,   0xA3,   0x01,   0x00,
+    0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,
+    0x01,   0x6A,   0x02,   0x06, 0x00,
+    0x01,   0x60,   0x08,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
+    0x02,   0x60,   0x0C,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x01,   0x08,   0x02,   0x01, 0x20,
+    0x01,   0x0A,   0x02,   0x00, 0x4E,
+    0x01,   0x02,   0x02,   0xFE, 0xB3,
+    0x02,   0x6C,   0x04,   0x00, 0x00, 0x00, 0x00, // READ
+    0x02,   0x6C,   0x04,   0xFA, 0xFE, 0x00, 0x00,
+    0x02,   0x60,   0x0C,   0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
+    0x02,   0xF4,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x02,   0xF8,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x02,   0xFC,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x00, 0x40, 0x00, 0x00
+};
+
+uint8_t MPU9150::dmpInitialize() {
+    // reset device
+    DEBUG_PRINTLN(F("\n\nResetting MPU9150..."));
+    reset();
+    delay(30); // wait after reset
+
+    // disable sleep mode
+    DEBUG_PRINTLN(F("Disabling sleep mode..."));
+    setSleepEnabled(false);
+
+    // get MPU product ID
+    DEBUG_PRINTLN(F("Getting product ID..."));
+    //uint8_t productID = 0; //getProductID();
+    DEBUG_PRINT(F("Product ID = "));
+    DEBUG_PRINT(productID);
+
+    // get MPU hardware revision
+    DEBUG_PRINTLN(F("Selecting user bank 16..."));
+    setMemoryBank(0x10, true, true);
+    DEBUG_PRINTLN(F("Selecting memory byte 6..."));
+    setMemoryStartAddress(0x06);
+    DEBUG_PRINTLN(F("Checking hardware revision..."));
+    uint8_t hwRevision = readMemoryByte();
+    DEBUG_PRINT(F("Revision @ user[16][6] = "));
+    DEBUG_PRINTLNF(hwRevision, HEX);
+    DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
+    setMemoryBank(0, false, false);
+
+    // check OTP bank valid
+    DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
+    uint8_t otpValid = getOTPBankValid();
+    DEBUG_PRINT(F("OTP bank is "));
+    DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
+
+    // get X/Y/Z gyro offsets
+    DEBUG_PRINTLN(F("Reading gyro offset values..."));
+    int8_t xgOffset = getXGyroOffset();
+    int8_t ygOffset = getYGyroOffset();
+    int8_t zgOffset = getZGyroOffset();
+    DEBUG_PRINT(F("X gyro offset = "));
+    DEBUG_PRINTLN(xgOffset);
+    DEBUG_PRINT(F("Y gyro offset = "));
+    DEBUG_PRINTLN(ygOffset);
+    DEBUG_PRINT(F("Z gyro offset = "));
+    DEBUG_PRINTLN(zgOffset);
+    
+    I2Cdev::readByte(devAddr, MPU9150_RA_USER_CTRL, buffer); // ?
+    
+    DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled"));
+    I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x32);
+
+    // enable MPU AUX I2C bypass mode
+    //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode..."));
+    //setI2CBypassEnabled(true);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
+    //mag -> setMode(0);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x00);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
+    //mag -> setMode(0x0F);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
+
+    DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
+    int8_t asax, asay, asaz;
+    //mag -> getAdjustment(&asax, &asay, &asaz);
+    I2Cdev::readBytes(0x0E, 0x10, 3, buffer);
+    asax = (int8_t)buffer[0];
+    asay = (int8_t)buffer[1];
+    asaz = (int8_t)buffer[2];
+    DEBUG_PRINT(F("Adjustment X/Y/Z = "));
+    DEBUG_PRINT(asax);
+    DEBUG_PRINT(F(" / "));
+    DEBUG_PRINT(asay);
+    DEBUG_PRINT(F(" / "));
+    DEBUG_PRINTLN(asaz);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
+    //mag -> setMode(0);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x00);
+
+    // load DMP code into memory banks
+    DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
+    DEBUG_PRINT(MPU9150_DMP_CODE_SIZE);
+    DEBUG_PRINTLN(F(" bytes)"));
+    if (writeProgMemoryBlock(dmpMemory, MPU9150_DMP_CODE_SIZE)) {
+        DEBUG_PRINTLN(F("Success! DMP code written and verified."));
+
+        DEBUG_PRINTLN(F("Configuring DMP and related settings..."));
+
+        // write DMP configuration
+        DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
+        DEBUG_PRINT(MPU9150_DMP_CONFIG_SIZE);
+        DEBUG_PRINTLN(F(" bytes in config def)"));
+        if (writeProgDMPConfigurationSet(dmpConfig, MPU9150_DMP_CONFIG_SIZE)) {
+            DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
+
+            DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
+            setIntEnabled(0x12);
+
+            DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
+            setRate(4); // 1khz / (1 + 4) = 200 Hz
+
+            DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
+            setClockSource(MPU9150_CLOCK_PLL_ZGYRO);
+
+            DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
+            setDLPFMode(MPU9150_DLPF_BW_42);
+
+            DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
+            setExternalFrameSync(MPU9150_EXT_SYNC_TEMP_OUT_L);
+
+            DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
+            setFullScaleGyroRange(MPU9150_GYRO_FS_2000);
+
+            DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
+            setDMPConfig1(0x03);
+            setDMPConfig2(0x00);
+
+            DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
+            setOTPBankValid(false);
+
+            DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values..."));
+            setXGyroOffsetTC(xgOffset);
+            setYGyroOffsetTC(ygOffset);
+            setZGyroOffsetTC(zgOffset);
+
+            DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
+            setXGyroOffset(0);
+            setYGyroOffset(0);
+            setZGyroOffset(0);
+
+            DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)..."));
+            uint8_t dmpUpdate[16], j;
+            uint16_t pos = 0;
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            resetFIFO();
+
+            DEBUG_PRINTLN(F("Reading FIFO count..."));
+            uint8_t fifoCount = getFIFOCount();
+
+            DEBUG_PRINT(F("Current FIFO count="));
+            DEBUG_PRINTLN(fifoCount);
+            uint8_t fifoBuffer[128];
+            //getFIFOBytes(fifoBuffer, fifoCount);
+
+            DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Disabling all standby flags..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_PWR_MGMT_2, 0x00);
+
+            DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_ACCEL_CONFIG, 0x00);
+
+            DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
+            setMotionDetectionThreshold(2);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
+            setZeroMotionDetectionThreshold(156);
+
+            DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
+            setMotionDetectionDuration(80);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
+            setZeroMotionDetectionDuration(0);
+
+            DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
+            //mag -> setMode(1);
+            I2Cdev::writeByte(0x0E, 0x0A, 0x01);
+
+            // setup AK8975 (0x0E) as Slave 0 in read mode
+            DEBUG_PRINTLN(F("Setting up AK8975 read slave 0..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_ADDR, 0x8E);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_REG,  0x01);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_CTRL, 0xDA);
+
+            // setup AK8975 (0x0E) as Slave 2 in write mode
+            DEBUG_PRINTLN(F("Setting up AK8975 write slave 2..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_ADDR, 0x0E);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_REG,  0x0A);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_CTRL, 0x81);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_DO,   0x01);
+
+            // setup I2C timing/delay control
+            DEBUG_PRINTLN(F("Setting up slave access delay..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV4_CTRL, 0x18);
+            I2Cdev::writeByte(0x68, MPU9150_RA_I2C_MST_DELAY_CTRL, 0x05);
+
+            // enable interrupts
+            DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_INT_PIN_CFG, 0x00);
+
+            // enable I2C master mode and reset DMP/FIFO
+            DEBUG_PRINTLN(F("Enabling I2C master mode..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20);
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x24);
+            DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
+            I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20);
+            DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
+            I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0xE8);
+
+            DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            
+            DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            #ifdef DEBUG
+                DEBUG_PRINT(F("Read bytes: "));
+                for (j = 0; j < 4; j++) {
+                    DEBUG_PRINTF(dmpUpdate[3 + j], HEX);
+                    DEBUG_PRINT(" ");
+                }
+                DEBUG_PRINTLN("");
+            #endif
+
+            DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 46..."));
+            while ((fifoCount = getFIFOCount()) < 46);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+
+            DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
+            while ((fifoCount = getFIFOCount()) < 48);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
+            while ((fifoCount = getFIFOCount()) < 48);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+
+            DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
+            setDMPEnabled(false);
+
+            DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer..."));
+            dmpPacketSize = 48;
+            /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
+                return 3; // TODO: proper error code for no memory
+            }*/
+
+            DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
+            resetFIFO();
+            getIntStatus();
+        } else {
+            DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
+            return 2; // configuration block loading failed
+        }
+    } else {
+        DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
+        return 1; // main binary block loading failed
+    }
+    return 0; // success
+}
+
+bool MPU9150::dmpPacketAvailable() {
+    return getFIFOCount() >= dmpGetFIFOPacketSize();
+}
+
+// uint8_t MPU9150::dmpSetFIFORate(uint8_t fifoRate);
+// uint8_t MPU9150::dmpGetFIFORate();
+// uint8_t MPU9150::dmpGetSampleStepSizeMS();
+// uint8_t MPU9150::dmpGetSampleFrequency();
+// int32_t MPU9150::dmpDecodeTemperature(int8_t tempReg);
+
+//uint8_t MPU9150::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU9150::dmpUnregisterFIFORateProcess(inv_obj_func func);
+//uint8_t MPU9150::dmpRunFIFORateProcesses();
+
+// uint8_t MPU9150::dmpSendQuaternion(uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendPacketNumber(uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU9150::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+uint8_t MPU9150::dmpGetAccel(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]);
+    data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]);
+    data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]);
+    return 0;
+}
+uint8_t MPU9150::dmpGetAccel(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[34] << 8) + packet[35];
+    data[1] = (packet[38] << 8) + packet[39];
+    data[2] = (packet[42] << 8) + packet[43];
+    return 0;
+}
+uint8_t MPU9150::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[34] << 8) + packet[35];
+    v -> y = (packet[38] << 8) + packet[39];
+    v -> z = (packet[42] << 8) + packet[43];
+    return 0;
+}
+uint8_t MPU9150::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
+    data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
+    data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
+    data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
+    return 0;
+}
+uint8_t MPU9150::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 8) + packet[1]);
+    data[1] = ((packet[4] << 8) + packet[5]);
+    data[2] = ((packet[8] << 8) + packet[9]);
+    data[3] = ((packet[12] << 8) + packet[13]);
+    return 0;
+}
+uint8_t MPU9150::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    if (status == 0) {
+        q -> w = (float)qI[0] / 16384.0f;
+        q -> x = (float)qI[1] / 16384.0f;
+        q -> y = (float)qI[2] / 16384.0f;
+        q -> z = (float)qI[3] / 16384.0f;
+        return 0;
+    }
+    return status; // int16 return value, indicates error if this line is reached
+}
+// uint8_t MPU9150::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
+uint8_t MPU9150::dmpGetGyro(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
+    data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
+    data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
+    return 0;
+}
+uint8_t MPU9150::dmpGetGyro(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[16] << 8) + packet[17];
+    data[1] = (packet[20] << 8) + packet[21];
+    data[2] = (packet[24] << 8) + packet[25];
+    return 0;
+}
+uint8_t MPU9150::dmpGetMag(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[28] << 8) + packet[29];
+    data[1] = (packet[30] << 8) + packet[31];
+    data[2] = (packet[32] << 8) + packet[33];
+    return 0;
+}
+// uint8_t MPU9150::dmpSetLinearAccelFilterCoefficient(float coef);
+// uint8_t MPU9150::dmpGetLinearAccel(long *data, const uint8_t* packet);
+uint8_t MPU9150::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
+    // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
+    v -> x = vRaw -> x - gravity -> x*4096;
+    v -> y = vRaw -> y - gravity -> y*4096;
+    v -> z = vRaw -> z - gravity -> z*4096;
+    return 0;
+}
+// uint8_t MPU9150::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
+uint8_t MPU9150::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
+    // rotate measured 3D acceleration vector into original state
+    // frame of reference based on orientation quaternion
+    memcpy(v, vReal, sizeof(VectorInt16));
+    v -> rotate(q);
+    return 0;
+}
+// uint8_t MPU9150::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetGyroSensor(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetControlData(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetTemperature(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU9150::dmpGetGravity(VectorFloat *v, Quaternion *q) {
+    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
+    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
+    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
+    return 0;
+}
+// uint8_t MPU9150::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetEIS(long *data, const uint8_t* packet);
+
+uint8_t MPU9150::dmpGetEuler(float *data, Quaternion *q) {
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
+    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
+    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
+    return 0;
+}
+uint8_t MPU9150::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
+    return 0;
+}
+
+// uint8_t MPU9150::dmpGetAccelFloat(float *data, const uint8_t* packet);
+// uint8_t MPU9150::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
+
+uint8_t MPU9150::dmpProcessFIFOPacket(const unsigned char *dmpData) {
+    /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
+        if (dmpData[k] < 0x10) Serial.print("0");
+        Serial.print(dmpData[k], HEX);
+        Serial.print(" ");
+    }
+    Serial.print("\n");*/
+    //Serial.println((uint16_t)dmpPacketBuffer);
+    return 0;
+}
+uint8_t MPU9150::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
+    uint8_t status;
+    uint8_t buf[dmpPacketSize];
+    for (uint8_t i = 0; i < numPackets; i++) {
+        // read packet from FIFO
+        getFIFOBytes(buf, dmpPacketSize);
+
+        // process packet
+        if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
+        
+        // increment external process count variable, if supplied
+        if (processed != 0) (*processed)++;
+    }
+    return 0;
+}
+
+// uint8_t MPU9150::dmpSetFIFOProcessedCallback(void (*func) (void));
+
+// uint8_t MPU9150::dmpInitFIFOParam();
+// uint8_t MPU9150::dmpCloseFIFO();
+// uint8_t MPU9150::dmpSetGyroDataSource(uint_fast8_t source);
+// uint8_t MPU9150::dmpDecodeQuantizedAccel();
+// uint32_t MPU9150::dmpGetGyroSumOfSquare();
+// uint32_t MPU9150::dmpGetAccelSumOfSquare();
+// void MPU9150::dmpOverrideQuaternion(long *q);
+uint16_t MPU9150::dmpGetFIFOPacketSize() {
+    return dmpPacketSize;
+}
+
+#endif /* _MPU9150_9AXIS_MOTIONAPPS41_H_ */
diff --git a/Arduino/MPU9150/examples/MPU9150_raw/MPU9150_raw.ino b/Arduino/MPU9150/examples/MPU9150_raw/MPU9150_raw.ino
new file mode 100644
index 00000000..36c2258b
--- /dev/null
+++ b/Arduino/MPU9150/examples/MPU9150_raw/MPU9150_raw.ino
@@ -0,0 +1,118 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU9150
+// 1/4/2013 original by Jeff Rowberg  at https://github.com/jrowberg/i2cdevlib
+//          modified by Aaron Weiss 
+//
+// Changelog:
+//     2011-10-07 - initial release
+//     2013-1-4 - added raw magnetometer output
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+*/
+
+// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
+// is used in I2Cdev.h
+#include "Wire.h"
+
+// I2Cdev and MPU9150 must be installed as libraries, or else the .cpp/.h files
+// for both classes must be in the include path of your project
+#include "I2Cdev.h"
+#include "MPU9150.h"
+#include "helper_3dmath.h"
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for InvenSense evaluation board)
+// AD0 high = 0x69
+MPU9150 accelGyroMag;
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+int16_t mx, my, mz;
+
+#define LED_PIN 13
+bool blinkState = false;
+
+void setup() {
+    // join I2C bus (I2Cdev library doesn't do this automatically)
+    Wire.begin();
+
+    // initialize serial communication
+    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
+    // it's really up to you depending on your project)
+    Serial.begin(115200);
+
+    // initialize device
+    Serial.println("Initializing I2C devices...");
+    accelGyroMag.initialize();
+
+    // verify connection
+    Serial.println("Testing device connections...");
+    Serial.println(accelGyroMag.testConnection() ? "MPU9150 connection successful" : "MPU9150 connection failed");
+
+    // configure Arduino LED pin for output
+    pinMode(LED_PIN, OUTPUT);
+}
+
+void loop() {
+    // read raw accel/gyro/mag measurements from device
+    accelGyroMag.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+
+    // these methods (and a few others) are also available
+    //accelGyroMag.getAcceleration(&ax, &ay, &az);
+    //accelGyroMag.getRotation(&gx, &gy, &gz);
+
+    // display tab-separated accel/gyro/mag x/y/z values
+//  Serial.print("a/g/m:\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.print(gz); Serial.print("\t");
+    //Serial.print(int(mx)*int(mx)); Serial.print("\t");
+    //Serial.print(int(my)*int(my)); Serial.print("\t");
+    //Serial.print(int(mz)*int(mz)); Serial.print("\t | ");
+
+    //const float N = 256;
+    //float mag = mx*mx/N + my*my/N + mz*mz/N;
+
+    //Serial.print(mag); Serial.print("\t");
+    //for (int i=0; i
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2012-06-05 - add 3D math helper file to DMP6 example sketch
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2012 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in
+all copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+THE SOFTWARE.
+===============================================
+*/
+
+#ifndef _HELPER_3DMATH_H_
+#define _HELPER_3DMATH_H_
+
+class Quaternion {
+    public:
+        float w;
+        float x;
+        float y;
+        float z;
+        
+        Quaternion() {
+            w = 1.0f;
+            x = 0.0f;
+            y = 0.0f;
+            z = 0.0f;
+        }
+        
+        Quaternion(float nw, float nx, float ny, float nz) {
+            w = nw;
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        Quaternion getProduct(Quaternion q) {
+            // Quaternion multiplication is defined by:
+            //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+            //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+            //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+            //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+            return Quaternion(
+                w*q.w - x*q.x - y*q.y - z*q.z,  // new w
+                w*q.x + x*q.w + y*q.z - z*q.y,  // new x
+                w*q.y - x*q.z + y*q.w + z*q.x,  // new y
+                w*q.z + x*q.y - y*q.x + z*q.w); // new z
+        }
+
+        Quaternion getConjugate() {
+            return Quaternion(w, -x, -y, -z);
+        }
+        
+        float getMagnitude() {
+            return sqrt(w*w + x*x + y*y + z*z);
+        }
+        
+        void normalize() {
+            float m = getMagnitude();
+            w /= m;
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        Quaternion getNormalized() {
+            Quaternion r(w, x, y, z);
+            r.normalize();
+            return r;
+        }
+};
+
+class VectorInt16 {
+    public:
+        int16_t x;
+        int16_t y;
+        int16_t z;
+
+        VectorInt16() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorInt16 getNormalized() {
+            VectorInt16 r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            // http://www.cprogramming.com/tutorial/3d/quaternions.html
+            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+            // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+            // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+        
+            // P_out = q * P_in * conj(q)
+            // - P_out is the output vector
+            // - q is the orientation quaternion
+            // - P_in is the input vector (a*aReal)
+            // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorInt16 getRotated(Quaternion *q) {
+            VectorInt16 r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+class VectorFloat {
+    public:
+        float x;
+        float y;
+        float z;
+
+        VectorFloat() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorFloat(float nx, float ny, float nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorFloat getNormalized() {
+            VectorFloat r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorFloat getRotated(Quaternion *q) {
+            VectorFloat r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+#endif /* _HELPER_3DMATH_H_ */
\ No newline at end of file
diff --git a/Arduino/MPU9150/library.json b/Arduino/MPU9150/library.json
new file mode 100644
index 00000000..dcabbee4
--- /dev/null
+++ b/Arduino/MPU9150/library.json
@@ -0,0 +1,18 @@
+{
+  "name": "I2Cdevlib-MPU9150",
+  "version": "1.0.0",
+  "keywords": "gyroscope, accelerometer, compass, sensor, i2cdevlib, i2c",
+  "description": "The MPU-9150 combines two chips: the MPU-6050, which contains a 3-axis gyroscope, 3-axis accelerometer and the AK8975, a 3-axis digital compass",
+  "include": "Arduino/MPU9150",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "dependencies":
+  {
+    "jrowberg/I2Cdevlib-Core": "*"
+  },
+  "frameworks": "arduino",
+  "platforms": "*"
+}
diff --git a/Arduino/MPU9250/BMP180.cpp b/Arduino/MPU9250/BMP180.cpp
new file mode 100644
index 00000000..aaa9c170
--- /dev/null
+++ b/Arduino/MPU9250/BMP180.cpp
@@ -0,0 +1,174 @@
+/*
+  Barometer library V1.0
+  2010 Copyright (c) Seeed Technology Inc.  All right reserved.
+ 
+  Original Author: LG
+  
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+#include "BMP180.h"
+#include 
+#include 
+
+void BMP180::init(void)
+{
+    Wire.begin();
+    ac1 = bmp180ReadInt(0xAA);
+    ac2 = bmp180ReadInt(0xAC);
+    ac3 = bmp180ReadInt(0xAE);
+    ac4 = bmp180ReadInt(0xB0);
+    ac5 = bmp180ReadInt(0xB2);
+    ac6 = bmp180ReadInt(0xB4);
+    b1 = bmp180ReadInt(0xB6);
+    b2 = bmp180ReadInt(0xB8);
+    mb = bmp180ReadInt(0xBA);
+    mc = bmp180ReadInt(0xBC);
+    md = bmp180ReadInt(0xBE);
+}
+// Read 1 byte from the BMP085 at 'address'
+// Return: the read byte;
+char BMP180::bmp180Read(unsigned char address)
+{
+    //Wire.begin();
+    unsigned char data;
+    Wire.beginTransmission(BMP180_ADDRESS);
+    Wire.write(address);
+    Wire.endTransmission();
+
+    Wire.requestFrom(BMP180_ADDRESS, 1);
+    while(!Wire.available());
+    return Wire.read();
+}
+// Read 2 bytes from the BMP085
+// First byte will be from 'address'
+// Second byte will be from 'address'+1
+int BMP180::bmp180ReadInt(unsigned char address)
+{
+    unsigned char msb, lsb;
+    Wire.beginTransmission(BMP180_ADDRESS);
+    Wire.write(address);
+    Wire.endTransmission();
+    Wire.requestFrom(BMP180_ADDRESS, 2);
+    while(Wire.available()<2);
+    msb = Wire.read();
+    lsb = Wire.read();
+    return (int) msb<<8 | lsb;
+}
+// Read the uncompensated temperature value
+unsigned int BMP180::bmp180ReadUT()
+{
+  unsigned int ut;
+  Wire.beginTransmission(BMP180_ADDRESS);
+  Wire.write(0xF4);
+  Wire.write(0x2E);
+  Wire.endTransmission();
+  delay(5);
+  ut = bmp180ReadInt(0xF6);
+  return ut;
+}
+// Read the uncompensated pressure value
+unsigned long BMP180::bmp180ReadUP()
+{
+    unsigned char msb, lsb, xlsb;
+    unsigned long up = 0;
+    Wire.beginTransmission(BMP180_ADDRESS);
+    Wire.write(0xF4);
+    Wire.write(0x34 + (OSS<<6));
+    Wire.endTransmission();
+    delay(2 + (3<> (8-OSS);
+    return up;
+}
+void BMP180::writeRegister(int deviceAddress, byte address, byte val)
+{
+    Wire.beginTransmission(deviceAddress); // start transmission to device 
+    Wire.write(address);       // send register address
+    Wire.write(val);         // send value to write
+    Wire.endTransmission();     // end transmission
+}
+int BMP180::readRegister(int deviceAddress, byte address)
+{
+    int v;
+    Wire.beginTransmission(deviceAddress);
+    Wire.write(address); // register to read
+    Wire.endTransmission();
+
+    Wire.requestFrom(deviceAddress, 1); // read a byte
+
+    while(!Wire.available()) {
+    // waiting
+    }
+
+    v = Wire.read();
+    return v;
+}
+float BMP180::calcAltitude(float pressure)
+{
+    float A = pressure/101325;
+    float B = 1/5.25588;
+    float C = pow(A,B);
+    C = 1 - C;
+    C = C /0.0000225577;
+    return C;
+}
+float BMP180::bmp180GetTemperature(unsigned int ut)
+{
+    long x1, x2;
+
+    x1 = (((long)ut - (long)ac6)*(long)ac5) >> 15;
+    x2 = ((long)mc << 11)/(x1 + md);
+    PressureCompensate = x1 + x2;
+
+    float temp = ((PressureCompensate + 8)>>4);
+    temp = temp /10;
+
+    return temp;
+}
+long BMP180::bmp180GetPressure(unsigned long up)
+{
+    long x1, x2, x3, b3, b6, p;
+    unsigned long b4, b7;
+    b6 = PressureCompensate - 4000;
+    x1 = (b2 * (b6 * b6)>>12)>>11;
+    x2 = (ac2 * b6)>>11;
+    x3 = x1 + x2;
+    b3 = (((((long)ac1)*4 + x3)<>2;
+
+    // Calculate B4
+    x1 = (ac3 * b6)>>13;
+    x2 = (b1 * ((b6 * b6)>>12))>>16;
+    x3 = ((x1 + x2) + 2)>>2;
+    b4 = (ac4 * (unsigned long)(x3 + 32768))>>15;
+
+    b7 = ((unsigned long)(up - b3) * (50000>>OSS));
+    if (b7 < 0x80000000)
+    p = (b7<<1)/b4;
+    else
+    p = (b7/b4)<<1;
+
+    x1 = (p>>8) * (p>>8);
+    x1 = (x1 * 3038)>>16;
+    x2 = (-7357 * p)>>16;
+    p += (x1 + x2 + 3791)>>4;
+
+    long temp = p;
+    return temp;
+}
+
diff --git a/Arduino/MPU9250/BMP180.h b/Arduino/MPU9250/BMP180.h
new file mode 100644
index 00000000..b8faac8d
--- /dev/null
+++ b/Arduino/MPU9250/BMP180.h
@@ -0,0 +1,58 @@
+/*
+  Barometer library V1.0
+  2010 Copyright (c) Seeed Technology Inc.  All right reserved.
+
+  Original Author: LG
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  This library is distributed in the hope that it will be useful,
+  but WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+  Lesser General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with this library; if not, write to the Free Software
+  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
+*/
+#ifndef __BAROMETER_H__
+#define __BAROMETER_H__
+
+#include 
+#include 
+
+const unsigned char OSS = 0;
+#define BMP180_ADDRESS 0x77
+class BMP180
+{
+public:
+    void init(void);
+    long PressureCompensate;
+    float bmp180GetTemperature(unsigned int ut);
+    long bmp180GetPressure(unsigned long up);
+    float calcAltitude(float pressure);
+    unsigned int bmp180ReadUT(void);
+    unsigned long bmp180ReadUP(void);
+
+private:
+    int ac1;
+    int ac2;
+    int ac3;
+    unsigned int ac4;
+    unsigned int ac5;
+    unsigned int ac6;
+    int b1;
+    int b2;
+    int mb;
+    int mc;
+    int md;
+    char bmp180Read(unsigned char address);
+    int bmp180ReadInt(unsigned char address);
+    void writeRegister(int deviceAddress, byte address, byte val);
+    int readRegister(int deviceAddress, byte address);
+};
+
+#endif
diff --git a/Arduino/MPU9250/MPU9250.cpp b/Arduino/MPU9250/MPU9250.cpp
new file mode 100644
index 00000000..62d71fcb
--- /dev/null
+++ b/Arduino/MPU9250/MPU9250.cpp
@@ -0,0 +1,3167 @@
+// I2Cdev library collection - MPU9250 I2C device class
+// Based on InvenSense MPU-9250 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 8/24/2011 by Jeff Rowberg 
+// 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 "MPU9250.h"
+
+/** Default constructor, uses default I2C address.
+ * @see MPU9250_DEFAULT_ADDRESS
+ */
+MPU9250::MPU9250() {
+    devAddr = MPU9250_DEFAULT_ADDRESS;
+}
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU9250_DEFAULT_ADDRESS
+ * @see MPU9250_ADDRESS_AD0_LOW
+ * @see MPU9250_ADDRESS_AD0_HIGH
+ */
+MPU9250::MPU9250(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 MPU9250::initialize() {
+    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
+    setClockSource(MPU9250_CLOCK_PLL_XGYRO);
+    setFullScaleGyroRange(MPU9250_GYRO_FS_250);
+    setFullScaleAccelRange(MPU9250_ACCEL_FS_2);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool MPU9250::testConnection() {
+    return getDeviceID() == 0x71;
+}
+
+// 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 MPU9250::getAuxVDDIOLevel() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_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 MPU9250::setAuxVDDIOLevel(uint8_t level) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_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-9250 Product Specification document.
+ *
+ * @return Current sample rate
+ * @see MPU9250_RA_SMPLRT_DIV
+ */
+uint8_t MPU9250::getRate() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_SMPLRT_DIV, buffer);
+    return buffer[0];
+}
+/** Set gyroscope sample rate divider.
+ * @param rate New sample rate divider
+ * @see getRate()
+ * @see MPU9250_RA_SMPLRT_DIV
+ */
+void MPU9250::setRate(uint8_t rate) {
+    I2Cdev::writeByte(devAddr, MPU9250_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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU9250::getExternalFrameSync() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_CONFIG, MPU9250_CFG_EXT_SYNC_SET_BIT, MPU9250_CFG_EXT_SYNC_SET_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set external FSYNC configuration.
+ * @see getExternalFrameSync()
+ * @see MPU9250_RA_CONFIG
+ * @param sync New FSYNC configuration value
+ */
+void MPU9250::setExternalFrameSync(uint8_t sync) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_CONFIG, MPU9250_CFG_EXT_SYNC_SET_BIT, MPU9250_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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @return DLFP configuration
+ * @see MPU9250_RA_CONFIG
+ * @see MPU9250_CFG_DLPF_CFG_BIT
+ * @see MPU9250_CFG_DLPF_CFG_LENGTH
+ */
+uint8_t MPU9250::getDLPFMode() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_CONFIG, MPU9250_CFG_DLPF_CFG_BIT, MPU9250_CFG_DLPF_CFG_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set digital low-pass filter configuration.
+ * @param mode New DLFP configuration setting
+ * @see getDLPFBandwidth()
+ * @see MPU9250_DLPF_BW_256
+ * @see MPU9250_RA_CONFIG
+ * @see MPU9250_CFG_DLPF_CFG_BIT
+ * @see MPU9250_CFG_DLPF_CFG_LENGTH
+ */
+void MPU9250::setDLPFMode(uint8_t mode) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_CONFIG, MPU9250_CFG_DLPF_CFG_BIT, MPU9250_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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @return Current full-scale gyroscope range setting
+ * @see MPU9250_GYRO_FS_250
+ * @see MPU9250_RA_GYRO_CONFIG
+ * @see MPU9250_GCONFIG_FS_SEL_BIT
+ * @see MPU9250_GCONFIG_FS_SEL_LENGTH
+ */
+uint8_t MPU9250::getFullScaleGyroRange() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_GYRO_CONFIG, MPU9250_GCONFIG_FS_SEL_BIT, MPU9250_GCONFIG_FS_SEL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set full-scale gyroscope range.
+ * @param range New full-scale gyroscope range value
+ * @see getFullScaleRange()
+ * @see MPU9250_GYRO_FS_250
+ * @see MPU9250_RA_GYRO_CONFIG
+ * @see MPU9250_GCONFIG_FS_SEL_BIT
+ * @see MPU9250_GCONFIG_FS_SEL_LENGTH
+ */
+void MPU9250::setFullScaleGyroRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_GYRO_CONFIG, MPU9250_GCONFIG_FS_SEL_BIT, MPU9250_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// ACCEL_CONFIG register
+
+/** Get self-test enabled setting for accelerometer X axis.
+ * @return Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+bool MPU9250::getAccelXSelfTest() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_XA_ST_BIT, buffer);
+    return buffer[0];
+}
+/** Get self-test enabled setting for accelerometer X axis.
+ * @param enabled Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+void MPU9250::setAccelXSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_XA_ST_BIT, enabled);
+}
+/** Get self-test enabled value for accelerometer Y axis.
+ * @return Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+bool MPU9250::getAccelYSelfTest() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_YA_ST_BIT, buffer);
+    return buffer[0];
+}
+/** Get self-test enabled value for accelerometer Y axis.
+ * @param enabled Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+void MPU9250::setAccelYSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_YA_ST_BIT, enabled);
+}
+/** Get self-test enabled value for accelerometer Z axis.
+ * @return Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+bool MPU9250::getAccelZSelfTest() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_ZA_ST_BIT, buffer);
+    return buffer[0];
+}
+/** Set self-test enabled value for accelerometer Z axis.
+ * @param enabled Self-test enabled value
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+void MPU9250::setAccelZSelfTest(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @return Current full-scale accelerometer range setting
+ * @see MPU9250_ACCEL_FS_2
+ * @see MPU9250_RA_ACCEL_CONFIG
+ * @see MPU9250_ACONFIG_AFS_SEL_BIT
+ * @see MPU9250_ACONFIG_AFS_SEL_LENGTH
+ */
+uint8_t MPU9250::getFullScaleAccelRange() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_AFS_SEL_BIT, MPU9250_ACONFIG_AFS_SEL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set full-scale accelerometer range.
+ * @param range New full-scale accelerometer range setting
+ * @see getFullScaleAccelRange()
+ */
+void MPU9250::setFullScaleAccelRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_AFS_SEL_BIT, MPU9250_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-9250 Product Specification document).
+ *
+ * The high pass filter has three modes:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU9250_DHPF_RESET
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+uint8_t MPU9250::getDHPFMode() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_ACCEL_HPF_BIT, MPU9250_ACONFIG_ACCEL_HPF_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set the high-pass filter configuration.
+ * @param bandwidth New high-pass filter configuration
+ * @see setDHPFMode()
+ * @see MPU9250_DHPF_RESET
+ * @see MPU9250_RA_ACCEL_CONFIG
+ */
+void MPU9250::setDHPFMode(uint8_t bandwidth) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_ACCEL_CONFIG, MPU9250_ACONFIG_ACCEL_HPF_BIT, MPU9250_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-9250 Product Specification document as well as Registers 56 and
+ * 58 of this document.
+ *
+ * @return Current free-fall acceleration threshold value (LSB = 2mg)
+ * @see MPU9250_RA_FF_THR
+ */
+uint8_t MPU9250::getFreefallDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_FF_THR
+ */
+void MPU9250::setFreefallDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 Product Specification document as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current free-fall duration threshold value (LSB = 1ms)
+ * @see MPU9250_RA_FF_DUR
+ */
+uint8_t MPU9250::getFreefallDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_FF_DUR
+ */
+void MPU9250::setFreefallDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 Product Specification document as well as Registers 56 and
+ * 58 of this document.
+ *
+ * @return Current motion detection acceleration threshold value (LSB = 2mg)
+ * @see MPU9250_RA_MOT_THR
+ */
+uint8_t MPU9250::getMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_MOT_THR, buffer);
+    return buffer[0];
+}
+/** Set free-fall event acceleration threshold.
+ * @param threshold New motion detection acceleration threshold value (LSB = 2mg)
+ * @see getMotionDetectionThreshold()
+ * @see MPU9250_RA_MOT_THR
+ */
+void MPU9250::setMotionDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 Product Specification document.
+ *
+ * @return Current motion detection duration threshold value (LSB = 1ms)
+ * @see MPU9250_RA_MOT_DUR
+ */
+uint8_t MPU9250::getMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_MOT_DUR
+ */
+void MPU9250::setMotionDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 Product Specification document as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current zero motion detection acceleration threshold value (LSB = 2mg)
+ * @see MPU9250_RA_ZRMOT_THR
+ */
+uint8_t MPU9250::getZeroMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_ZRMOT_THR
+ */
+void MPU9250::setZeroMotionDetectionThreshold(uint8_t threshold) {
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 Product Specification document, as well as Registers 56
+ * and 58 of this document.
+ *
+ * @return Current zero motion detection duration threshold value (LSB = 64ms)
+ * @see MPU9250_RA_ZRMOT_DUR
+ */
+uint8_t MPU9250::getZeroMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_ZRMOT_DUR
+ */
+void MPU9250::setZeroMotionDetectionDuration(uint8_t duration) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getTempFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_TEMP_FIFO_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set temperature FIFO enabled value.
+ * @param enabled New temperature FIFO enabled value
+ * @see getTempFIFOEnabled()
+ * @see MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setTempFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getXGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setXGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getYGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setYGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getZGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setZGyroFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getAccelFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_ACCEL_FIFO_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set accelerometer FIFO enabled value.
+ * @param enabled New accelerometer FIFO enabled value
+ * @see getAccelFIFOEnabled()
+ * @see MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setAccelFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getSlave2FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setSlave2FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getSlave1FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setSlave1FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+bool MPU9250::getSlave0FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_FIFO_EN
+ */
+void MPU9250::setSlave0FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_FIFO_EN, MPU9250_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 MPU9250_RA_I2C_MST_CTRL
+ */
+bool MPU9250::getMultiMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_MULT_MST_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set multi-master enabled value.
+ * @param enabled New multi-master enabled value
+ * @see getMultiMasterEnabled()
+ * @see MPU9250_RA_I2C_MST_CTRL
+ */
+void MPU9250::setMultiMasterEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_CTRL
+ */
+bool MPU9250::getWaitForExternalSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_CTRL
+ */
+void MPU9250::setWaitForExternalSensorEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_MST_CTRL
+ */
+bool MPU9250::getSlave3FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_MST_CTRL
+ */
+void MPU9250::setSlave3FIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_CTRL
+ */
+bool MPU9250::getSlaveReadWriteTransitionEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_CTRL
+ */
+void MPU9250::setSlaveReadWriteTransitionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU9250_RA_I2C_MST_CTRL
+ */
+uint8_t MPU9250::getMasterClockSpeed() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_I2C_MST_CLK_BIT, MPU9250_I2C_MST_CLK_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set I2C master clock speed.
+ * @reparam speed Current I2C master clock speed
+ * @see MPU9250_RA_I2C_MST_CTRL
+ */
+void MPU9250::setMasterClockSpeed(uint8_t speed) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_I2C_MST_CTRL, MPU9250_I2C_MST_CLK_BIT, MPU9250_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-9250 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 MPU9250_RA_I2C_SLV0_ADDR
+ */
+uint8_t MPU9250::getSlaveAddress(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV0_ADDR
+ */
+void MPU9250::setSlaveAddress(uint8_t num, uint8_t address) {
+    if (num > 3) return;
+    I2Cdev::writeByte(devAddr, MPU9250_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-9250 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 MPU9250_RA_I2C_SLV0_REG
+ */
+uint8_t MPU9250::getSlaveRegister(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV0_REG
+ */
+void MPU9250::setSlaveRegister(uint8_t num, uint8_t reg) {
+    if (num > 3) return;
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+bool MPU9250::getSlaveEnabled(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+void MPU9250::setSlaveEnabled(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+bool MPU9250::getSlaveWordByteSwap(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+void MPU9250::setSlaveWordByteSwap(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+bool MPU9250::getSlaveWriteMode(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+void MPU9250::setSlaveWriteMode(uint8_t num, bool mode) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+bool MPU9250::getSlaveWordGroupOffset(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+void MPU9250::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+uint8_t MPU9250::getSlaveDataLength(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBits(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_I2C_SLV_LEN_BIT, MPU9250_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 MPU9250_RA_I2C_SLV0_CTRL
+ */
+void MPU9250::setSlaveDataLength(uint8_t num, uint8_t length) {
+    if (num > 3) return;
+    I2Cdev::writeBits(devAddr, MPU9250_RA_I2C_SLV0_CTRL + num*3, MPU9250_I2C_SLV_LEN_BIT, MPU9250_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 MPU9250_RA_I2C_SLV4_ADDR
+ */
+uint8_t MPU9250::getSlave4Address() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV4_ADDR
+ */
+void MPU9250::setSlave4Address(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV4_REG
+ */
+uint8_t MPU9250::getSlave4Register() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV4_REG
+ */
+void MPU9250::setSlave4Register(uint8_t reg) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV4_DO
+ */
+void MPU9250::setSlave4OutputByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+bool MPU9250::getSlave4Enabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+void MPU9250::setSlave4Enabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+bool MPU9250::getSlave4InterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+void MPU9250::setSlave4InterruptEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+bool MPU9250::getSlave4WriteMode() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+void MPU9250::setSlave4WriteMode(bool mode) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+uint8_t MPU9250::getSlave4MasterDelay() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_I2C_SLV4_MST_DLY_BIT, MPU9250_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 MPU9250_RA_I2C_SLV4_CTRL
+ */
+void MPU9250::setSlave4MasterDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_I2C_SLV4_CTRL, MPU9250_I2C_SLV4_MST_DLY_BIT, MPU9250_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 MPU9250_RA_I2C_SLV4_DI
+ */
+uint8_t MPU9250::getSlate4InputByte() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getPassthroughStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave4IsDone() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getLostArbitration() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave4Nack() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave3Nack() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave2Nack() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave1Nack() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_I2C_MST_STATUS
+ */
+bool MPU9250::getSlave0Nack() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_STATUS, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_LEVEL_BIT
+ */
+bool MPU9250::getInterruptMode() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_LEVEL_BIT
+ */
+void MPU9250::setInterruptMode(bool mode) {
+   I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_OPEN_BIT
+ */
+bool MPU9250::getInterruptDrive() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_OPEN_BIT
+ */
+void MPU9250::setInterruptDrive(bool drive) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_LATCH_INT_EN_BIT
+ */
+bool MPU9250::getInterruptLatch() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_LATCH_INT_EN_BIT
+ */
+void MPU9250::setInterruptLatch(bool latch) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_RD_CLEAR_BIT
+ */
+bool MPU9250::getInterruptLatchClear() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_INT_RD_CLEAR_BIT
+ */
+void MPU9250::setInterruptLatchClear(bool clear) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_FSYNC_INT_LEVEL_BIT
+ */
+bool MPU9250::getFSyncInterruptLevel() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_FSYNC_INT_LEVEL_BIT
+ */
+void MPU9250::setFSyncInterruptLevel(bool level) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_FSYNC_INT_EN_BIT
+ */
+bool MPU9250::getFSyncInterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_FSYNC_INT_EN_BIT
+ */
+void MPU9250::setFSyncInterruptEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_I2C_BYPASS_EN_BIT
+ */
+bool MPU9250::getI2CBypassEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_I2C_BYPASS_EN_BIT
+ */
+void MPU9250::setI2CBypassEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_CLKOUT_EN_BIT
+ */
+bool MPU9250::getClockOutputEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_PIN_CFG
+ * @see MPU9250_INTCFG_CLKOUT_EN_BIT
+ */
+void MPU9250::setClockOutputEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_PIN_CFG, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FF_BIT
+ **/
+uint8_t MPU9250::getIntEnabled() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FF_BIT
+ **/
+void MPU9250::setIntEnabled(uint8_t enabled) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FF_BIT
+ **/
+bool MPU9250::getIntFreefallEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_FF_BIT, buffer);
+    return buffer[0];
+}
+/** Set Free Fall interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntFreefallEnabled()
+ * @see MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FF_BIT
+ **/
+void MPU9250::setIntFreefallEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_MOT_BIT
+ **/
+bool MPU9250::getIntMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_MOT_BIT, buffer);
+    return buffer[0];
+}
+/** Set Motion Detection interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntMotionEnabled()
+ * @see MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_MOT_BIT
+ **/
+void MPU9250::setIntMotionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_ZMOT_BIT
+ **/
+bool MPU9250::getIntZeroMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_ZMOT_BIT, buffer);
+    return buffer[0];
+}
+/** Set Zero Motion Detection interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntZeroMotionEnabled()
+ * @see MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_ZMOT_BIT
+ **/
+void MPU9250::setIntZeroMotionEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FIFO_OFLOW_BIT
+ **/
+bool MPU9250::getIntFIFOBufferOverflowEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_FIFO_OFLOW_BIT, buffer);
+    return buffer[0];
+}
+/** Set FIFO Buffer Overflow interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntFIFOBufferOverflowEnabled()
+ * @see MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_FIFO_OFLOW_BIT
+ **/
+void MPU9250::setIntFIFOBufferOverflowEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_I2C_MST_INT_BIT
+ **/
+bool MPU9250::getIntI2CMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_I2C_MST_INT_BIT, buffer);
+    return buffer[0];
+}
+/** Set I2C Master interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntI2CMasterEnabled()
+ * @see MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_I2C_MST_INT_BIT
+ **/
+void MPU9250::setIntI2CMasterEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_ENABLE
+ * @see MPU9250_INTERRUPT_DATA_RDY_BIT
+ */
+bool MPU9250::getIntDataReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_DATA_RDY_BIT, buffer);
+    return buffer[0];
+}
+/** Set Data Ready interrupt enabled status.
+ * @param enabled New interrupt enabled status
+ * @see getIntDataReadyEnabled()
+ * @see MPU9250_RA_INT_CFG
+ * @see MPU9250_INTERRUPT_DATA_RDY_BIT
+ */
+void MPU9250::setIntDataReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_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 MPU9250_RA_INT_STATUS
+ */
+uint8_t MPU9250::getIntStatus() {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_FF_BIT
+ */
+bool MPU9250::getIntFreefallStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_MOT_BIT
+ */
+bool MPU9250::getIntMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_ZMOT_BIT
+ */
+bool MPU9250::getIntZeroMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_FIFO_OFLOW_BIT
+ */
+bool MPU9250::getIntFIFOBufferOverflowStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_I2C_MST_INT_BIT
+ */
+bool MPU9250::getIntI2CMasterStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_INT_STATUS
+ * @see MPU9250_INTERRUPT_DATA_RDY_BIT
+ */
+bool MPU9250::getIntDataReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_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 MPU9250_RA_ACCEL_XOUT_H
+ */
+void MPU9250::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) {
+    
+	//get accel and gyro
+	getMotion6(ax, ay, az, gx, gy, gz);
+	
+	//read mag
+	I2Cdev::writeByte(devAddr, MPU9250_RA_INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
+	delay(10);
+	I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
+	delay(10);
+	I2Cdev::readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer);
+	*mx = (((int16_t)buffer[1]) << 8) | buffer[0];
+    *my = (((int16_t)buffer[3]) << 8) | buffer[2];
+    *mz = (((int16_t)buffer[5]) << 8) | buffer[4];		
+}
+/** 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 MPU9250_RA_ACCEL_XOUT_H
+ */
+void MPU9250::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
+    I2Cdev::readBytes(devAddr, MPU9250_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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 MPU9250_RA_GYRO_XOUT_H
+ */
+void MPU9250::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_ACCEL_XOUT_H
+ */
+int16_t MPU9250::getAccelerationX() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_ACCEL_YOUT_H
+ */
+int16_t MPU9250::getAccelerationY() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_ACCEL_ZOUT_H
+ */
+int16_t MPU9250::getAccelerationZ() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_TEMP_OUT_H
+ */
+int16_t MPU9250::getTemperature() {
+    I2Cdev::readBytes(devAddr, MPU9250_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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 MPU9250_RA_GYRO_XOUT_H
+ */
+void MPU9250::getRotation(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_GYRO_XOUT_H
+ */
+int16_t MPU9250::getRotationX() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_GYRO_YOUT_H
+ */
+int16_t MPU9250::getRotationY() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250_RA_GYRO_ZOUT_H
+ */
+int16_t MPU9250::getRotationZ() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250::getExternalSensorByte(int position) {
+    I2Cdev::readByte(devAddr, MPU9250_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 MPU9250::getExternalSensorWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250::getExternalSensorDWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU9250_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 X-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_XNEG_BIT
+ */
+bool MPU9250::getXNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_XNEG_BIT, buffer);
+    return buffer[0];
+}
+/** Get X-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_XPOS_BIT
+ */
+bool MPU9250::getXPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_XPOS_BIT, buffer);
+    return buffer[0];
+}
+/** Get Y-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_YNEG_BIT
+ */
+bool MPU9250::getYNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_YNEG_BIT, buffer);
+    return buffer[0];
+}
+/** Get Y-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_YPOS_BIT
+ */
+bool MPU9250::getYPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_YPOS_BIT, buffer);
+    return buffer[0];
+}
+/** Get Z-axis negative motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_ZNEG_BIT
+ */
+bool MPU9250::getZNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_ZNEG_BIT, buffer);
+    return buffer[0];
+}
+/** Get Z-axis positive motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_ZPOS_BIT
+ */
+bool MPU9250::getZPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_MOTION_MOT_ZPOS_BIT, buffer);
+    return buffer[0];
+}
+/** Get zero motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU9250_RA_MOT_DETECT_STATUS
+ * @see MPU9250_MOTION_MOT_ZRMOT_BIT
+ */
+bool MPU9250::getZeroMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_MOT_DETECT_STATUS, MPU9250_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 MPU9250_RA_I2C_SLV0_DO
+ */
+void MPU9250::setSlaveOutputByte(uint8_t num, uint8_t data) {
+    if (num > 3) return;
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_I2C_MST_DELAY_CTRL
+ * @see MPU9250_DELAYCTRL_DELAY_ES_SHADOW_BIT
+ */
+bool MPU9250::getExternalShadowDelayEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_I2C_MST_DELAY_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_DELAY_CTRL
+ * @see MPU9250_DELAYCTRL_DELAY_ES_SHADOW_BIT
+ */
+void MPU9250::setExternalShadowDelayEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_I2C_MST_DELAY_CTRL, MPU9250_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 MPU9250_RA_I2C_MST_DELAY_CTRL
+ * @see MPU9250_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+ */
+bool MPU9250::getSlaveDelayEnabled(uint8_t num) {
+    // MPU9250_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
+    if (num > 4) return 0;
+    I2Cdev::readBit(devAddr, MPU9250_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 MPU9250_RA_I2C_MST_DELAY_CTRL
+ * @see MPU9250_DELAYCTRL_I2C_SLV0_DLY_EN_BIT
+ */
+void MPU9250::setSlaveDelayEnabled(uint8_t num, bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_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 MPU9250_RA_SIGNAL_PATH_RESET
+ * @see MPU9250_PATHRESET_GYRO_RESET_BIT
+ */
+void MPU9250::resetGyroscopePath() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_SIGNAL_PATH_RESET, MPU9250_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 MPU9250_RA_SIGNAL_PATH_RESET
+ * @see MPU9250_PATHRESET_ACCEL_RESET_BIT
+ */
+void MPU9250::resetAccelerometerPath() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_SIGNAL_PATH_RESET, MPU9250_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 MPU9250_RA_SIGNAL_PATH_RESET
+ * @see MPU9250_PATHRESET_TEMP_RESET_BIT
+ */
+void MPU9250::resetTemperaturePath() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_SIGNAL_PATH_RESET, MPU9250_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-9250 Product Specification document for
+ * further information regarding the detection modules.
+ * @return Current accelerometer power-on delay
+ * @see MPU9250_RA_MOT_DETECT_CTRL
+ * @see MPU9250_DETECT_ACCEL_ON_DELAY_BIT
+ */
+uint8_t MPU9250::getAccelerometerPowerOnDelay() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_ACCEL_ON_DELAY_BIT, MPU9250_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 MPU9250_RA_MOT_DETECT_CTRL
+ * @see MPU9250_DETECT_ACCEL_ON_DELAY_BIT
+ */
+void MPU9250::setAccelerometerPowerOnDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_ACCEL_ON_DELAY_BIT, MPU9250_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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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 MPU9250_RA_MOT_DETECT_CTRL
+ * @see MPU9250_DETECT_FF_COUNT_BIT
+ */
+uint8_t MPU9250::getFreefallDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_FF_COUNT_BIT, MPU9250_DETECT_FF_COUNT_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set Free Fall detection counter decrement configuration.
+ * @param decrement New decrement configuration value
+ * @see getFreefallDetectionCounterDecrement()
+ * @see MPU9250_RA_MOT_DETECT_CTRL
+ * @see MPU9250_DETECT_FF_COUNT_BIT
+ */
+void MPU9250::setFreefallDetectionCounterDecrement(uint8_t decrement) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_FF_COUNT_BIT, MPU9250_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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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 MPU9250::getMotionDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_MOT_COUNT_BIT, MPU9250_DETECT_MOT_COUNT_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set Motion detection counter decrement configuration.
+ * @param decrement New decrement configuration value
+ * @see getMotionDetectionCounterDecrement()
+ * @see MPU9250_RA_MOT_DETECT_CTRL
+ * @see MPU9250_DETECT_MOT_COUNT_BIT
+ */
+void MPU9250::setMotionDetectionCounterDecrement(uint8_t decrement) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_MOT_DETECT_CTRL, MPU9250_DETECT_MOT_COUNT_BIT, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_FIFO_EN_BIT
+ */
+bool MPU9250::getFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_USERCTRL_FIFO_EN_BIT, buffer);
+    return buffer[0];
+}
+/** Set FIFO enabled status.
+ * @param enabled New FIFO enabled status
+ * @see getFIFOEnabled()
+ * @see MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_FIFO_EN_BIT
+ */
+void MPU9250::setFIFOEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_I2C_MST_EN_BIT
+ */
+bool MPU9250::getI2CMasterModeEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_I2C_MST_EN_BIT
+ */
+void MPU9250::setI2CMasterModeEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250::switchSPIEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_FIFO_RESET_BIT
+ */
+void MPU9250::resetFIFO() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_I2C_MST_RESET_BIT
+ */
+void MPU9250::resetI2CMaster() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_USER_CTRL
+ * @see MPU9250_USERCTRL_SIG_COND_RESET_BIT
+ */
+void MPU9250::resetSensors() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_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 MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_DEVICE_RESET_BIT
+ */
+void MPU9250::reset() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_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 MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_SLEEP_BIT
+ */
+bool MPU9250::getSleepEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_PWR1_SLEEP_BIT, buffer);
+    return buffer[0];
+}
+/** Set sleep mode status.
+ * @param enabled New sleep mode enabled status
+ * @see getSleepEnabled()
+ * @see MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_SLEEP_BIT
+ */
+void MPU9250::setSleepEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_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 MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_CYCLE_BIT
+ */
+bool MPU9250::getWakeCycleEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_PWR1_CYCLE_BIT, buffer);
+    return buffer[0];
+}
+/** Set wake cycle enabled status.
+ * @param enabled New sleep mode enabled status
+ * @see getWakeCycleEnabled()
+ * @see MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_CYCLE_BIT
+ */
+void MPU9250::setWakeCycleEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_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 MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_TEMP_DIS_BIT
+ */
+bool MPU9250::getTempSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_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 MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_TEMP_DIS_BIT
+ */
+void MPU9250::setTempSensorEnabled(bool enabled) {
+    // 1 is actually disabled here
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_PWR1_TEMP_DIS_BIT, !enabled);
+}
+/** Get clock source setting.
+ * @return Current clock source setting
+ * @see MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_CLKSEL_BIT
+ * @see MPU9250_PWR1_CLKSEL_LENGTH
+ */
+uint8_t MPU9250::getClockSource() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_PWR1_CLKSEL_BIT, MPU9250_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @param source New clock source setting
+ * @see getClockSource()
+ * @see MPU9250_RA_PWR_MGMT_1
+ * @see MPU9250_PWR1_CLKSEL_BIT
+ * @see MPU9250_PWR1_CLKSEL_LENGTH
+ */
+void MPU9250::setClockSource(uint8_t source) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_PWR_MGMT_1, MPU9250_PWR1_CLKSEL_BIT, MPU9250_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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * For further information regarding the MPU-60X0's power modes, please refer to
+ * Register 107.
+ *
+ * @return Current wake frequency
+ * @see MPU9250_RA_PWR_MGMT_2
+ */
+uint8_t MPU9250::getWakeFrequency() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_PWR2_LP_WAKE_CTRL_BIT, MPU9250_PWR2_LP_WAKE_CTRL_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set wake frequency in Accel-Only Low Power Mode.
+ * @param frequency New wake frequency
+ * @see MPU9250_RA_PWR_MGMT_2
+ */
+void MPU9250::setWakeFrequency(uint8_t frequency) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_PWR2_LP_WAKE_CTRL_BIT, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_XA_BIT
+ */
+bool MPU9250::getStandbyXAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_XA_BIT
+ */
+void MPU9250::setStandbyXAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_YA_BIT
+ */
+bool MPU9250::getStandbyYAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_YA_BIT
+ */
+void MPU9250::setStandbyYAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_ZA_BIT
+ */
+bool MPU9250::getStandbyZAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_ZA_BIT
+ */
+void MPU9250::setStandbyZAccelEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_XG_BIT
+ */
+bool MPU9250::getStandbyXGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_XG_BIT
+ */
+void MPU9250::setStandbyXGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_YG_BIT
+ */
+bool MPU9250::getStandbyYGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_YG_BIT
+ */
+void MPU9250::setStandbyYGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_ZG_BIT
+ */
+bool MPU9250::getStandbyZGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250_RA_PWR_MGMT_2
+ * @see MPU9250_PWR2_STBY_ZG_BIT
+ */
+void MPU9250::setStandbyZGyroEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_PWR_MGMT_2, MPU9250_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 MPU9250::getFIFOCount() {
+    I2Cdev::readBytes(devAddr, MPU9250_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 MPU9250::getFIFOByte() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_FIFO_R_W, buffer);
+    return buffer[0];
+}
+void MPU9250::getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO buffer.
+ * @see getFIFOByte()
+ * @see MPU9250_RA_FIFO_R_W
+ */
+void MPU9250::setFIFOByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU9250_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 MPU9250_RA_WHO_AM_I
+ * @see MPU9250_WHO_AM_I_BIT
+ * @see MPU9250_WHO_AM_I_LENGTH
+ */
+uint8_t MPU9250::getDeviceID() {
+//	#define MPU9250_RA_WHO_AM_I         0x75
+// 	#define MPU9250_WHO_AM_I_BIT        6
+//	#define MPU9250_WHO_AM_I_LENGTH     8
+//    I2Cdev::readBits(devAddr, MPU9250_RA_WHO_AM_I, MPU9250_WHO_AM_I_BIT, MPU9250_WHO_AM_I_LENGTH, buffer);
+//    return buffer[0];
+    I2Cdev::readByte(devAddr, MPU9250_RA_WHO_AM_I, 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 MPU9250_RA_WHO_AM_I
+ * @see MPU9250_WHO_AM_I_BIT
+ * @see MPU9250_WHO_AM_I_LENGTH
+ */
+void MPU9250::setDeviceID(uint8_t id) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_WHO_AM_I, MPU9250_WHO_AM_I_BIT, MPU9250_WHO_AM_I_LENGTH, id);
+}
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+
+uint8_t MPU9250::getOTPBankValid() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_XG_OFFS_TC, MPU9250_TC_OTP_BNK_VLD_BIT, buffer);
+    return buffer[0];
+}
+void MPU9250::setOTPBankValid(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_XG_OFFS_TC, MPU9250_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU9250::getXGyroOffset() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_XG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9250::setXGyroOffset(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_XG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, offset);
+}
+
+// YG_OFFS_TC register
+
+int8_t MPU9250::getYGyroOffset() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9250::setYGyroOffset(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_YG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, offset);
+}
+
+// ZG_OFFS_TC register
+
+int8_t MPU9250::getZGyroOffset() {
+    I2Cdev::readBits(devAddr, MPU9250_RA_ZG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, buffer);
+    return buffer[0];
+}
+void MPU9250::setZGyroOffset(int8_t offset) {
+    I2Cdev::writeBits(devAddr, MPU9250_RA_ZG_OFFS_TC, MPU9250_TC_OFFSET_BIT, MPU9250_TC_OFFSET_LENGTH, offset);
+}
+
+// X_FINE_GAIN register
+
+int8_t MPU9250::getXFineGain() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_X_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9250::setXFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU9250::getYFineGain() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_Y_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9250::setYFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU9250::getZFineGain() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_Z_FINE_GAIN, buffer);
+    return buffer[0];
+}
+void MPU9250::setZFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU9250::getXAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_XA_OFFS_H, 2, buffer);
+    return ((((int16_t)buffer[0]) << 8) | buffer[1]);
+}
+void MPU9250::setXAccelOffset(int16_t offset) {
+    int16_t bit0 = getXAccelOffset() & 1;
+    int16_t value = offset & 0xFFFE | bit0;
+    
+    I2Cdev::writeWord(devAddr, MPU9250_RA_XA_OFFS_H, value);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU9250::getYAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_YA_OFFS_H, 2, buffer);
+    return ((((int16_t)buffer[0]) << 8) | buffer[1]);
+}
+void MPU9250::setYAccelOffset(int16_t offset) {
+    int16_t bit0 = getYAccelOffset() & 1;
+    int16_t value = offset & 0xFFFE | bit0;
+
+    I2Cdev::writeWord(devAddr, MPU9250_RA_YA_OFFS_H, value);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU9250::getZAccelOffset() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_ZA_OFFS_H, 2, buffer);
+    return ((((int16_t)buffer[0]) << 8) | buffer[1]);
+}
+void MPU9250::setZAccelOffset(int16_t offset) {
+    int16_t bit0 = getZAccelOffset() & 1;
+    int16_t value = offset & 0xFFFE | bit0;
+
+    I2Cdev::writeWord(devAddr, MPU9250_RA_ZA_OFFS_H, value);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU9250::getXGyroOffsetUser() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_XG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9250::setXGyroOffsetUser(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9250_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU9250::getYGyroOffsetUser() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_YG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9250::setYGyroOffsetUser(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9250_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU9250::getZGyroOffsetUser() {
+    I2Cdev::readBytes(devAddr, MPU9250_RA_ZG_OFFS_USRH, 2, buffer);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU9250::setZGyroOffsetUser(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU9250_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU9250::getIntPLLReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+    return buffer[0];
+}
+void MPU9250::setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU9250::getIntDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_DMP_INT_BIT, buffer);
+    return buffer[0];
+}
+void MPU9250::setIntDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_INT_ENABLE, MPU9250_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU9250::getDMPInt5Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_5_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getDMPInt4Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_4_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getDMPInt3Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_3_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getDMPInt2Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_2_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getDMPInt1Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_1_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getDMPInt0Status() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_DMP_INT_STATUS, MPU9250_DMPINT_0_BIT, buffer);
+    return buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU9250::getIntPLLReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_INTERRUPT_PLL_RDY_INT_BIT, buffer);
+    return buffer[0];
+}
+bool MPU9250::getIntDMPStatus() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_INT_STATUS, MPU9250_INTERRUPT_DMP_INT_BIT, buffer);
+    return buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU9250::getDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_USERCTRL_DMP_EN_BIT, buffer);
+    return buffer[0];
+}
+void MPU9250::setDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU9250::resetDMP() {
+    I2Cdev::writeBit(devAddr, MPU9250_RA_USER_CTRL, MPU9250_USERCTRL_DMP_RESET_BIT, true);
+}
+
+// BANK_SEL register
+
+void MPU9250::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
+    bank &= 0x1F;
+    if (userBank) bank |= 0x20;
+    if (prefetchEnabled) bank |= 0x40;
+    I2Cdev::writeByte(devAddr, MPU9250_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU9250::setMemoryStartAddress(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU9250::readMemoryByte() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_MEM_R_W, buffer);
+    return buffer[0];
+}
+void MPU9250::writeMemoryByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_MEM_R_W, data);
+}
+void MPU9250::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 = MPU9250_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, MPU9250_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 MPU9250::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;
+    uint8_t *progBuffer;
+    uint16_t i;
+    uint8_t j;
+    if (verify) verifyBuffer = (uint8_t *)malloc(MPU9250_DMP_MEMORY_CHUNK_SIZE);
+    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU9250_DMP_MEMORY_CHUNK_SIZE);
+    for (i = 0; i < dataSize;) {
+        // determine correct chunk size according to bank position and data size
+        chunkSize = MPU9250_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, MPU9250_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            I2Cdev::readBytes(devAddr, MPU9250_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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 MPU9250::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
+    return writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU9250::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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, MPU9250_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 MPU9250::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
+    return writeDMPConfigurationSet(data, dataSize, true);
+}
+
+// DMP_CFG_1 register
+
+uint8_t MPU9250::getDMPConfig1() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_DMP_CFG_1, buffer);
+    return buffer[0];
+}
+void MPU9250::setDMPConfig1(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU9250::getDMPConfig2() {
+    I2Cdev::readByte(devAddr, MPU9250_RA_DMP_CFG_2, buffer);
+    return buffer[0];
+}
+void MPU9250::setDMPConfig2(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU9250_RA_DMP_CFG_2, config);
+}
\ No newline at end of file
diff --git a/Arduino/MPU9250/MPU9250.h b/Arduino/MPU9250/MPU9250.h
new file mode 100644
index 00000000..afd59daa
--- /dev/null
+++ b/Arduino/MPU9250/MPU9250.h
@@ -0,0 +1,1004 @@
+// I2Cdev library collection - MPU9250 I2C device class
+// Based on InvenSense MPU-9250 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 10/3/2011 by Jeff Rowberg 
+// 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 _MPU9250_H_
+#define _MPU9250_H_
+
+#include "I2Cdev.h"
+#include 
+
+//Magnetometer Registers
+#define MPU9150_RA_MAG_ADDRESS		0x0C
+#define MPU9150_RA_MAG_XOUT_L		0x03
+#define MPU9150_RA_MAG_XOUT_H		0x04
+#define MPU9150_RA_MAG_YOUT_L		0x05
+#define MPU9150_RA_MAG_YOUT_H		0x06
+#define MPU9150_RA_MAG_ZOUT_L		0x07
+#define MPU9150_RA_MAG_ZOUT_H		0x08
+
+#define MPU9250_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
+#define MPU9250_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
+#define MPU9250_DEFAULT_ADDRESS     MPU9250_ADDRESS_AD0_LOW
+
+#define MPU9250_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9250_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9250_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
+#define MPU9250_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN
+#define MPU9250_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN
+#define MPU9250_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN
+
+#define MPU9250_RA_XA_OFFS_H        0x77 //[15:0] XA_OFFS
+#define MPU9250_RA_XA_OFFS_L_TC     0x78
+#define MPU9250_RA_YA_OFFS_H        0x7A //[15:0] YA_OFFS
+#define MPU9250_RA_YA_OFFS_L_TC     0x7B
+#define MPU9250_RA_ZA_OFFS_H        0x7D //[15:0] ZA_OFFS
+#define MPU9250_RA_ZA_OFFS_L_TC     0x7E
+
+//#define MPU9250_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
+//#define MPU9250_RA_XA_OFFS_L_TC     0x07
+//#define MPU9250_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS
+//#define MPU9250_RA_YA_OFFS_L_TC     0x09
+//#define MPU9250_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
+//#define MPU9250_RA_ZA_OFFS_L_TC     0x0B
+
+#define MPU9250_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
+#define MPU9250_RA_XG_OFFS_USRL     0x14
+#define MPU9250_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR
+#define MPU9250_RA_YG_OFFS_USRL     0x16
+#define MPU9250_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR
+#define MPU9250_RA_ZG_OFFS_USRL     0x18
+
+#define MPU9250_RA_SMPLRT_DIV       0x19
+#define MPU9250_RA_CONFIG           0x1A
+#define MPU9250_RA_GYRO_CONFIG      0x1B
+#define MPU9250_RA_ACCEL_CONFIG     0x1C
+#define MPU9250_RA_FF_THR           0x1D
+#define MPU9250_RA_FF_DUR           0x1E
+#define MPU9250_RA_MOT_THR          0x1F
+#define MPU9250_RA_MOT_DUR          0x20
+#define MPU9250_RA_ZRMOT_THR        0x21
+#define MPU9250_RA_ZRMOT_DUR        0x22
+#define MPU9250_RA_FIFO_EN          0x23
+#define MPU9250_RA_I2C_MST_CTRL     0x24
+#define MPU9250_RA_I2C_SLV0_ADDR    0x25
+#define MPU9250_RA_I2C_SLV0_REG     0x26
+#define MPU9250_RA_I2C_SLV0_CTRL    0x27
+#define MPU9250_RA_I2C_SLV1_ADDR    0x28
+#define MPU9250_RA_I2C_SLV1_REG     0x29
+#define MPU9250_RA_I2C_SLV1_CTRL    0x2A
+#define MPU9250_RA_I2C_SLV2_ADDR    0x2B
+#define MPU9250_RA_I2C_SLV2_REG     0x2C
+#define MPU9250_RA_I2C_SLV2_CTRL    0x2D
+#define MPU9250_RA_I2C_SLV3_ADDR    0x2E
+#define MPU9250_RA_I2C_SLV3_REG     0x2F
+#define MPU9250_RA_I2C_SLV3_CTRL    0x30
+#define MPU9250_RA_I2C_SLV4_ADDR    0x31
+#define MPU9250_RA_I2C_SLV4_REG     0x32
+#define MPU9250_RA_I2C_SLV4_DO      0x33
+#define MPU9250_RA_I2C_SLV4_CTRL    0x34
+#define MPU9250_RA_I2C_SLV4_DI      0x35
+#define MPU9250_RA_I2C_MST_STATUS   0x36
+#define MPU9250_RA_INT_PIN_CFG      0x37
+#define MPU9250_RA_INT_ENABLE       0x38
+#define MPU9250_RA_DMP_INT_STATUS   0x39
+#define MPU9250_RA_INT_STATUS       0x3A
+#define MPU9250_RA_ACCEL_XOUT_H     0x3B
+#define MPU9250_RA_ACCEL_XOUT_L     0x3C
+#define MPU9250_RA_ACCEL_YOUT_H     0x3D
+#define MPU9250_RA_ACCEL_YOUT_L     0x3E
+#define MPU9250_RA_ACCEL_ZOUT_H     0x3F
+#define MPU9250_RA_ACCEL_ZOUT_L     0x40
+#define MPU9250_RA_TEMP_OUT_H       0x41
+#define MPU9250_RA_TEMP_OUT_L       0x42
+#define MPU9250_RA_GYRO_XOUT_H      0x43
+#define MPU9250_RA_GYRO_XOUT_L      0x44
+#define MPU9250_RA_GYRO_YOUT_H      0x45
+#define MPU9250_RA_GYRO_YOUT_L      0x46
+#define MPU9250_RA_GYRO_ZOUT_H      0x47
+#define MPU9250_RA_GYRO_ZOUT_L      0x48
+#define MPU9250_RA_EXT_SENS_DATA_00 0x49
+#define MPU9250_RA_EXT_SENS_DATA_01 0x4A
+#define MPU9250_RA_EXT_SENS_DATA_02 0x4B
+#define MPU9250_RA_EXT_SENS_DATA_03 0x4C
+#define MPU9250_RA_EXT_SENS_DATA_04 0x4D
+#define MPU9250_RA_EXT_SENS_DATA_05 0x4E
+#define MPU9250_RA_EXT_SENS_DATA_06 0x4F
+#define MPU9250_RA_EXT_SENS_DATA_07 0x50
+#define MPU9250_RA_EXT_SENS_DATA_08 0x51
+#define MPU9250_RA_EXT_SENS_DATA_09 0x52
+#define MPU9250_RA_EXT_SENS_DATA_10 0x53
+#define MPU9250_RA_EXT_SENS_DATA_11 0x54
+#define MPU9250_RA_EXT_SENS_DATA_12 0x55
+#define MPU9250_RA_EXT_SENS_DATA_13 0x56
+#define MPU9250_RA_EXT_SENS_DATA_14 0x57
+#define MPU9250_RA_EXT_SENS_DATA_15 0x58
+#define MPU9250_RA_EXT_SENS_DATA_16 0x59
+#define MPU9250_RA_EXT_SENS_DATA_17 0x5A
+#define MPU9250_RA_EXT_SENS_DATA_18 0x5B
+#define MPU9250_RA_EXT_SENS_DATA_19 0x5C
+#define MPU9250_RA_EXT_SENS_DATA_20 0x5D
+#define MPU9250_RA_EXT_SENS_DATA_21 0x5E
+#define MPU9250_RA_EXT_SENS_DATA_22 0x5F
+#define MPU9250_RA_EXT_SENS_DATA_23 0x60
+#define MPU9250_RA_MOT_DETECT_STATUS    0x61
+#define MPU9250_RA_I2C_SLV0_DO      0x63
+#define MPU9250_RA_I2C_SLV1_DO      0x64
+#define MPU9250_RA_I2C_SLV2_DO      0x65
+#define MPU9250_RA_I2C_SLV3_DO      0x66
+#define MPU9250_RA_I2C_MST_DELAY_CTRL   0x67
+#define MPU9250_RA_SIGNAL_PATH_RESET    0x68
+#define MPU9250_RA_MOT_DETECT_CTRL      0x69
+#define MPU9250_RA_USER_CTRL        0x6A
+#define MPU9250_RA_PWR_MGMT_1       0x6B
+#define MPU9250_RA_PWR_MGMT_2       0x6C
+#define MPU9250_RA_BANK_SEL         0x6D
+#define MPU9250_RA_MEM_START_ADDR   0x6E
+#define MPU9250_RA_MEM_R_W          0x6F
+#define MPU9250_RA_DMP_CFG_1        0x70
+#define MPU9250_RA_DMP_CFG_2        0x71
+#define MPU9250_RA_FIFO_COUNTH      0x72
+#define MPU9250_RA_FIFO_COUNTL      0x73
+#define MPU9250_RA_FIFO_R_W         0x74
+#define MPU9250_RA_WHO_AM_I         0x75
+
+#define MPU9250_TC_PWR_MODE_BIT     7
+#define MPU9250_TC_OFFSET_BIT       6
+#define MPU9250_TC_OFFSET_LENGTH    6
+#define MPU9250_TC_OTP_BNK_VLD_BIT  0
+
+#define MPU9250_VDDIO_LEVEL_VLOGIC  0
+#define MPU9250_VDDIO_LEVEL_VDD     1
+
+#define MPU9250_CFG_EXT_SYNC_SET_BIT    5
+#define MPU9250_CFG_EXT_SYNC_SET_LENGTH 3
+#define MPU9250_CFG_DLPF_CFG_BIT    2
+#define MPU9250_CFG_DLPF_CFG_LENGTH 3
+
+#define MPU9250_EXT_SYNC_DISABLED       0x0
+#define MPU9250_EXT_SYNC_TEMP_OUT_L     0x1
+#define MPU9250_EXT_SYNC_GYRO_XOUT_L    0x2
+#define MPU9250_EXT_SYNC_GYRO_YOUT_L    0x3
+#define MPU9250_EXT_SYNC_GYRO_ZOUT_L    0x4
+#define MPU9250_EXT_SYNC_ACCEL_XOUT_L   0x5
+#define MPU9250_EXT_SYNC_ACCEL_YOUT_L   0x6
+#define MPU9250_EXT_SYNC_ACCEL_ZOUT_L   0x7
+
+#define MPU9250_DLPF_BW_256         0x00
+#define MPU9250_DLPF_BW_188         0x01
+#define MPU9250_DLPF_BW_98          0x02
+#define MPU9250_DLPF_BW_42          0x03
+#define MPU9250_DLPF_BW_20          0x04
+#define MPU9250_DLPF_BW_10          0x05
+#define MPU9250_DLPF_BW_5           0x06
+
+#define MPU9250_GCONFIG_FS_SEL_BIT      4
+#define MPU9250_GCONFIG_FS_SEL_LENGTH   2
+
+#define MPU9250_GYRO_FS_250         0x00
+#define MPU9250_GYRO_FS_500         0x01
+#define MPU9250_GYRO_FS_1000        0x02
+#define MPU9250_GYRO_FS_2000        0x03
+
+#define MPU9250_ACONFIG_XA_ST_BIT           7
+#define MPU9250_ACONFIG_YA_ST_BIT           6
+#define MPU9250_ACONFIG_ZA_ST_BIT           5
+#define MPU9250_ACONFIG_AFS_SEL_BIT         4
+#define MPU9250_ACONFIG_AFS_SEL_LENGTH      2
+#define MPU9250_ACONFIG_ACCEL_HPF_BIT       2
+#define MPU9250_ACONFIG_ACCEL_HPF_LENGTH    3
+
+#define MPU9250_ACCEL_FS_2          0x00
+#define MPU9250_ACCEL_FS_4          0x01
+#define MPU9250_ACCEL_FS_8          0x02
+#define MPU9250_ACCEL_FS_16         0x03
+
+#define MPU9250_DHPF_RESET          0x00
+#define MPU9250_DHPF_5              0x01
+#define MPU9250_DHPF_2P5            0x02
+#define MPU9250_DHPF_1P25           0x03
+#define MPU9250_DHPF_0P63           0x04
+#define MPU9250_DHPF_HOLD           0x07
+
+#define MPU9250_TEMP_FIFO_EN_BIT    7
+#define MPU9250_XG_FIFO_EN_BIT      6
+#define MPU9250_YG_FIFO_EN_BIT      5
+#define MPU9250_ZG_FIFO_EN_BIT      4
+#define MPU9250_ACCEL_FIFO_EN_BIT   3
+#define MPU9250_SLV2_FIFO_EN_BIT    2
+#define MPU9250_SLV1_FIFO_EN_BIT    1
+#define MPU9250_SLV0_FIFO_EN_BIT    0
+
+#define MPU9250_MULT_MST_EN_BIT     7
+#define MPU9250_WAIT_FOR_ES_BIT     6
+#define MPU9250_SLV_3_FIFO_EN_BIT   5
+#define MPU9250_I2C_MST_P_NSR_BIT   4
+#define MPU9250_I2C_MST_CLK_BIT     3
+#define MPU9250_I2C_MST_CLK_LENGTH  4
+
+#define MPU9250_CLOCK_DIV_348       0x0
+#define MPU9250_CLOCK_DIV_333       0x1
+#define MPU9250_CLOCK_DIV_320       0x2
+#define MPU9250_CLOCK_DIV_308       0x3
+#define MPU9250_CLOCK_DIV_296       0x4
+#define MPU9250_CLOCK_DIV_286       0x5
+#define MPU9250_CLOCK_DIV_276       0x6
+#define MPU9250_CLOCK_DIV_267       0x7
+#define MPU9250_CLOCK_DIV_258       0x8
+#define MPU9250_CLOCK_DIV_500       0x9
+#define MPU9250_CLOCK_DIV_471       0xA
+#define MPU9250_CLOCK_DIV_444       0xB
+#define MPU9250_CLOCK_DIV_421       0xC
+#define MPU9250_CLOCK_DIV_400       0xD
+#define MPU9250_CLOCK_DIV_381       0xE
+#define MPU9250_CLOCK_DIV_364       0xF
+
+#define MPU9250_I2C_SLV_RW_BIT      7
+#define MPU9250_I2C_SLV_ADDR_BIT    6
+#define MPU9250_I2C_SLV_ADDR_LENGTH 7
+#define MPU9250_I2C_SLV_EN_BIT      7
+#define MPU9250_I2C_SLV_BYTE_SW_BIT 6
+#define MPU9250_I2C_SLV_REG_DIS_BIT 5
+#define MPU9250_I2C_SLV_GRP_BIT     4
+#define MPU9250_I2C_SLV_LEN_BIT     3
+#define MPU9250_I2C_SLV_LEN_LENGTH  4
+
+#define MPU9250_I2C_SLV4_RW_BIT         7
+#define MPU9250_I2C_SLV4_ADDR_BIT       6
+#define MPU9250_I2C_SLV4_ADDR_LENGTH    7
+#define MPU9250_I2C_SLV4_EN_BIT         7
+#define MPU9250_I2C_SLV4_INT_EN_BIT     6
+#define MPU9250_I2C_SLV4_REG_DIS_BIT    5
+#define MPU9250_I2C_SLV4_MST_DLY_BIT    4
+#define MPU9250_I2C_SLV4_MST_DLY_LENGTH 5
+
+#define MPU9250_MST_PASS_THROUGH_BIT    7
+#define MPU9250_MST_I2C_SLV4_DONE_BIT   6
+#define MPU9250_MST_I2C_LOST_ARB_BIT    5
+#define MPU9250_MST_I2C_SLV4_NACK_BIT   4
+#define MPU9250_MST_I2C_SLV3_NACK_BIT   3
+#define MPU9250_MST_I2C_SLV2_NACK_BIT   2
+#define MPU9250_MST_I2C_SLV1_NACK_BIT   1
+#define MPU9250_MST_I2C_SLV0_NACK_BIT   0
+
+#define MPU9250_INTCFG_INT_LEVEL_BIT        7
+#define MPU9250_INTCFG_INT_OPEN_BIT         6
+#define MPU9250_INTCFG_LATCH_INT_EN_BIT     5
+#define MPU9250_INTCFG_INT_RD_CLEAR_BIT     4
+#define MPU9250_INTCFG_FSYNC_INT_LEVEL_BIT  3
+#define MPU9250_INTCFG_FSYNC_INT_EN_BIT     2
+#define MPU9250_INTCFG_I2C_BYPASS_EN_BIT    1
+#define MPU9250_INTCFG_CLKOUT_EN_BIT        0
+
+#define MPU9250_INTMODE_ACTIVEHIGH  0x00
+#define MPU9250_INTMODE_ACTIVELOW   0x01
+
+#define MPU9250_INTDRV_PUSHPULL     0x00
+#define MPU9250_INTDRV_OPENDRAIN    0x01
+
+#define MPU9250_INTLATCH_50USPULSE  0x00
+#define MPU9250_INTLATCH_WAITCLEAR  0x01
+
+#define MPU9250_INTCLEAR_STATUSREAD 0x00
+#define MPU9250_INTCLEAR_ANYREAD    0x01
+
+#define MPU9250_INTERRUPT_FF_BIT            7
+#define MPU9250_INTERRUPT_MOT_BIT           6
+#define MPU9250_INTERRUPT_ZMOT_BIT          5
+#define MPU9250_INTERRUPT_FIFO_OFLOW_BIT    4
+#define MPU9250_INTERRUPT_I2C_MST_INT_BIT   3
+#define MPU9250_INTERRUPT_PLL_RDY_INT_BIT   2
+#define MPU9250_INTERRUPT_DMP_INT_BIT       1
+#define MPU9250_INTERRUPT_DATA_RDY_BIT      0
+
+// TODO: figure out what these actually do
+// UMPL source code is not very obivous
+#define MPU9250_DMPINT_5_BIT            5
+#define MPU9250_DMPINT_4_BIT            4
+#define MPU9250_DMPINT_3_BIT            3
+#define MPU9250_DMPINT_2_BIT            2
+#define MPU9250_DMPINT_1_BIT            1
+#define MPU9250_DMPINT_0_BIT            0
+
+#define MPU9250_MOTION_MOT_XNEG_BIT     7
+#define MPU9250_MOTION_MOT_XPOS_BIT     6
+#define MPU9250_MOTION_MOT_YNEG_BIT     5
+#define MPU9250_MOTION_MOT_YPOS_BIT     4
+#define MPU9250_MOTION_MOT_ZNEG_BIT     3
+#define MPU9250_MOTION_MOT_ZPOS_BIT     2
+#define MPU9250_MOTION_MOT_ZRMOT_BIT    0
+
+#define MPU9250_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
+#define MPU9250_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
+#define MPU9250_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
+#define MPU9250_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
+#define MPU9250_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
+#define MPU9250_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0
+
+#define MPU9250_PATHRESET_GYRO_RESET_BIT    2
+#define MPU9250_PATHRESET_ACCEL_RESET_BIT   1
+#define MPU9250_PATHRESET_TEMP_RESET_BIT    0
+
+#define MPU9250_DETECT_ACCEL_ON_DELAY_BIT       5
+#define MPU9250_DETECT_ACCEL_ON_DELAY_LENGTH    2
+#define MPU9250_DETECT_FF_COUNT_BIT             3
+#define MPU9250_DETECT_FF_COUNT_LENGTH          2
+#define MPU9250_DETECT_MOT_COUNT_BIT            1
+#define MPU9250_DETECT_MOT_COUNT_LENGTH         2
+
+#define MPU9250_DETECT_DECREMENT_RESET  0x0
+#define MPU9250_DETECT_DECREMENT_1      0x1
+#define MPU9250_DETECT_DECREMENT_2      0x2
+#define MPU9250_DETECT_DECREMENT_4      0x3
+
+#define MPU9250_USERCTRL_DMP_EN_BIT             7
+#define MPU9250_USERCTRL_FIFO_EN_BIT            6
+#define MPU9250_USERCTRL_I2C_MST_EN_BIT         5
+#define MPU9250_USERCTRL_I2C_IF_DIS_BIT         4
+#define MPU9250_USERCTRL_DMP_RESET_BIT          3
+#define MPU9250_USERCTRL_FIFO_RESET_BIT         2
+#define MPU9250_USERCTRL_I2C_MST_RESET_BIT      1
+#define MPU9250_USERCTRL_SIG_COND_RESET_BIT     0
+
+#define MPU9250_PWR1_DEVICE_RESET_BIT   7
+#define MPU9250_PWR1_SLEEP_BIT          6
+#define MPU9250_PWR1_CYCLE_BIT          5
+#define MPU9250_PWR1_TEMP_DIS_BIT       3
+#define MPU9250_PWR1_CLKSEL_BIT         2
+#define MPU9250_PWR1_CLKSEL_LENGTH      3
+
+#define MPU9250_CLOCK_INTERNAL          0x00
+#define MPU9250_CLOCK_PLL_XGYRO         0x01
+#define MPU9250_CLOCK_PLL_YGYRO         0x02
+#define MPU9250_CLOCK_PLL_ZGYRO         0x03
+#define MPU9250_CLOCK_PLL_EXT32K        0x04
+#define MPU9250_CLOCK_PLL_EXT19M        0x05
+#define MPU9250_CLOCK_KEEP_RESET        0x07
+
+#define MPU9250_PWR2_LP_WAKE_CTRL_BIT       7
+#define MPU9250_PWR2_LP_WAKE_CTRL_LENGTH    2
+#define MPU9250_PWR2_STBY_XA_BIT            5
+#define MPU9250_PWR2_STBY_YA_BIT            4
+#define MPU9250_PWR2_STBY_ZA_BIT            3
+#define MPU9250_PWR2_STBY_XG_BIT            2
+#define MPU9250_PWR2_STBY_YG_BIT            1
+#define MPU9250_PWR2_STBY_ZG_BIT            0
+
+#define MPU9250_WAKE_FREQ_1P25      0x0
+#define MPU9250_WAKE_FREQ_2P5       0x1
+#define MPU9250_WAKE_FREQ_5         0x2
+#define MPU9250_WAKE_FREQ_10        0x3
+
+#define MPU9250_BANKSEL_PRFTCH_EN_BIT       6
+#define MPU9250_BANKSEL_CFG_USER_BANK_BIT   5
+#define MPU9250_BANKSEL_MEM_SEL_BIT         4
+#define MPU9250_BANKSEL_MEM_SEL_LENGTH      5
+
+#define MPU9250_WHO_AM_I_BIT        8
+#define MPU9250_WHO_AM_I_LENGTH     8
+
+#define MPU9250_DMP_MEMORY_BANKS        8
+#define MPU9250_DMP_MEMORY_BANK_SIZE    256
+#define MPU9250_DMP_MEMORY_CHUNK_SIZE   16
+
+// note: DMP code memory blocks defined at end of header file
+
+class MPU9250 {
+    public:
+        MPU9250();
+        MPU9250(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);
+
+        // 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
+        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 getXGyroOffset();
+        void setXGyroOffset(int8_t offset);
+
+        // YG_OFFS_TC register
+        int8_t getYGyroOffset();
+        void setYGyroOffset(int8_t offset);
+
+        // ZG_OFFS_TC register
+        int8_t getZGyroOffset();
+        void setZGyroOffset(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 getXGyroOffsetUser();
+        void setXGyroOffsetUser(int16_t offset);
+
+        // YG_OFFS_USR* register
+        int16_t getYGyroOffsetUser();
+        void setYGyroOffsetUser(int16_t offset);
+
+        // ZG_OFFS_USR* register
+        int16_t getZGyroOffsetUser();
+        void setZGyroOffsetUser(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 MPU9250_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 MPU9250_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
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[14];
+};
+
+#endif /* _MPU9250_H_ */
diff --git a/Arduino/MPU9250/README.md b/Arduino/MPU9250/README.md
new file mode 100644
index 00000000..982802b5
--- /dev/null
+++ b/Arduino/MPU9250/README.md
@@ -0,0 +1,23 @@
+# IMU_10DOF
+
+This library is for Grove - IMU 10DOF and Xadow - IMU 10DOF modules. All the mentioned modules are combination of MPU-9250 and BMP180. So this library is the combination of:
+
+* Library for MPU9250 - thanks to Jeff Rowberg , the MIT license
+* Library for BMP180 - thanks to LG, LGPL license
+
+
+----
+
+Contributing to this software is warmly welcomed. You can do this basically by

+[forking](https://help.github.com/articles/fork-a-repo), committing modifications and then [pulling requests](https://help.github.com/articles/using-pull-requests) (follow the links above

+for operating guide). Adding change log and your contact into file header is encouraged.

+Thanks for your contribution.
+
+Seeed Studio is an open hardware facilitation company based in Shenzhen, China. 

+Benefiting from local manufacture power and convenient global logistic system, 

+we integrate resources to serve new era of innovation. Seeed also works with 

+global distributors and partners to push open hardware movement.

+
+
+
+[![Analytics](https://ga-beacon.appspot.com/UA-46589105-3/IMU_10DOF)](https://github.com/igrigorik/ga-beacon)
\ No newline at end of file
diff --git a/Arduino/MPU9250/examples/IMU_10DOF_Test/IMU_10DOF_Test.ino b/Arduino/MPU9250/examples/IMU_10DOF_Test/IMU_10DOF_Test.ino
new file mode 100644
index 00000000..614ee4cf
--- /dev/null
+++ b/Arduino/MPU9250/examples/IMU_10DOF_Test/IMU_10DOF_Test.ino
@@ -0,0 +1,291 @@
+#include "Wire.h"
+
+// I2Cdev and MPU9250 must be installed as libraries, or else the .cpp/.h files
+// for both classes must be in the include path of your project
+#include "I2Cdev.h"
+#include "MPU9250.h"
+#include "BMP180.h"
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for InvenSense evaluation board)
+// AD0 high = 0x69
+MPU9250 accelgyro;
+I2Cdev   I2C_M;
+
+uint8_t buffer_m[6];
+
+
+int16_t ax, ay, az;
+int16_t gx, gy, gz;
+int16_t   mx, my, mz;
+
+
+
+float heading;
+float tiltheading;
+
+float Axyz[3];
+float Gxyz[3];
+float Mxyz[3];
+
+
+#define sample_num_mdate  5000
+
+volatile float mx_sample[3];
+volatile float my_sample[3];
+volatile float mz_sample[3];
+
+static float mx_centre = 0;
+static float my_centre = 0;
+static float mz_centre = 0;
+
+volatile int mx_max = 0;
+volatile int my_max = 0;
+volatile int mz_max = 0;
+
+volatile int mx_min = 0;
+volatile int my_min = 0;
+volatile int mz_min = 0;
+
+float temperature;
+float pressure;
+float atm;
+float altitude;
+BMP180 Barometer;
+
+void setup()
+{
+    // join I2C bus (I2Cdev library doesn't do this automatically)
+    Wire.begin();
+
+    // initialize serial communication
+    // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
+    // it's really up to you depending on your project)
+    Serial.begin(38400);
+
+    // initialize device
+    Serial.println("Initializing I2C devices...");
+    accelgyro.initialize();
+    Barometer.init();
+
+    // verify connection
+    Serial.println("Testing device connections...");
+    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");
+
+    delay(1000);
+    Serial.println("     ");
+
+    //  Mxyz_init_calibrated ();
+
+}
+
+void loop()
+{
+
+    getAccel_Data();
+    getGyro_Data();
+    getCompassDate_calibrated(); // compass data has been calibrated here
+    getHeading();               //before we use this function we should run 'getCompassDate_calibrated()' frist, so that we can get calibrated data ,then we can get correct angle .
+    getTiltHeading();
+
+    Serial.println("calibration parameter: ");
+    Serial.print(mx_centre);
+    Serial.print("         ");
+    Serial.print(my_centre);
+    Serial.print("         ");
+    Serial.println(mz_centre);
+    Serial.println("     ");
+
+
+    Serial.println("Acceleration(g) of X,Y,Z:");
+    Serial.print(Axyz[0]);
+    Serial.print(",");
+    Serial.print(Axyz[1]);
+    Serial.print(",");
+    Serial.println(Axyz[2]);
+    Serial.println("Gyro(degress/s) of X,Y,Z:");
+    Serial.print(Gxyz[0]);
+    Serial.print(",");
+    Serial.print(Gxyz[1]);
+    Serial.print(",");
+    Serial.println(Gxyz[2]);
+    Serial.println("Compass Value of X,Y,Z:");
+    Serial.print(Mxyz[0]);
+    Serial.print(",");
+    Serial.print(Mxyz[1]);
+    Serial.print(",");
+    Serial.println(Mxyz[2]);
+    Serial.println("The clockwise angle between the magnetic north and X-Axis:");
+    Serial.print(heading);
+    Serial.println(" ");
+    Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:");
+    Serial.println(tiltheading);
+    Serial.println("   ");
+
+    temperature = Barometer.bmp180GetTemperature(Barometer.bmp180ReadUT()); //Get the temperature, bmp180ReadUT MUST be called first
+    pressure = Barometer.bmp180GetPressure(Barometer.bmp180ReadUP());//Get the temperature
+    altitude = Barometer.calcAltitude(pressure); //Uncompensated caculation - in Meters
+    atm = pressure / 101325;
+
+    Serial.print("Temperature: ");
+    Serial.print(temperature, 2); //display 2 decimal places
+    Serial.println("deg C");
+
+    Serial.print("Pressure: ");
+    Serial.print(pressure, 0); //whole number only.
+    Serial.println(" Pa");
+
+    Serial.print("Ralated Atmosphere: ");
+    Serial.println(atm, 4); //display 4 decimal places
+
+    Serial.print("Altitude: ");
+    Serial.print(altitude, 2); //display 2 decimal places
+    Serial.println(" m");
+
+    Serial.println();
+    delay(1000);
+
+}
+
+
+void getHeading(void)
+{
+    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
+    if (heading < 0) heading += 360;
+}
+
+void getTiltHeading(void)
+{
+    float pitch = asin(-Axyz[0]);
+    float roll = asin(Axyz[1] / cos(pitch));
+
+    float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
+    float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
+    float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
+    tiltheading = 180 * atan2(yh, xh) / PI;
+    if (yh < 0)    tiltheading += 360;
+}
+
+
+
+void Mxyz_init_calibrated ()
+{
+
+    Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
+    Serial.print("  ");
+    Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
+    Serial.print("  ");
+    Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
+    while (!Serial.find("ready"));
+    Serial.println("  ");
+    Serial.println("ready");
+    Serial.println("Sample starting......");
+    Serial.println("waiting ......");
+
+    get_calibration_Data ();
+
+    Serial.println("     ");
+    Serial.println("compass calibration parameter ");
+    Serial.print(mx_centre);
+    Serial.print("     ");
+    Serial.print(my_centre);
+    Serial.print("     ");
+    Serial.println(mz_centre);
+    Serial.println("    ");
+}
+
+
+void get_calibration_Data ()
+{
+    for (int i = 0; i < sample_num_mdate; i++)
+    {
+        get_one_sample_date_mxyz();
+        /*
+        Serial.print(mx_sample[2]);
+        Serial.print(" ");
+        Serial.print(my_sample[2]);                            //you can see the sample data here .
+        Serial.print(" ");
+        Serial.println(mz_sample[2]);
+        */
+
+
+
+        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
+        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
+        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];
+
+        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
+        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
+        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
+
+    }
+
+    mx_max = mx_sample[1];
+    my_max = my_sample[1];
+    mz_max = mz_sample[1];
+
+    mx_min = mx_sample[0];
+    my_min = my_sample[0];
+    mz_min = mz_sample[0];
+
+
+
+    mx_centre = (mx_max + mx_min) / 2;
+    my_centre = (my_max + my_min) / 2;
+    mz_centre = (mz_max + mz_min) / 2;
+
+}
+
+
+
+
+
+
+void get_one_sample_date_mxyz()
+{
+    getCompass_Data();
+    mx_sample[2] = Mxyz[0];
+    my_sample[2] = Mxyz[1];
+    mz_sample[2] = Mxyz[2];
+}
+
+
+void getAccel_Data(void)
+{
+    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+    Axyz[0] = (double) ax / 16384;
+    Axyz[1] = (double) ay / 16384;
+    Axyz[2] = (double) az / 16384;
+}
+
+void getGyro_Data(void)
+{
+    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
+    Gxyz[0] = (double) gx * 250 / 32768;
+    Gxyz[1] = (double) gy * 250 / 32768;
+    Gxyz[2] = (double) gz * 250 / 32768;
+}
+
+void getCompass_Data(void)
+{
+    I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
+    delay(10);
+    I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);
+
+    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
+    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
+    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;
+
+    Mxyz[0] = (double) mx * 1200 / 4096;
+    Mxyz[1] = (double) my * 1200 / 4096;
+    Mxyz[2] = (double) mz * 1200 / 4096;
+}
+
+void getCompassDate_calibrated ()
+{
+    getCompass_Data();
+    Mxyz[0] = Mxyz[0] - mx_centre;
+    Mxyz[1] = Mxyz[1] - my_centre;
+    Mxyz[2] = Mxyz[2] - mz_centre;
+}
diff --git a/Arduino/MS5803/MS5803.cpp b/Arduino/MS5803/MS5803.cpp
new file mode 100644
index 00000000..0f9d68cd
--- /dev/null
+++ b/Arduino/MS5803/MS5803.cpp
@@ -0,0 +1,346 @@
+// I2Cdev library collection - MS5803 I2C device class
+// Based on Measurement Specialties MS5803 document, 3/25/2013 (DA5803-01BA_010)
+// 3/29/2016 by Ryan Neve 
+// 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) 2014 Ryan Neve
+
+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 "MS5803_I2C.h"
+
+const static uint8_t INIT_TRIES = 3;
+const static uint16_t PRESS_ATM_MBAR_DEFAULT = 1015;
+
+	
+const char* CALIBRATION_CONSTANTS[] = {
+	"_c1_SENSt1",
+	"_c2_OFFt1",
+	"_c3_TCS",
+	"_c4_TCO",
+	"_c5_Tref",
+	"_c6_TEMPSENS"
+};
+
+/** Default constructor, uses default I2C address.
+* @see MPU6050_DEFAULT_ADDRESS
+*/
+MS5803::MS5803() { MS5803(MS5803_DEFAULT_ADDRESS);}
+
+/** Specific address constructor.
+* @param address I2C address
+* @see MS5803_DEFAULT_ADDRESS
+* @see MS5803_ADDRESS_AD0_LOW
+* @see MS5803_ADDRESS_AD0_HIGH
+*/
+MS5803::MS5803(uint8_t address) {
+	setAddress(address);
+	_initialized = false;
+	_c1_SENSt1		= 0;
+	_c2_OFFt1		= 0;
+	_c3_TCS			= 0;
+	_c4_TCO			= 0;
+	_c5_Tref		= 0;
+	_c6_TEMPSENS	= 0;
+	_press_atm_mBar = (float)PRESS_ATM_MBAR_DEFAULT/1000.0; //default, can be changed with setAtmospheric() 
+}
+// Because sometimes you want to set the address later.
+void MS5803::setAddress(uint8_t address) {
+	_dev_address = address;
+}
+
+/** Power on and prepare for general usage.
+* This will reset the device to make sure that the calibration PROM gets loaded into
+* the internal register. It will then read the calibration constants from the PROM
+*/
+bool MS5803::initialize(uint8_t model) {
+	_initialized = false;
+	// Make sure model is valid.
+	switch (model) {
+		case ( 1): _model = BA01; break;
+		case ( 2): _model = BA02; break;
+		case ( 5): _model = BA05; break;
+		case (14): _model = BA14; break;
+		case (30): _model = BA30; break;
+		default:
+			Serial.print(F("MS5803 Model entered (")); Serial.print(model); Serial.println(F(") is not valid/supported."));
+			_model = INVALID; 
+			return 0;
+	}
+	if (_debug) Serial.println(reset());
+	uint8_t tries = 0;
+	do {
+		_getCalConstants(); // Seems to partially fail the first try sometimes.
+		if (_c1_SENSt1 && _c2_OFFt1 && _c3_TCS && _c4_TCO && _c5_Tref && _c6_TEMPSENS ) _initialized = true; // They must all be non-0
+		tries++;
+	} while (!_initialized && ( tries < INIT_TRIES) );
+	if (_debug) {
+		for ( uint8_t i = 1 ; i <= 6; i++){
+			Serial.print(i);
+			Serial.print(": ");
+			Serial.print(CALIBRATION_CONSTANTS[i-1]);
+			Serial.print(" = ");
+			Serial.println(_getCalConstant(i));
+		}
+	}
+	return _initialized;
+}
+
+// See if we can communicate
+bool MS5803::testConnection(){
+	uint8_t reg_address = CMD_ADC_CONV + TEMPERATURE + ADC_256;
+	return I2Cdev::writeBytes(_dev_address,reg_address,0,_buffer);
+}
+
+uint16_t MS5803::reset(){
+	// Not sure how to send no buffer to an address.
+	return I2Cdev::writeBytes(_dev_address, MS5803_RESET,0,_buffer);
+}
+
+void MS5803::_getCalConstants(){
+	/* Query and parse calibration constants */
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C1,2,_buffer);
+	_c1_SENSt1 =   (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C2,2,_buffer);
+	_c2_OFFt1 =    (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C3,2,_buffer);
+	_c3_TCS =      (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C4,2,_buffer);
+	_c4_TCO =      (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C5,2,_buffer);
+	_c5_Tref =     (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+	I2Cdev::readBytes(_dev_address,MS5803_PROM_C6,2,_buffer);
+	_c6_TEMPSENS = (((uint16_t)_buffer[0] << 8) + _buffer[1]);
+}
+
+int32_t MS5803::_getCalConstant(uint8_t constant_no){
+	switch ( constant_no ) {
+		case 1: return _c1_SENSt1;
+		case 2: return _c2_OFFt1;
+		case 3: return _c3_TCS;
+		case 4: return _c4_TCO;
+		case 5: return _c5_Tref;
+		case 6: return _c6_TEMPSENS;
+		default: return 0;
+	}
+}
+
+/*	This function communicates with the sensor and does all the math to convert 
+	raw values to good data. Data can be accessed with various getters.
+*/
+void MS5803::calcMeasurements(precision _precision){
+	// Get raw temperature and pressure values
+	_d2_temperature = _getADCconversion(TEMPERATURE, _precision);
+	_d1_pressure = _getADCconversion(PRESSURE, _precision);
+	//Now that we have a raw temperature, let's compute our actual.
+	_dT = _d2_temperature - ((int32_t)_c5_Tref << 8);
+	double temp_dT = _dT / (double)pow(2,23);
+	_TEMP = 2000 +  (int32_t)(temp_dT * _c6_TEMPSENS);
+	if ( _debug ) {
+		Serial.println("Raw values:");
+		Serial.print("    _d2_temperature = "); Serial.println(_d2_temperature);
+		Serial.print("    _d1_pressure = "); Serial.println(_d1_pressure);
+		Serial.println("First order values:");
+		Serial.print("    _dT = "); Serial.println(_dT);
+		Serial.print("    _TEMP = "); Serial.println(_TEMP);
+	}
+	// Second order variables
+	int64_t T2 = 0;
+	int64_t off2 = 0;
+	int64_t sens2 = 0;
+	// Every variant does the calculations differently, so...
+	switch (_model) {
+		case (BA01):  //MS5803-01-----------------------------------------------------------
+			_OFF  = ((int64_t)_c2_OFFt1  << 16 ) + ((((int64_t)_c4_TCO * (int64_t)_dT)) >> 7 );
+			_SENS = ((int64_t)_c1_SENSt1 << 15 ) + ((((int64_t)_c3_TCS * (int64_t)_dT)) >> 8 );
+			// 2nd Order calculations
+			if ( _TEMP < 2000.0) {  // Is temperature below or above 20.00 deg C ?
+				T2 = 3 * pow(_dT,2)/(int64_t)pow(2,31);
+				off2  = 3 * pow((_TEMP - 2000.0),2);
+				sens2 = 7 * pow((_TEMP - 2000.0),2) / 8;
+				if ( _TEMP < 1500.0 ) sens2 += 2 * pow((_TEMP + 1500.0),2);// below 15C
+			}
+			else {
+				T2 = 0;
+				off2 = 0;
+				sens2 = 0;
+				if ( _TEMP >= 4500.0 ) {
+					sens2 -= ((int64_t)pow((_TEMP - 4500.0),2) >> 3);
+				}
+			}
+			break;
+		case (BA02):  //MS5803-02-----------------------------------------------------------
+			_OFF  = ((int64_t)_c2_OFFt1  << 17 ) + ((((int64_t)_c4_TCO * (int64_t)_dT)) >> 6 );
+			_SENS = ((int64_t)_c1_SENSt1 << 16 ) + ((((int64_t)_c3_TCS * (int64_t)_dT)) >> 7 );
+			// 2nd Order calculations
+			if ( _TEMP < 2000.0) {  // Is temperature below or above 20.00 deg C ?
+				T2 = 3 * pow(_dT,2)/(int64_t)pow(2,31);
+				off2 = 61 * pow((_TEMP - 2000.0),2) / 16;
+				sens2 = 2 * pow(((int64_t)_TEMP - 2000.0),2);
+				if ( _TEMP < 1500.0 ) { // below 15C
+					off2  += 20 * pow((_TEMP + 1500.0),2);
+					sens2 += 12 * pow((_TEMP + 1500.0),2);
+				}
+			}
+			else {
+				T2 = 0;
+				off2 = 0;
+				sens2 = 0;
+			}
+			break;
+		case (BA05):  //MS5803-05-----------------------------------------------------------
+			_OFF  = ((int64_t)_c2_OFFt1  << 18 ) + ((((int64_t)_c4_TCO * (int64_t)_dT)) >> 5 );
+			_SENS = ((int64_t)_c1_SENSt1 << 17 ) + ((((int64_t)_c3_TCS * (int64_t)_dT)) >> 7 );
+			// 2nd Order calculations
+			if ( _TEMP < 2000.0) {  // Is temperature below or above 20.00 deg C ?
+				T2 = 3 * pow(_dT,2)/(int64_t)pow(2,33);
+				off2 =  3 * pow((_TEMP - 2000.0),2) / 8;
+				sens2 = 7 * pow((_TEMP - 2000.0),2) / 8;
+				if ( _TEMP < -1500.0 ) { // below -15C
+					sens2 += 3 * pow((_TEMP + 1500.0),2);
+				}
+			}
+			else {
+				T2 = 0;
+				off2 = 0;
+				sens2 = 0;
+			}
+			break;
+		case (BA14):  //MS5803-14-----------------------------------------------------------
+			// 14 and 30 are the same calculations...
+		case (BA30):  //MS5803-30-----------------------------------------------------------
+			_OFF  = ((int64_t)_c2_OFFt1  << 16 ) + ((((int64_t)_c4_TCO * (int64_t)_dT)) >> 7 );
+			_SENS = ((int64_t)_c1_SENSt1 << 15 ) + ((((int64_t)_c3_TCS * (int64_t)_dT)) >> 8 );
+			// 2nd Order calculations
+			if ( _TEMP < 2000.0) {  // Is temperature below or above 20.00 deg C ?
+				T2 = 3 * pow(_dT,2)/(int64_t)pow(2,33);
+				off2 =  3 * pow((_TEMP - 2000.0),2) / 2;
+				sens2 = 5 * pow((_TEMP - 2000.0),2) / 8;
+				if ( _TEMP < 1500.0 ) { // below 15C
+					off2  += 7 * pow((_TEMP + 1500.0),2);
+					sens2 += 4 * pow((_TEMP + 1500.0),2);
+				}
+			}
+			else {
+				T2 =    7 * ((int64_t)_dT * _dT) >> 37;
+				off2 =  1 * (pow((_TEMP - 2000.0),2)) / 16;
+				sens2 = 0;
+			}
+			break;
+		default:
+			_OFF = 0;
+			_SENS = 0;
+			T2 = 0;
+			sens2 = 0;
+			off2 = 0;
+	}
+	if ( _debug ) {
+		Serial.print("    _OFF = "); serialPrintln64(_OFF);
+		Serial.print("    _SENS = "); serialPrintln64(_SENS);
+	}
+	 // Second Order
+	_TEMP  -= T2;
+	_SENS  -= sens2;
+	_OFF   -= off2;
+	// Now pressure
+	switch (_model) {
+		case (BA05):  //MS5803-05-----------------------------------------------------------
+			_P = ((((int32_t)_d1_pressure * _SENS) >> 21 ) - _OFF) >> 15;
+			_P /= 10; // NO IDEA WHY THIS NEEDS TO BE DONE. PERHAPS AN ERROR IN THE DATASHEET?
+			break;
+		case (BA01):  //MS5803-01--passthrough----------------------------------------------
+		case (BA02):  //MS5803-02--passthrough----------------------------------------------
+		case (BA14):  //MS5803-14--passthrough----------------------------------------------
+			_P = ((((int32_t)_d1_pressure * _SENS) >> 21 ) - _OFF) >> 15;
+			break;
+		case (BA30):  //MS5803-30-----------------------------------------------------------
+			_P = ((((int32_t)_d1_pressure * _SENS) >> 21 ) - _OFF) >> 13;
+			break;
+		default:
+			_P = 0;
+	}
+	if ( _debug ) {
+		Serial.println("Second order values:");
+		Serial.print("    T2 = "); serialPrintln64(T2);
+		Serial.print("    sens2 = "); serialPrintln64(sens2);
+		Serial.print("    off2 = "); serialPrintln64(off2);
+		Serial.print("    _TEMP = "); Serial.println(_TEMP);
+		Serial.print("    _SENS = "); serialPrintln64(_SENS);
+		Serial.print("    _OFF = "); serialPrintln64(_OFF);
+		Serial.println("Pressure:");
+		Serial.print("    _P = "); Serial.println(_P);
+	}
+}
+
+int32_t MS5803::_getADCconversion(measurement _measurement, precision _precision){
+	// Retrieve ADC measurement from the device.
+	// Select measurement type and precision
+	uint32_t result;
+	uint8_t reg_address = CMD_ADC_CONV + _measurement + _precision;
+	uint8_t write_length = 0;
+	uint8_t read_length = 3;
+	uint16_t read_timeout = 2000;
+	//sendCommand(CMD_ADC_CONV + _measurement + _precision);
+	I2Cdev::writeBytes(_dev_address,reg_address,write_length,_buffer); // buffer is ignored when write_length is 0
+		
+	// Wait for conversion to complete
+	delay(1); //general delay
+	switch( _precision )
+	{
+		case ADC_256 : delay(1); break;
+		case ADC_512 : delay(3 >> CLKPR); break;
+		case ADC_1024: delay(4 >> CLKPR); break;
+		case ADC_2048: delay(6 >> CLKPR); break;
+		case ADC_4096: delay(10 >> CLKPR); break;
+	}
+	I2Cdev::readBytes(_dev_address,MS5803_ADC_READ,read_length,_buffer,read_timeout);
+	result = ((uint32_t)_buffer[0] << 16) + ((uint32_t)_buffer[1] << 8) + _buffer[2];
+	if (_debug) {
+		Serial.print("Reading MS5803 ADC");
+		switch (_measurement) {
+			case PRESSURE:    Serial.println(" Pressure: "   ); break;
+			case TEMPERATURE: Serial.println(" Temperature: "); break;
+		}
+		Serial.print("    buffer[0] = "); Serial.println(_buffer[0]);
+		Serial.print("    buffer[1] = "); Serial.println(_buffer[1]);
+		Serial.print("    buffer[2] = "); Serial.println(_buffer[2]);
+	}
+	return result;
+}
+
+void serialPrintln64(int64_t val64){
+	serialPrint64(val64);
+	Serial.println();
+}
+void serialPrint64(int64_t val64){
+	int32_t result_high = val64 / 10000000L;
+	int32_t result_low = val64 - (10000000LL * result_high);
+	Serial.print(result_high);
+	Serial.print(result_low);
+}
\ No newline at end of file
diff --git a/Arduino/MS5803/MS5803.h b/Arduino/MS5803/MS5803.h
new file mode 100644
index 00000000..c6b6de21
--- /dev/null
+++ b/Arduino/MS5803/MS5803.h
@@ -0,0 +1,159 @@
+// I2Cdev library collection - MS5803 I2C device class
+// Based on Measurement Specialties MS5803 document, 3/25/2013 (DA5803-01BA_010)
+// 4/12/2014 by Ryan Neve 
+// 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) 2014 Ryan Neve
+
+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 _MS5803_H_
+#define _MS5803_H_
+
+#define I2CDEV_SERIAL_DEBUG
+
+#include "I2Cdev.h"
+#include 
+
+#define MS5803_ADDRESS
+// Should convert these to enum
+#define MS5803_ADDRESS_AD0_LOW     0x77 // address pin low (GND), default for InvenSense evaluation board
+#define MS5803_ADDRESS_AD0_HIGH    0x76 // address pin high (VCC)
+
+#define MS5803_DEFAULT_ADDRESS     MS5803_ADDRESS_AD0_LOW
+
+#define MS5803_RESET     0x1E
+
+#define MS5803_PROM_BASE 0xA0
+#define MS5803_PROM_C1   0xA2
+#define MS5803_PROM_C2   0xA4
+#define MS5803_PROM_C3   0xA6
+#define MS5803_PROM_C4   0xA8
+#define MS5803_PROM_C5   0xAA
+#define MS5803_PROM_C6   0xAC
+#define MS5803_PROM_CRC  0xAE
+
+
+
+#define MS5803_ADC_READ  0x00
+#define CMD_ADC_CONV 0x40 // ADC conversion command
+
+// Define measurement type.
+enum measurement {
+	PRESSURE    = 0x00,
+	TEMPERATURE = 0x10
+};
+
+// Define constants for Conversion precision
+enum precision {
+	ADC_256  = 0x00,
+	ADC_512  = 0x02,
+	ADC_1024 = 0x04,
+	ADC_2048 = 0x06,
+	ADC_4096 = 0x08
+};
+
+enum ms5803_model {
+	INVALID =  0,
+	BA01    =  1,
+	BA02    =  2,
+	BA05    =  5,
+	BA14    = 14,
+	BA30    = 30
+};
+
+
+const static float FRESH_WATER_CONSTANT = 1.019716; // kg/m^3
+const static float BAR_IN_PSI = 14.50377;
+
+class MS5803 {
+	public:
+		MS5803();
+		MS5803(uint8_t address);
+	
+		void		setAddress(uint8_t address);
+		uint8_t		getAddress() {return _dev_address;}
+		bool		initialize(uint8_t model);
+		bool		initialized() {return _initialized;}
+		bool		testConnection();
+		void		calcMeasurements(precision _precision);	// Here's where the heavy lifting occurs.
+		uint16_t	reset();
+
+		// Setters
+		void		setAtmospheric(float pressure) {_press_atm_mBar = pressure;}
+		void		setDebug(bool debug) { _debug = debug; }
+
+		// Getters
+		bool		getDebug() { return _debug; }
+		float		getTemp_C() {return (float)_TEMP / 100.0;}
+		float		getPress_mBar() {return (float)_P / 10.0;}
+		float		getPress_kPa() {return (float)_P / 100.0;}
+		float		getPress_gauge() {return ((float)_P / 10.0) - _press_atm_mBar;}
+		float		getPress_psi() {return (((float)_P / 10.0) - _press_atm_mBar) * BAR_IN_PSI;}
+		float		getDepthFresh_m() {return ((float)_P/1000.0) * FRESH_WATER_CONSTANT;}
+
+
+	protected:
+	private:
+		void		_getCalConstants();
+		int32_t		_getCalConstant(uint8_t constant_no);
+		int32_t		_getADCconversion(measurement _measurement, precision _precision);
+		uint8_t		_buffer[14];
+		uint8_t		_dev_address;
+		ms5803_model	_model;	// the suffix after ms5803. E.g 2 for MS5803-02 indicates range.
+		bool		_initialized;
+		bool		_debug;
+		// Calibration Constants
+		int32_t		_c1_SENSt1;		// Pressure Sensitivity
+		int32_t		_c2_OFFt1;		// Pressure Offset
+		int32_t		_c3_TCS;		// Temperature coefficient of pressure sensitivity
+		int32_t		_c4_TCO;		// Temperature coefficient of pressure offset
+		int32_t		_c5_Tref;		// Reference Temperature
+		int32_t		_c6_TEMPSENS;	// Temperature coefficient of the temperature
+		// intermediate pressure and temperature data
+		int32_t		_d1_pressure;	// AdcPressure
+		int32_t		_d2_temperature;// AdcTemperature
+		// Calculated values
+		int32_t		_dT;		//TempDifference
+		int32_t		_TEMP;		// Actual temperature -40 to 85C with .01 resolution (divide by 100) - Temperature float
+		// Temperature compensated pressure
+		int64_t		_OFF;		// First Order Offset at actual temperature // Offset - float
+		int64_t		_SENS;		// Sensitivity at actual temperature // Sensitivity - float
+		int32_t		_P;			// Temperature compensated pressure 10...1300 mbar (divide by 100 to get mBar)
+		float		_press_atm_mBar;	// Atmospheric pressure
+
+};
+
+// Function prototype:
+void serialPrintln64(int64_t val64);
+void serialPrint64(int64_t val64);
+
+
+#endif
\ No newline at end of file
diff --git a/Arduino/MS5803/examples/test_MS5803/test_MS5803.ino b/Arduino/MS5803/examples/test_MS5803/test_MS5803.ino
new file mode 100644
index 00000000..8f370e59
--- /dev/null
+++ b/Arduino/MS5803/examples/test_MS5803/test_MS5803.ino
@@ -0,0 +1,32 @@
+
+
+#include 
+#include 
+#include 
+
+//const uint8_t MS_MODEL = 1; // MS5803-01BA
+//const uint8_t MS_MODEL = 2; // MS5803-02BA
+const uint8_t MS_MODEL = 5; // MS5803-05BA
+//const uint8_t MS_MODEL = 14; // MS5803-14BA
+//const uint8_t MS_MODEL = 30; // MS5803-30BA
+
+MS5803 presstemp(0x76);
+const uint8_t loop_delay = 10; // Seconds between readings
+uint32_t wake_time = millis();
+
+
+void setup() {
+  Serial.begin(57600);
+  Wire.begin();
+  // Start up and get Calibration constants.
+  presstemp.initialize(MS_MODEL);
+  if ( presstemp.testConnection() ) Serial.println("We are communicating with MS5803 via I2C.");
+  else Serial.println("I2C Communications with MS5803 failed.");
+}
+void loop(){
+    Serial.print("Getting temperature");
+    presstemp.calcMeasurements(ADC_4096);
+    Serial.print("The temperature is "); Serial.print(presstemp.getTemp_C()); Serial.println(" C");
+    Serial.print("The pressure is "); Serial.print(presstemp.getPress_mBar()); Serial.println(" mBar");
+  delay(2000);
+}
\ No newline at end of file
diff --git a/Arduino/MS5803/keywords.txt b/Arduino/MS5803/keywords.txt
new file mode 100644
index 00000000..c0b9b189
--- /dev/null
+++ b/Arduino/MS5803/keywords.txt
@@ -0,0 +1,42 @@
+###########################################
+# Datatypes (KEYWORD1)
+###########################################
+
+MS5803	KEYWORD1
+
+###########################################
+# Methods and Functions (KEYWORD2)
+###########################################
+setAddress	KEYWORD2
+getAddress	KEYWORD2
+initialize	KEYWORD2
+testConnection	KEYWORD2
+setAtmospheric	KEYWORD2
+calcMeasurements	KEYWORD2
+getD1Pressure	KEYWORD2
+getD2Temperature	KEYWORD2
+getTemp_C	KEYWORD2
+getPress_mBar	KEYWORD2
+getPress_kPa	KEYWORD2
+initialized	KEYWORD2
+getDebug	KEYWORD2
+setDebug	KEYWORD2
+
+###########################################
+# Constants (LITERAL1)
+###########################################
+MS5803_ADDRESS	LITERAL1
+MS5803_ADDRESS_AD0_LOW	LITERAL1
+MS5803_ADDRESS_AD0_HIGH	LITERAL1
+MS5803_DEFAULT_ADDRESS	LITERAL1
+MS5803_RESET	LITERAL1
+MS5803_PROM_BASE	LITERAL1
+MS5803_PROM_C1	LITERAL1
+MS5803_PROM_C2	LITERAL1
+MS5803_PROM_C3	LITERAL1
+MS5803_PROM_C4	LITERAL1
+MS5803_PROM_C5	LITERAL1
+MS5803_PROM_C6	LITERAL1
+MS5803_PROM_CRC	LITERAL1
+MS5803_ADC_READ	LITERAL1
+CMD_ADC_CONV	LITERAL1
\ No newline at end of file
diff --git a/Arduino/SSD1308/library.json b/Arduino/SSD1308/library.json
new file mode 100644
index 00000000..8fb684fe
--- /dev/null
+++ b/Arduino/SSD1308/library.json
@@ -0,0 +1,18 @@
+{
+  "name": "I2Cdevlib-SSD1308",
+  "version": "1.0.0",
+  "keywords": "oled, pled, display, driver, i2cdevlib, i2c",
+  "description": "SSD1308 is a single-chip CMOS OLED/PLED driver with controller for organic / polymer light emitting diode dot-matrix graphic display system",
+  "include": "Arduino/SSD1308",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "dependencies":
+  {
+    "jrowberg/I2Cdevlib-Core": "*"
+  },
+  "frameworks": "arduino",
+  "platforms": "*"
+}
diff --git a/Arduino/TCA6424A/TCA6424A.cpp b/Arduino/TCA6424A/TCA6424A.cpp
index 851ee6b5..3edc84a3 100644
--- a/Arduino/TCA6424A/TCA6424A.cpp
+++ b/Arduino/TCA6424A/TCA6424A.cpp
@@ -87,7 +87,7 @@ uint8_t TCA6424A::readBank(uint8_t bank) {
  * @param banks Container for all bank's pin values (P00-P27)
  */
 void TCA6424A::readAll(uint8_t *banks) {
-    I2Cdev::readBytes(devAddr, TCA6424A_RA_INPUT0, 3, banks);
+    I2Cdev::readBytes(devAddr, TCA6424A_RA_INPUT0 | TCA6424A_AUTO_INCREMENT, 3, banks);
 }
 /** Get all pin logic levels from all banks.
  * Reads into individual 1-byte containers.
@@ -96,7 +96,7 @@ void TCA6424A::readAll(uint8_t *banks) {
  * @param bank2 Container for Bank 2's pin values (P20-P27)
  */
 void TCA6424A::readAll(uint8_t *bank0, uint8_t *bank1, uint8_t *bank2) {
-    I2Cdev::readBytes(devAddr, TCA6424A_RA_INPUT0, 3, buffer);
+    I2Cdev::readBytes(devAddr, TCA6424A_RA_INPUT0 | TCA6424A_AUTO_INCREMENT, 3, buffer);
     *bank0 = buffer[0];
     *bank1 = buffer[1];
     *bank2 = buffer[2];
diff --git a/Arduino/TCA6424A/TCA6424A.h b/Arduino/TCA6424A/TCA6424A.h
index b255f04c..afcacc48 100644
--- a/Arduino/TCA6424A/TCA6424A.h
+++ b/Arduino/TCA6424A/TCA6424A.h
@@ -35,8 +35,8 @@ THE SOFTWARE.
 
 #include "I2Cdev.h"
 
-#define TCA6424A_ADDRESS_ADDR_LOW   0x34 // address pin low (GND)
-#define TCA6424A_ADDRESS_ADDR_HIGH  0x35 // address pin high (VCC)
+#define TCA6424A_ADDRESS_ADDR_LOW   0x22 // address pin low (GND)
+#define TCA6424A_ADDRESS_ADDR_HIGH  0x23 // address pin high (VCC)
 #define TCA6424A_DEFAULT_ADDRESS    TCA6424A_ADDRESS_ADDR_LOW
 
 #define TCA6424A_RA_INPUT0          0x00
diff --git a/Arduino/TCA6424A/library.json b/Arduino/TCA6424A/library.json
new file mode 100644
index 00000000..53018378
--- /dev/null
+++ b/Arduino/TCA6424A/library.json
@@ -0,0 +1,18 @@
+{
+  "name": "I2Cdevlib-TCA6424A",
+  "version": "1.0.0",
+  "keywords": "io, expander, i2cdevlib, i2c",
+  "description": "The TCA6424A is a low-voltage 24-Bit I2C and SMBus I/O expander with interrupt output, reset and configuration registers",
+  "include": "Arduino/TCA6424A",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "dependencies":
+  {
+    "jrowberg/I2Cdevlib-Core": "*"
+  },
+  "frameworks": "arduino",
+  "platforms": "*"
+}
diff --git a/BeagleBoneBlack/I2Cdev/I2Cdev.cpp b/BeagleBoneBlack/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..bd62ebc9
--- /dev/null
+++ b/BeagleBoneBlack/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,411 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+// BeagleBone Black (potencially other linux boards) port by Mateus Amarante 
+//
+// Changelog:
+//      2018-03-02 - Initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+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 "I2Cdev.h"
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+
+#define OPEN_ERROR_MSG  "Failed to open "
+#define WRITE_ERROR_MSG "Failed to write into "
+#define READ_ERROR_MSG  "Failed to read from "
+
+I2Cdev::I2Cdev() : I2Cdev(DEFAULT_BBB_I2C_BUS) {}
+
+I2Cdev::I2Cdev(uint8_t busAddr)
+{
+    sprintf(path_, "/dev/i2c-%hhu", busAddr);
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data)
+{
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data)
+{
+    uint16_t b;
+    uint8_t count = readWord(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data)
+{
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b)) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data)
+{
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked#include 
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = readWord(devAddr, regAddr, &w)) != 0)
+    {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data)
+{
+    return readBytes(devAddr, regAddr, 1, data);
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data)
+{
+    return readWords(devAddr, regAddr, 1, data);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
+{
+    int fd;
+
+    if ((fd = open(path_, O_RDWR)) < 0)
+    {
+        char error_msg[sizeof(OPEN_ERROR_MSG) + sizeof(path_)] = OPEN_ERROR_MSG;
+
+        perror(strcat(error_msg, path_));
+        return -1;
+    }
+
+    if (ioctl(fd, I2C_SLAVE, devAddr) < 0)
+    {
+        fprintf(stderr, "Failed to access slave at %u address. %s\n", regAddr, strerror(errno));
+        return -1;
+    }
+
+    if (write(fd, ®Addr, 1) != 1)
+    {
+        char error_msg[sizeof(WRITE_ERROR_MSG) + sizeof(path_)] = WRITE_ERROR_MSG;
+
+        perror(strcat(error_msg, path_));
+    }
+
+    if (read(fd, data, length) != length)
+    {
+        char error_msg[sizeof(READ_ERROR_MSG) + sizeof(path_)] = READ_ERROR_MSG;
+
+        perror(strcat(error_msg, path_));
+        return -1;
+    }
+
+    close(fd);
+
+    return length;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
+{
+    uint8_t buff[length * 2];
+
+    if (readBytes(devAddr, regAddr, length * 2, buff) > 0)
+    {
+        for (int i = 0; i < length; i++)
+        {
+            data[i] = (buff[i * 2] << 8) | buff[i * 2 + 1];
+        }
+        return length;
+    }
+
+    return -1;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
+{
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data)
+{
+    uint16_t w;
+    readWord(devAddr, regAddr, &w);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
+{
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask;                     // zero all non-important bits in data
+        b &= ~(mask);                     // zero all important bits in existing byte
+        b |= data;                        // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data)
+{
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (readWord(devAddr, regAddr, &w) != 0)
+    {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask;                     // zero all non-important bits in data
+        w &= ~(mask);                     // zero all important bits in existing word
+        w |= data;                        // combine data with existing word
+        return writeWord(devAddr, regAddr, w);
+    }
+    else
+    {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
+{
+    return writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data)
+{
+    return writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data)
+{
+    int fd;
+
+    if ((fd = open(path_, O_RDWR)) < 0)
+    {
+        char error_msg[sizeof(OPEN_ERROR_MSG) + sizeof(path_)] = OPEN_ERROR_MSG;
+
+        perror(strcat(error_msg, path_));
+        return false;
+    }
+
+    if (ioctl(fd, I2C_SLAVE, devAddr) < 0)
+    {
+        fprintf(stderr, "Failed to access slave at %u address. %s\n", regAddr, strerror(errno));
+        return false;
+    }
+
+    uint16_t buff_length = length + 1;
+    uint8_t buff[buff_length];
+    buff[0] = regAddr;
+
+    memcpy(&buff[1], data, length);
+
+    if (write(fd, buff, buff_length) != buff_length)
+    {
+        char error_msg[sizeof(WRITE_ERROR_MSG) + sizeof(path_)] = WRITE_ERROR_MSG;
+
+        perror(strcat(error_msg, path_));
+        return false;
+    }
+
+    close(fd);
+
+    return true;
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data)
+{
+    uint8_t buff[length * 2];
+
+    for (int i = 0; i < length; i++)
+    {
+        buff[2 * i] = (uint8_t)(data[i] >> 8);     //MSByte
+        buff[1 + 2 * i] = (uint8_t)(data[i] >> 0); //LSByte
+    }
+
+    return writeBytes(devAddr, regAddr, length * 2, buff);
+}
diff --git a/BeagleBoneBlack/I2Cdev/I2Cdev.h b/BeagleBoneBlack/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..245302b5
--- /dev/null
+++ b/BeagleBoneBlack/I2Cdev/I2Cdev.h
@@ -0,0 +1,64 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+// BeagleBone Black (potencially other linux boards) port by Mateus Amarante 
+//
+// Changelog:
+//      2018-03-02 - Initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+
+#define DEFAULT_BBB_I2C_BUS 2
+
+class I2Cdev
+{
+public:
+  I2Cdev();
+  I2Cdev(uint8_t busAddr);
+
+  static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+  static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+  static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+  static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+  static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+  static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+  static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+  static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+  static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+  static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+  static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+  static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+  static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+  static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+  static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+  static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+private:
+  char path_[13]; // up to "/dev/i2c-255"
+};
+
+#endif /* _I2CDEV_H_ */
diff --git a/EFM32/I2Cdev/I2Cdev.cpp b/EFM32/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..a70796e6
--- /dev/null
+++ b/EFM32/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,241 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// EFM32 stub port by Nicolas Baldeck 
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+// Changelog:
+//      2015-01-02 - Initial release
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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 "I2Cdev.h"
+
+/** Default constructor.
+ */
+I2Cdev::I2Cdev() {
+}
+
+/** Initialize I2C0
+ */
+void I2Cdev::initialize() {
+	CMU_ClockEnable(cmuClock_I2C0, true);
+
+	GPIO_PinModeSet(I2C_SDA_PORT, I2C_SDA_PIN, I2C_SDA_MODE, I2C_SDA_DOUT);
+	GPIO_PinModeSet(I2C_SCL_PORT, I2C_SCL_PIN, I2C_SCL_MODE, I2C_SCL_DOUT);
+
+	I2C0->ROUTE |= I2C_ROUTE_SDAPEN | I2C_ROUTE_SCLPEN;
+
+	const I2C_Init_TypeDef init = I2C_INIT_DEFAULT;
+	I2C_Init(I2C0, &init);
+
+	I2C_Enable(I2C0, true);
+}
+
+/** Enable or disable I2C
+ * @param isEnabled true = enable, false = disable
+ */
+void I2Cdev::enable(bool isEnabled) {
+  
+  I2C_Enable(I2C0, isEnabled);
+  CMU_ClockEnable(cmuClock_I2C0, isEnabled);
+  
+}
+
+I2C_TransferReturn_TypeDef I2Cdev::transfer(I2C_TransferSeq_TypeDef *seq, uint16_t timeout) {
+
+  I2C_TransferReturn_TypeDef ret;
+  /* Do a polled transfer */
+  ret = I2C_TransferInit(I2C0, seq);
+
+  while (ret == i2cTransferInProgress && timeout--) {
+    ret = I2C_Transfer(I2C0);
+  }
+
+  return(ret);
+  
+}
+
+/** Default timeout value for read operations.
+ */
+uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
+
+  I2C_TransferSeq_TypeDef seq;
+  
+  uint8_t regid[1];
+ 
+  seq.addr = devAddr << 1;
+  seq.flags = I2C_FLAG_WRITE_READ;
+  
+  /* Select register to be read */
+  regid[0] = regAddr;
+  seq.buf[0].data = regid;
+  seq.buf[0].len = 1;
+ 
+  /* 1 bytes reg */
+  seq.buf[1].data = data;
+  seq.buf[1].len = length;
+  
+  if (transfer(&seq, timeout) == i2cTransferDone) {
+	  return seq.buf[1].len;
+  } else {
+	  return false;
+  }
+  
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+
+  I2C_TransferSeq_TypeDef seq;
+
+  uint8_t writeData[3];
+
+  seq.addr = devAddr << 1;
+  seq.flags = I2C_FLAG_WRITE;
+
+  /* Select register to be written */
+  writeData[0] = regAddr;
+  seq.buf[0].data = writeData;
+
+  /* Only 1 byte reg */
+  writeData[1] = data;
+  seq.buf[0].len = 2;
+
+  if (transfer(&seq) == i2cTransferDone) {
+  	  return true;
+    } else {
+  	  return false;
+    }
+
+}
diff --git a/EFM32/I2Cdev/I2Cdev.h b/EFM32/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..52d19564
--- /dev/null
+++ b/EFM32/I2Cdev/I2Cdev.h
@@ -0,0 +1,84 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// EFM32 stub port by Nicolas Baldeck 
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+// Changelog:
+//      2015-01-02 - Initial release
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+#include 
+#include 
+
+#define I2C_SDA_PORT gpioPortA
+#define I2C_SDA_PIN 0
+#define I2C_SDA_MODE gpioModeWiredAnd
+#define I2C_SDA_DOUT 1
+
+#define I2C_SCL_PORT gpioPortA
+#define I2C_SCL_PIN 1
+#define I2C_SCL_MODE gpioModeWiredAnd
+#define I2C_SCL_DOUT 1
+
+#define I2CDEV_DEFAULT_READ_TIMEOUT 1000000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static void initialize();
+        static void enable(bool isEnabled);
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        //TODO static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        //TODO static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        //TODO static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+
+    private:
+        static I2C_TransferReturn_TypeDef transfer(I2C_TransferSeq_TypeDef *seq, uint16_t timeout=I2Cdev::readTimeout);
+};
+
+#endif /* _I2CDEV_H_ */
diff --git a/ESP32_ESP-IDF/.idea/ESP32_ESP-IDF.iml b/ESP32_ESP-IDF/.idea/ESP32_ESP-IDF.iml
new file mode 100644
index 00000000..bc2cd874
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/ESP32_ESP-IDF.iml
@@ -0,0 +1,8 @@
+
+
+  
+    
+    
+    
+  
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/.idea/encodings.xml b/ESP32_ESP-IDF/.idea/encodings.xml
new file mode 100644
index 00000000..15a15b21
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/encodings.xml
@@ -0,0 +1,4 @@
+
+
+  
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/.idea/misc.xml b/ESP32_ESP-IDF/.idea/misc.xml
new file mode 100644
index 00000000..28a804d8
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/misc.xml
@@ -0,0 +1,6 @@
+
+
+  
+    
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/.idea/modules.xml b/ESP32_ESP-IDF/.idea/modules.xml
new file mode 100644
index 00000000..b92c3089
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+  
+    
+      
+    
+  
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/.idea/vcs.xml b/ESP32_ESP-IDF/.idea/vcs.xml
new file mode 100644
index 00000000..6c0b8635
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+  
+    
+  
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/.idea/workspace.xml b/ESP32_ESP-IDF/.idea/workspace.xml
new file mode 100644
index 00000000..8f9f3d89
--- /dev/null
+++ b/ESP32_ESP-IDF/.idea/workspace.xml
@@ -0,0 +1,18 @@
+
+
+  
+    
+      
+    
+  
+  
+    
+  
+    
+    
+    
+  
+
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/CMakeLists.txt b/ESP32_ESP-IDF/CMakeLists.txt
new file mode 100644
index 00000000..d31555b9
--- /dev/null
+++ b/ESP32_ESP-IDF/CMakeLists.txt
@@ -0,0 +1,8 @@
+# The following lines of boilerplate have to be in your project's
+# CMakeLists in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.5)
+
+
+
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+project(ESP32_ESP-IDF)
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/Makefile b/ESP32_ESP-IDF/Makefile
new file mode 100644
index 00000000..9f2c3278
--- /dev/null
+++ b/ESP32_ESP-IDF/Makefile
@@ -0,0 +1,9 @@
+#
+# This is a project Makefile. It is assumed the directory this Makefile resides in is a
+# project subdirectory.
+#
+
+PROJECT_NAME := app-template
+
+include $(IDF_PATH)/make/project.mk
+
diff --git a/ESP32_ESP-IDF/README.md b/ESP32_ESP-IDF/README.md
new file mode 100644
index 00000000..ebb18425
--- /dev/null
+++ b/ESP32_ESP-IDF/README.md
@@ -0,0 +1,6 @@
+I2CDev library rewritted to match esp-idf. DMP should work, only need is to setup 
+    mpu.setXGyroOffset(220);
+    mpu.setYGyroOffset(76);
+    mpu.setZGyroOffset(-85);
+    mpu.setZAccelOffset(1788);
+and eventually change last value in components/MPU6050/MPU6050_6Axis_MotionApps20.h.
diff --git a/ESP32_ESP-IDF/components/I2Cdev/CMakeLists.txt b/ESP32_ESP-IDF/components/I2Cdev/CMakeLists.txt
new file mode 100644
index 00000000..bfe2fc9a
--- /dev/null
+++ b/ESP32_ESP-IDF/components/I2Cdev/CMakeLists.txt
@@ -0,0 +1,3 @@
+idf_component_register(SRCS "I2Cdev.cpp"
+                       INCLUDE_DIRS "."
+)
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.cpp b/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..2c7a0e2f
--- /dev/null
+++ b/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,270 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// EFM32 stub port by Nicolas Baldeck 
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+// Changelog:
+//      2015-01-02 - Initial release
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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 
+#include 
+#include 
+#include 
+#include "sdkconfig.h"
+
+#include "I2Cdev.h"
+
+#define I2C_NUM I2C_NUM_0
+
+#undef ESP_ERROR_CHECK
+#define ESP_ERROR_CHECK(x)   do { esp_err_t rc = (x); if (rc != ESP_OK) { ESP_LOGE("err", "esp_err_t = %d", rc); /*assert(0 && #x);*/} } while(0);
+
+/** Default constructor.
+ */
+I2Cdev::I2Cdev() {
+}
+
+/** Initialize I2C0
+ */
+void I2Cdev::initialize() {
+
+}
+
+/** Enable or disable I2C
+ * @param isEnabled true = enable, false = disable
+ */
+void I2Cdev::enable(bool isEnabled) {
+  
+}
+
+/** Default timeout value for read operations.
+ */
+uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
+
+
+	uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
+	i2c_cmd_handle_t cmd;
+	SelectRegister(devAddr, regAddr);
+
+	cmd = i2c_cmd_link_create();
+	ESP_ERROR_CHECK(i2c_master_start(cmd));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_READ, 1));
+
+	if(length>1)
+		ESP_ERROR_CHECK(i2c_master_read(cmd, data, length-1, I2C_MASTER_ACK));
+
+	ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+length-1, I2C_MASTER_NACK));
+
+	ESP_ERROR_CHECK(i2c_master_stop(cmd));
+	ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
+	i2c_cmd_link_delete(cmd);
+
+	return length;
+}
+
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data){
+
+	uint8_t data1[] = {(uint8_t)(data>>8), (uint8_t)(data & 0xff)};
+	writeBytes(devAddr, regAddr, 2, data1);
+	return true;
+}
+
+void I2Cdev::SelectRegister(uint8_t dev, uint8_t reg){
+	i2c_cmd_handle_t cmd;
+
+	cmd = i2c_cmd_link_create();
+	ESP_ERROR_CHECK(i2c_master_start(cmd));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (dev << 1) | I2C_MASTER_WRITE, 1));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, reg, 1));
+	ESP_ERROR_CHECK(i2c_master_stop(cmd));
+	ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
+	i2c_cmd_link_delete(cmd);
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b = 0;
+    if (readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+	i2c_cmd_handle_t cmd;
+
+	cmd = i2c_cmd_link_create();
+	ESP_ERROR_CHECK(i2c_master_start(cmd));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data, 1));
+	ESP_ERROR_CHECK(i2c_master_stop(cmd));
+	ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
+	i2c_cmd_link_delete(cmd);
+
+	return true;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param length Number of bytes to write
+ * @param data Array of bytes to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){
+	i2c_cmd_handle_t cmd;
+
+	cmd = i2c_cmd_link_create();
+	ESP_ERROR_CHECK(i2c_master_start(cmd));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1));
+	ESP_ERROR_CHECK(i2c_master_write(cmd, data, length-1, 0));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data[length-1], 1));
+	ESP_ERROR_CHECK(i2c_master_stop(cmd));
+	ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
+	i2c_cmd_link_delete(cmd);
+	return true;
+}
+
+
+/**
+ * read word
+ * @param devAddr
+ * @param regAddr
+ * @param data
+ * @param timeout
+ * @return
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout){
+	uint8_t msb[2] = {0,0};
+	readBytes(devAddr, regAddr, 2, msb);
+	*data = (int16_t)((msb[0] << 8) | msb[1]);
+	return 0;
+}
+
+
diff --git a/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.h b/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..f2e8ce13
--- /dev/null
+++ b/ESP32_ESP-IDF/components/I2Cdev/I2Cdev.h
@@ -0,0 +1,83 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// EFM32 stub port by Nicolas Baldeck 
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+// Changelog:
+//      2015-01-02 - Initial release
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+
+#define I2C_SDA_PORT gpioPortA
+#define I2C_SDA_PIN 0
+#define I2C_SDA_MODE gpioModeWiredAnd
+#define I2C_SDA_DOUT 1
+
+#define I2C_SCL_PORT gpioPortA
+#define I2C_SCL_PIN 1
+#define I2C_SCL_MODE gpioModeWiredAnd
+#define I2C_SCL_DOUT 1
+
+#define I2CDEV_DEFAULT_READ_TIMEOUT 1000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static void initialize();
+        static void enable(bool isEnabled);
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        //TODO static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+
+    //private:
+        static void SelectRegister(uint8_t dev, uint8_t reg);
+        //static I2C_TransferReturn_TypeDef transfer(I2C_TransferSeq_TypeDef *seq, uint16_t timeout=I2Cdev::readTimeout);
+};
+
+#endif /* _I2CDEV_H_ */
diff --git a/ESP32_ESP-IDF/components/I2Cdev/component.mk b/ESP32_ESP-IDF/components/I2Cdev/component.mk
new file mode 100644
index 00000000..3d61395f
--- /dev/null
+++ b/ESP32_ESP-IDF/components/I2Cdev/component.mk
@@ -0,0 +1,10 @@
+#
+# Main component makefile.
+#
+# This Makefile can be left empty. By default, it will take the sources in the 
+# src/ directory, compile them and link them into lib(subdirectory_name).a 
+# in the build directory. This behaviour is entirely configurable,
+# please read the ESP-IDF documents if you need to do this.
+#
+
+COMPONENT_ADD_INCLUDEDIRS=.
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/components/MPU6050/CMakeLists.txt b/ESP32_ESP-IDF/components/MPU6050/CMakeLists.txt
new file mode 100644
index 00000000..38d04eb4
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/CMakeLists.txt
@@ -0,0 +1,4 @@
+idf_component_register(SRCS "MPU6050.cpp"
+                       INCLUDE_DIRS "."
+                       REQUIRES I2Cdev
+)
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/components/MPU6050/MPU6050.cpp b/ESP32_ESP-IDF/components/MPU6050/MPU6050.cpp
new file mode 100644
index 00000000..61f5c7e1
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050.cpp
@@ -0,0 +1,3355 @@
+// 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 
+// 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 
+
+#define I2C_NUM I2C_NUM_0
+
+void MPU6050::ReadRegister(uint8_t reg, uint8_t *data, uint8_t len){
+	uint8_t dev = 0x68;
+	i2c_cmd_handle_t cmd;
+	I2Cdev::SelectRegister(dev, reg);
+
+	cmd = i2c_cmd_link_create();
+	ESP_ERROR_CHECK(i2c_master_start(cmd));
+	ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (dev << 1) | I2C_MASTER_READ, 1));
+
+	if(len>1)
+		ESP_ERROR_CHECK(i2c_master_read(cmd, data, len, I2C_MASTER_ACK));
+
+	ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+len-1, I2C_MASTER_NACK));
+
+	ESP_ERROR_CHECK(i2c_master_stop(cmd));
+	ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000));
+	i2c_cmd_link_delete(cmd);
+}
+
+
+/** 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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 

+ *
+ * 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
+ *
+ */
+
+
+/**
+ *
+ * @param val
+ * @param I_Min
+ * @param I_Max
+ * @param O_Min
+ * @param O_Max
+ * @return
+ */
+
+float map(float val, float I_Min, float I_Max, float O_Min, float O_Max){
+    return(val/(I_Max-I_Min)*(O_Max-O_Min) + O_Min);
+}
+
+
+
+
+
+/**
+  @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();
+}
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/components/MPU6050/MPU6050.h b/ESP32_ESP-IDF/components/MPU6050/MPU6050.h
new file mode 100644
index 00000000..304bfa1e
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050.h
@@ -0,0 +1,1038 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 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 "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 ReadRegister(uint8_t reg, uint8_t *data, uint8_t len);
+
+        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/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h b/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h
new file mode 100644
index 00000000..66426c58
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050_6Axis_MotionApps20.h
@@ -0,0 +1,764 @@
+// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 5/20/2013 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ... - ongoing debug release
+
+/* ============================================
+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_6AXIS_MOTIONAPPS20_H_
+#define _MPU6050_6AXIS_MOTIONAPPS20_H_
+
+#include "I2Cdev.h"
+//#include "helper_3dmath.h"
+#include 
+#include 
+#include 
+#include "sdkconfig.h"
+
+// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
+
+#include "MPU6050.h"
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifdef __AVR__
+    #include 
+#else
+    // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+    #ifndef __PGMSPACE_H_
+        #define __PGMSPACE_H_ 1
+        #include 
+
+        #define PROGMEM
+        #define PGM_P  const char *
+        #define PSTR(str) (str)
+        #define F(x) x
+
+        typedef void prog_void;
+        typedef char prog_char;
+        typedef unsigned char prog_uchar;
+        typedef int8_t prog_int8_t;
+        typedef uint8_t prog_uint8_t;
+        typedef int16_t prog_int16_t;
+        typedef uint16_t prog_uint16_t;
+        typedef int32_t prog_int32_t;
+        typedef uint32_t prog_uint32_t;
+        
+        #define strcpy_P(dest, src) strcpy((dest), (src))
+        #define strcat_P(dest, src) strcat((dest), (src))
+        #define strcmp_P(a, b) strcmp((a), (b))
+        
+        #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+        #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+        #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+        #define pgm_read_float(addr) (*(const float *)(addr))
+        
+        #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+        #define pgm_read_word_near(addr) pgm_read_word(addr)
+        #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+        #define pgm_read_float_near(addr) pgm_read_float(addr)
+        #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+        #define pgm_read_word_far(addr) pgm_read_word(addr)
+        #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+        #define pgm_read_float_far(addr) pgm_read_float(addr)
+    #endif
+#endif
+
+/* Source is from the InvenSense MotionApps v2 demo code. Original source is
+ * unavailable, unless you happen to be amazing as decompiling binary by
+ * hand (in which case, please contact me, and I'm totally serious).
+ *
+ * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
+ * DMP reverse-engineering he did to help make this bit of wizardry
+ * possible.
+ */
+
+// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
+// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
+// after moving string constants to flash memory storage using the F()
+// compiler macro (Arduino IDE 1.0+ required).
+
+#define DEBUG
+#ifdef DEBUG
+#define HEX "f"
+    #define DEBUG_PRINT(x) printf("%d", x)
+    #define DEBUG_PRINTF(x, y) Serial.print(x, y)
+	#define DEBUG_PRINTLN(x) printf("%s\n", x)
+    #define DEBUG_PRINTLNF(x, y) printf("%#06x%s\n", x, y)
+#else
+    #define DEBUG_PRINT(x)
+    #define DEBUG_PRINTF(x, y)
+    #define DEBUG_PRINTLN(x)
+    #define DEBUG_PRINTLNF(x, y)
+#endif
+
+#define MPU6050_DMP_CODE_SIZE       1929    // dmpMemory[]
+#define MPU6050_DMP_CONFIG_SIZE     192     // dmpConfig[]
+#define MPU6050_DMP_UPDATES_SIZE    47      // dmpUpdates[]
+
+/* ================================================================================================ *
+ | Default MotionApps v2.0 42-byte FIFO packet structure:                                           |
+ |                                                                                                  |
+ | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
+ |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
+ |                                                                                                  |
+ | [GYRO Z][      ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ]                         |
+ |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41                          |
+ * ================================================================================================ */
+
+// this block of memory gets written to the MPU on start-up, and it seems
+// to be volatile memory, so it has to be done each time (it only takes ~1
+// second though)
+const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] = {
+	/* bank # 0 */
+	0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
+	0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
+	0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00,
+	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+	0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
+	0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
+	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
+	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
+	0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
+	0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
+	/* bank # 1 */
+	0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
+	0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
+	0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
+	0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00,
+	0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
+	/* bank # 2 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	/* bank # 3 */
+	0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
+	0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
+	0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
+	0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
+	0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
+	0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
+	0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
+	0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C,
+	0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
+	0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
+	0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
+	0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
+	0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
+	0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
+	0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
+	0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
+	/* bank # 4 */
+	0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
+	0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
+	0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
+	0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
+	0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
+	0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
+	0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
+	0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
+	0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
+	0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
+	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
+	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
+	0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
+	0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
+	/* bank # 5 */
+	0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
+	0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
+	0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
+	0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
+	0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
+	0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
+	0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
+	0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
+	0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
+	0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
+	0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
+	0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
+	0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
+	0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
+	0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
+	0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
+	/* bank # 6 */
+	0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
+	0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
+	0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
+	0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
+	0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
+	0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
+	0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
+	0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
+	0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
+	0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
+	0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
+	0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
+	0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
+	0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
+	0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
+	0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
+	/* bank # 7 */
+	0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
+	0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
+	0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
+	0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
+	0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
+	0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+	0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38,
+	0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC,
+	0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF,
+};
+
+// thanks to Noah Zerkin for piecing this stuff together!
+const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] = {
+//  BANK    OFFSET  LENGTH  [DATA]
+    0x03,   0x7B,   0x03,   0x4C, 0xCD, 0x6C,         // FCFG_1 inv_set_gyro_calibration
+    0x03,   0xAB,   0x03,   0x36, 0x56, 0x76,         // FCFG_3 inv_set_gyro_calibration
+    0x00,   0x68,   0x04,   0x02, 0xCB, 0x47, 0xA2,   // D_0_104 inv_set_gyro_calibration
+    0x02,   0x18,   0x04,   0x00, 0x05, 0x8B, 0xC1,   // D_0_24 inv_set_gyro_calibration
+    0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // D_1_152 inv_set_accel_calibration
+    0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration
+    0x03,   0x89,   0x03,   0x26, 0x46, 0x66,         // FCFG_7 inv_set_accel_calibration
+    0x00,   0x6C,   0x02,   0x20, 0x00,               // D_0_108 inv_set_accel_calibration
+    0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_00 inv_set_compass_calibration
+    0x02,   0x44,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_01
+    0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_02
+    0x02,   0x4C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_10
+    0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_11
+    0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_12
+    0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_20
+    0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_21
+    0x02,   0xBC,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_22
+    0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,   // D_1_236 inv_apply_endian_accel
+    0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
+    0x04,   0x02,   0x03,   0x0D, 0x35, 0x5D,         // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
+    0x04,   0x09,   0x04,   0x87, 0x2D, 0x35, 0x3D,   // FCFG_5 inv_set_bias_update
+    0x00,   0xA3,   0x01,   0x00,                     // D_0_163 inv_set_dead_zone
+                 // SPECIAL 0x01 = enable interrupts
+    0x00,   0x00,   0x00,   0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
+    0x07,   0x86,   0x01,   0xFE,                     // CFG_6 inv_set_fifo_interupt
+    0x07,   0x41,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
+    0x07,   0x7E,   0x01,   0x30,                     // CFG_16 inv_set_footer
+    0x07,   0x46,   0x01,   0x9A,                     // CFG_GYRO_SOURCE inv_send_gyro
+    0x07,   0x47,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_9 inv_send_gyro -> inv_construct3_fifo
+    0x07,   0x6C,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
+    0x02,   0x16,   0x02,   0x00, 0x13                // D_0_22 inv_set_fifo_rate
+
+    // This very last 0x13 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
+    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
+    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
+
+    // It is important to make sure the host processor can keep up with reading and processing
+    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
+};
+
+const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] = {
+    0x01,   0xB2,   0x02,   0xFF, 0xFF,
+    0x01,   0x90,   0x04,   0x09, 0x23, 0xA1, 0x35,
+    0x01,   0x6A,   0x02,   0x06, 0x00,
+    0x01,   0x60,   0x08,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
+    0x01,   0x62,   0x02,   0x00, 0x00,
+    0x00,   0x60,   0x04,   0x00, 0x40, 0x00, 0x00
+};
+
+#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR 
+#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x00 // The New instance of the Firmware has this as the default
+#endif
+
+uint8_t MPU6050::dmpInitialize() {
+    // reset device
+    DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
+    reset();
+    vTaskDelay(30/portTICK_PERIOD_MS); // wait after reset
+
+    // enable sleep mode and wake cycle
+    /*Serial.println(F("Enabling sleep mode..."));
+    setSleepEnabled(true);
+    Serial.println(F("Enabling wake cycle..."));
+    setWakeCycleEnabled(true);*/
+
+    // disable sleep mode
+    DEBUG_PRINTLN(F("Disabling sleep mode..."));
+    setSleepEnabled(false);
+
+    // get MPU hardware revision
+    DEBUG_PRINTLN(F("Selecting user bank 16..."));
+    setMemoryBank(0x10, true, true);
+    DEBUG_PRINTLN(F("Selecting memory byte 6..."));
+    setMemoryStartAddress(0x06);
+    DEBUG_PRINTLN(F("Checking hardware revision..."));
+    DEBUG_PRINTLN(F("Revision @ user[16][6] = "));
+    DEBUG_PRINTLNF(readMemoryByte(), HEX);
+    DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
+    setMemoryBank(0, false, false);
+
+    // check OTP bank valid
+    DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
+    DEBUG_PRINTLN(F("OTP bank is "));
+    DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!"));
+
+    // get X/Y/Z gyro offsets
+    DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
+    int8_t xgOffsetTC = getXGyroOffsetTC();
+    int8_t ygOffsetTC = getYGyroOffsetTC();
+    int8_t zgOffsetTC = getZGyroOffsetTC();
+    DEBUG_PRINTLN(F("X gyro offset = "));
+    DEBUG_PRINT(xgOffsetTC);
+    DEBUG_PRINTLN(F("Y gyro offset = "));
+    DEBUG_PRINT(ygOffsetTC);
+    DEBUG_PRINTLN(F("Z gyro offset = "));
+    DEBUG_PRINT(zgOffsetTC);
+
+    // setup weird slave stuff (?)
+    DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
+    setSlaveAddress(0, 0x7F);
+    DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
+    setI2CMasterModeEnabled(false);
+    DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
+    setSlaveAddress(0, 0x68);
+    DEBUG_PRINTLN(F("Resetting I2C Master control..."));
+    resetI2CMaster();
+    vTaskDelay(20/portTICK_PERIOD_MS); // wait after reset
+
+    // load DMP code into memory banks
+    DEBUG_PRINTLN(F("Writing DMP code to MPU memory banks ("));
+    DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
+    DEBUG_PRINTLN(F(" bytes)"));
+    if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
+        DEBUG_PRINTLN(F("Success! DMP code written and verified."));
+
+        // Set the FIFO Rate Divisor int the DMP Firmware Memory
+        unsigned char dmpUpdate[] = {0x00, MPU6050_DMP_FIFO_RATE_DIVISOR};
+        writeMemoryBlock(dmpUpdate, 0x02, 0x02, 0x16); // Lets write the dmpUpdate data to the Firmware image, we have 2 bytes to write in bank 0x02 with the Offset 0x16
+
+        // write DMP configuration
+        // DEBUG_PRINTLN(F("Writing DMP configuration to MPU memory banks ("));
+        // DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
+        // DEBUG_PRINTLN(F(" bytes in config def)"));
+        if (true) {
+            DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
+
+            DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
+            setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
+
+            DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
+            setIntEnabled(0x12);
+
+            DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
+            setRate(4); // 1khz / (1 + 4) = 200 Hz
+
+            DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
+            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
+
+            DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
+            setDLPFMode(MPU6050_DLPF_BW_42);
+
+            DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
+            setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+
+            DEBUG_PRINTLN(F("Setting DMP programm start address"));
+            //write start address MSB into register
+            setDMPConfig1(0x03);
+            //write start address LSB into register
+            setDMPConfig2(0x00);
+
+            DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
+            setOTPBankValid(false);
+
+            DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
+            setXGyroOffsetTC(xgOffsetTC);
+            setYGyroOffsetTC(ygOffsetTC);
+            setZGyroOffsetTC(zgOffsetTC);
+
+            //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
+            //setXGyroOffset(0);
+            //setYGyroOffset(0);
+            //setZGyroOffset(0);
+
+            // DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)..."));
+            // uint8_t dmpUpdate[16], j;
+            // uint16_t pos = 0;
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            // DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            resetFIFO();
+
+            DEBUG_PRINTLN(F("Reading FIFO count..."));
+            uint16_t fifoCount = getFIFOCount();
+            uint8_t fifoBuffer[128];
+
+            DEBUG_PRINTLN(F("Current FIFO count="));
+            DEBUG_PRINT(fifoCount);
+            getFIFOBytes(fifoBuffer, fifoCount);
+
+            DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
+            setMotionDetectionThreshold(2);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
+            setZeroMotionDetectionThreshold(156);
+
+            DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
+            setMotionDetectionDuration(80);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
+            setZeroMotionDetectionDuration(0);
+
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            resetFIFO();
+
+            DEBUG_PRINTLN(F("Enabling FIFO..."));
+            setFIFOEnabled(true);
+
+            DEBUG_PRINTLN(F("Enabling DMP..."));
+            setDMPEnabled(true);
+
+            DEBUG_PRINTLN(F("Resetting DMP..."));
+            resetDMP();
+
+            // DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            // DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            // DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
+            while ((fifoCount = getFIFOCount()) < 3);
+
+            DEBUG_PRINTLN(F("Current FIFO count="));
+            DEBUG_PRINT(fifoCount);
+            DEBUG_PRINTLN(F("Reading FIFO data..."));
+            getFIFOBytes(fifoBuffer, fifoCount);
+
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+
+            DEBUG_PRINTLN(F("Current interrupt status="));
+            DEBUG_PRINTLNF(getIntStatus(), HEX);
+
+            // DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
+            while ((fifoCount = getFIFOCount()) < 3);
+
+            DEBUG_PRINTLN(F("Current FIFO count="));
+            DEBUG_PRINT(fifoCount);
+
+            DEBUG_PRINTLN(F("Reading FIFO data..."));
+            getFIFOBytes(fifoBuffer, fifoCount);
+
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+
+            DEBUG_PRINTLN(F("Current interrupt status="));
+            DEBUG_PRINTLNF(getIntStatus(), HEX);
+
+            // DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
+            // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            // writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("DMP is good to go! Finally."));
+
+            DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
+            setDMPEnabled(false);
+
+            DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
+            dmpPacketSize = 42;
+            /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
+                return 3; // TODO: proper error code for no memory
+            }*/
+
+            DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
+            resetFIFO();
+            getIntStatus();
+        } else {
+            DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
+            return 2; // configuration block loading failed
+        }
+    } else {
+        DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
+        return 1; // main binary block loading failed
+    }
+    return 0; // success
+}
+
+bool MPU6050::dmpPacketAvailable() {
+    return getFIFOCount() >= dmpGetFIFOPacketSize();
+}
+
+// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
+// uint8_t MPU6050::dmpGetFIFORate();
+// uint8_t MPU6050::dmpGetSampleStepSizeMS();
+// uint8_t MPU6050::dmpGetSampleFrequency();
+// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
+
+//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
+//uint8_t MPU6050::dmpRunFIFORateProcesses();
+
+// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]);
+    data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]);
+    data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[28] << 8) | packet[29];
+    data[1] = (packet[32] << 8) | packet[33];
+    data[2] = (packet[36] << 8) | packet[37];
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[28] << 8) | packet[29];
+    v -> y = (packet[32] << 8) | packet[33];
+    v -> z = (packet[36] << 8) | packet[37];
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
+    data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
+    data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
+    data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 8) | packet[1]);
+    data[1] = ((packet[4] << 8) | packet[5]);
+    data[2] = ((packet[8] << 8) | packet[9]);
+    data[3] = ((packet[12] << 8) | packet[13]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    if (status == 0) {
+        q -> w = (float)qI[0] / 16384.0f;
+        q -> x = (float)qI[1] / 16384.0f;
+        q -> y = (float)qI[2] / 16384.0f;
+        q -> z = (float)qI[3] / 16384.0f;
+        return 0;
+    }
+    return status; // int16 return value, indicates error if this line is reached
+}
+// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
+    data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
+    data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[16] << 8) | packet[17];
+    data[1] = (packet[20] << 8) | packet[21];
+    data[2] = (packet[24] << 8) | packet[25];
+    return 0;
+}
+uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[16] << 8) | packet[17];
+    v -> y = (packet[20] << 8) | packet[21];
+    v -> z = (packet[24] << 8) | packet[25];
+    return 0;
+}
+// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
+// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
+    // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
+    v -> x = vRaw -> x - gravity -> x*8192;
+    v -> y = vRaw -> y - gravity -> y*8192;
+    v -> z = vRaw -> z - gravity -> z*8192;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
+    // rotate measured 3D acceleration vector into original state
+    // frame of reference based on orientation quaternion
+    memcpy(v, vReal, sizeof(VectorInt16));
+    v -> rotate(q);
+    return 0;
+}
+// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
+    return status;
+}
+
+uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
+    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
+    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
+    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
+    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
+    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
+    return 0;
+}
+uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
+    return 0;
+}
+
+// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
+    /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
+        if (dmpData[k] < 0x10) Serial.print("0");
+        Serial.print(dmpData[k], HEX);
+        Serial.print(" ");
+    }
+    Serial.print("\n");*/
+    //Serial.println((uint16_t)dmpPacketBuffer);
+    return 0;
+}
+uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
+    uint8_t status;
+    uint8_t buf[dmpPacketSize];
+    for (uint8_t i = 0; i < numPackets; i++) {
+        // read packet from FIFO
+        getFIFOBytes(buf, dmpPacketSize);
+
+        // process packet
+        if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
+        
+        // increment external process count variable, if supplied
+        if (processed != 0) (*processed)++;
+    }
+    return 0;
+}
+
+// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
+
+// uint8_t MPU6050::dmpInitFIFOParam();
+// uint8_t MPU6050::dmpCloseFIFO();
+// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
+// uint8_t MPU6050::dmpDecodeQuantizedAccel();
+// uint32_t MPU6050::dmpGetGyroSumOfSquare();
+// uint32_t MPU6050::dmpGetAccelSumOfSquare();
+// void MPU6050::dmpOverrideQuaternion(long *q);
+uint16_t MPU6050::dmpGetFIFOPacketSize() {
+    return dmpPacketSize;
+}
+
+#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
diff --git a/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h b/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h
new file mode 100644
index 00000000..b86a476e
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/MPU6050_9Axis_MotionApps41.h
@@ -0,0 +1,863 @@
+// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 6/18/2012 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ... - ongoing debug release
+
+/* ============================================
+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_9AXIS_MOTIONAPPS41_H_
+#define _MPU6050_9AXIS_MOTIONAPPS41_H_
+
+#include "I2Cdev.h"
+#include "helper_3dmath.h"
+
+// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS41
+
+#include "MPU6050.h"
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifdef __AVR__
+    #include 
+#else
+    // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+    #ifndef __PGMSPACE_H_
+        #define __PGMSPACE_H_ 1
+        #include 
+
+        #define PROGMEM
+        #define PGM_P  const char *
+        #define PSTR(str) (str)
+        #define F(x) x
+
+        typedef void prog_void;
+        typedef char prog_char;
+        //typedef unsigned char prog_uchar;
+        typedef int8_t prog_int8_t;
+        typedef uint8_t prog_uint8_t;
+        typedef int16_t prog_int16_t;
+        typedef uint16_t prog_uint16_t;
+        typedef int32_t prog_int32_t;
+        typedef uint32_t prog_uint32_t;
+        
+        #define strcpy_P(dest, src) strcpy((dest), (src))
+        #define strcat_P(dest, src) strcat((dest), (src))
+        #define strcmp_P(a, b) strcmp((a), (b))
+        
+        #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
+        #define pgm_read_word(addr) (*(const unsigned short *)(addr))
+        #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
+        #define pgm_read_float(addr) (*(const float *)(addr))
+        
+        #define pgm_read_byte_near(addr) pgm_read_byte(addr)
+        #define pgm_read_word_near(addr) pgm_read_word(addr)
+        #define pgm_read_dword_near(addr) pgm_read_dword(addr)
+        #define pgm_read_float_near(addr) pgm_read_float(addr)
+        #define pgm_read_byte_far(addr) pgm_read_byte(addr)
+        #define pgm_read_word_far(addr) pgm_read_word(addr)
+        #define pgm_read_dword_far(addr) pgm_read_dword(addr)
+        #define pgm_read_float_far(addr) pgm_read_float(addr)
+    #endif
+#endif
+
+// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
+// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
+// after moving string constants to flash memory storage using the F()
+// compiler macro (Arduino IDE 1.0+ required).
+
+//#define DEBUG
+#ifdef DEBUG
+    #define DEBUG_PRINT(x) Serial.print(x)
+    #define DEBUG_PRINTF(x, y) Serial.print(x, y)
+    #define DEBUG_PRINTLN(x) Serial.println(x)
+    #define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
+#else
+    #define DEBUG_PRINT(x)
+    #define DEBUG_PRINTF(x, y)
+    #define DEBUG_PRINTLN(x)
+    #define DEBUG_PRINTLNF(x, y)
+#endif
+
+#define MPU6050_DMP_CODE_SIZE       1962    // dmpMemory[]
+#define MPU6050_DMP_CONFIG_SIZE     232     // dmpConfig[]
+#define MPU6050_DMP_UPDATES_SIZE    140     // dmpUpdates[]
+
+/* ================================================================================================ *
+ | Default MotionApps v4.1 48-byte FIFO packet structure:                                           |
+ |                                                                                                  |
+ | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
+ |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
+ |                                                                                                  |
+ | [GYRO Z][      ][MAG X ][MAG Y ][MAG Z ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ] |
+ |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41  42  43  44  45  46  47  |
+ * ================================================================================================ */
+
+// this block of memory gets written to the MPU on start-up, and it seems
+// to be volatile memory, so it has to be done each time (it only takes ~1
+// second though)
+const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
+    // bank 0, 256 bytes
+    0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
+    0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
+    0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
+    0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+    0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
+    0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
+    0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
+    0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
+
+    // bank 1, 256 bytes
+    0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
+    0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
+    0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
+    0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
+    
+    // bank 2, 256 bytes
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
+    0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
+    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    
+    // bank 3, 256 bytes
+    0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
+    0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
+    0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
+    0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
+    0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
+    0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
+    0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
+    0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
+    0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
+    0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
+    0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
+    0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
+    0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
+    0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
+    0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
+    0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,
+    
+    // bank 4, 256 bytes
+    0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
+    0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
+    0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
+    0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
+    0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
+    0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
+    0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
+    0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
+    0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
+    0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
+    0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
+    0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
+    0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+    0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
+    0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+    0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
+    
+    // bank 5, 256 bytes
+    0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
+    0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
+    0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
+    0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
+    0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
+    0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
+    0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
+    0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
+    0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
+    0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
+    0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
+    0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
+    0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
+    0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
+    0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
+    0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,
+
+    // bank 6, 256 bytes
+    0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
+    0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
+    0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
+    0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
+    0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
+    0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
+    0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
+    0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
+    0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
+    0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
+    0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
+    0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
+    0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
+    0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
+    0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
+    0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,
+    
+    // bank 7, 170 bytes (remainder)
+    0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
+    0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
+    0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
+    0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
+    0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
+    0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
+    0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
+    0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+    0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
+    0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
+    0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
+};
+
+const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
+//  BANK    OFFSET  LENGTH  [DATA]
+    0x02,   0xEC,   0x04,   0x00, 0x47, 0x7D, 0x1A,   // ?
+    0x03,   0x82,   0x03,   0x4C, 0xCD, 0x6C,         // FCFG_1 inv_set_gyro_calibration
+    0x03,   0xB2,   0x03,   0x36, 0x56, 0x76,         // FCFG_3 inv_set_gyro_calibration
+    0x00,   0x68,   0x04,   0x02, 0xCA, 0xE3, 0x09,   // D_0_104 inv_set_gyro_calibration
+    0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // D_1_152 inv_set_accel_calibration
+    0x03,   0x86,   0x03,   0x0C, 0xC9, 0x2C,         // FCFG_2 inv_set_accel_calibration
+    0x03,   0x90,   0x03,   0x26, 0x46, 0x66,         //   (continued)...FCFG_2 inv_set_accel_calibration
+    0x00,   0x6C,   0x02,   0x40, 0x00,               // D_0_108 inv_set_accel_calibration
+
+    0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_00 inv_set_compass_calibration
+    0x02,   0x44,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_01
+    0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_02
+    0x02,   0x4C,   0x04,   0x40, 0x00, 0x00, 0x00,   // CPASS_MTX_10
+    0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_11
+    0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_12
+    0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_20
+    0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_21
+    0x02,   0xBC,   0x04,   0xC0, 0x00, 0x00, 0x00,   // CPASS_MTX_22
+
+    0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,   // D_1_236 inv_apply_endian_accel
+    0x03,   0x86,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
+    0x04,   0x22,   0x03,   0x0D, 0x35, 0x5D,         // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
+    0x00,   0xA3,   0x01,   0x00,                     // ?
+    0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,   // FCFG_5 inv_set_bias_update
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
+    0x07,   0x9F,   0x01,   0x30,                     // CFG_16 inv_set_footer
+    0x07,   0x67,   0x01,   0x9A,                     // CFG_GYRO_SOURCE inv_send_gyro
+    0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_9 inv_send_gyro -> inv_construct3_fifo
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
+    0x02,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // ?
+    0x07,   0x83,   0x06,   0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
+                 // SPECIAL 0x01 = enable interrupts
+    0x00,   0x00,   0x00,   0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
+    0x07,   0xA7,   0x01,   0xFE,                     // ?
+    0x07,   0x62,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // ?
+    0x07,   0x67,   0x01,   0x9A,                     // ?
+    0x07,   0x68,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
+    0x07,   0x8D,   0x04,   0xF1, 0x28, 0x30, 0x38,   // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
+    0x02,   0x16,   0x02,   0x00, 0x03                // D_0_22 inv_set_fifo_rate
+
+    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
+    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
+    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
+
+    // It is important to make sure the host processor can keep up with reading and processing
+    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
+};
+
+const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
+    0x01,   0xB2,   0x02,   0xFF, 0xF5,
+    0x01,   0x90,   0x04,   0x0A, 0x0D, 0x97, 0xC0,
+    0x00,   0xA3,   0x01,   0x00,
+    0x04,   0x29,   0x04,   0x87, 0x2D, 0x35, 0x3D,
+    0x01,   0x6A,   0x02,   0x06, 0x00,
+    0x01,   0x60,   0x08,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
+    0x02,   0x60,   0x0C,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+    0x01,   0x08,   0x02,   0x01, 0x20,
+    0x01,   0x0A,   0x02,   0x00, 0x4E,
+    0x01,   0x02,   0x02,   0xFE, 0xB3,
+    0x02,   0x6C,   0x04,   0x00, 0x00, 0x00, 0x00, // READ
+    0x02,   0x6C,   0x04,   0xFA, 0xFE, 0x00, 0x00,
+    0x02,   0x60,   0x0C,   0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
+    0x02,   0xF4,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x02,   0xF8,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x02,   0xFC,   0x04,   0x00, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
+    0x00,   0x60,   0x04,   0x00, 0x40, 0x00, 0x00
+};
+
+uint8_t MPU6050::dmpInitialize() {
+    // reset device
+    DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
+    reset();
+    delay(30); // wait after reset
+
+    // disable sleep mode
+    DEBUG_PRINTLN(F("Disabling sleep mode..."));
+    setSleepEnabled(false);
+
+    // get MPU product ID
+    DEBUG_PRINTLN(F("Getting product ID..."));
+    //uint8_t productID = 0; //getProductID();
+    DEBUG_PRINT(F("Product ID = "));
+    DEBUG_PRINT(productID);
+
+    // get MPU hardware revision
+    DEBUG_PRINTLN(F("Selecting user bank 16..."));
+    setMemoryBank(0x10, true, true);
+    DEBUG_PRINTLN(F("Selecting memory byte 6..."));
+    setMemoryStartAddress(0x06);
+    DEBUG_PRINTLN(F("Checking hardware revision..."));
+    uint8_t hwRevision = readMemoryByte();
+    DEBUG_PRINT(F("Revision @ user[16][6] = "));
+    DEBUG_PRINTLNF(hwRevision, HEX);
+    DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
+    setMemoryBank(0, false, false);
+
+    // check OTP bank valid
+    DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
+    uint8_t otpValid = getOTPBankValid();
+    DEBUG_PRINT(F("OTP bank is "));
+    DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
+
+    // get X/Y/Z gyro offsets
+    DEBUG_PRINTLN(F("Reading gyro offset values..."));
+    int8_t xgOffset = getXGyroOffset();
+    int8_t ygOffset = getYGyroOffset();
+    int8_t zgOffset = getZGyroOffset();
+    DEBUG_PRINT(F("X gyro offset = "));
+    DEBUG_PRINTLN(xgOffset);
+    DEBUG_PRINT(F("Y gyro offset = "));
+    DEBUG_PRINTLN(ygOffset);
+    DEBUG_PRINT(F("Z gyro offset = "));
+    DEBUG_PRINTLN(zgOffset);
+    
+    I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ?
+    
+    DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled"));
+    I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32);
+
+    // enable MPU AUX I2C bypass mode
+    //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode..."));
+    //setI2CBypassEnabled(true);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
+    //mag -> setMode(0);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x00);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
+    //mag -> setMode(0x0F);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
+
+    DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
+    int8_t asax, asay, asaz;
+    //mag -> getAdjustment(&asax, &asay, &asaz);
+    I2Cdev::readBytes(0x0E, 0x10, 3, buffer);
+    asax = (int8_t)buffer[0];
+    asay = (int8_t)buffer[1];
+    asaz = (int8_t)buffer[2];
+    DEBUG_PRINT(F("Adjustment X/Y/Z = "));
+    DEBUG_PRINT(asax);
+    DEBUG_PRINT(F(" / "));
+    DEBUG_PRINT(asay);
+    DEBUG_PRINT(F(" / "));
+    DEBUG_PRINTLN(asaz);
+
+    DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
+    //mag -> setMode(0);
+    I2Cdev::writeByte(0x0E, 0x0A, 0x00);
+
+    // load DMP code into memory banks
+    DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
+    DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
+    DEBUG_PRINTLN(F(" bytes)"));
+    if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
+        DEBUG_PRINTLN(F("Success! DMP code written and verified."));
+
+        DEBUG_PRINTLN(F("Configuring DMP and related settings..."));
+
+        // write DMP configuration
+        DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
+        DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
+        DEBUG_PRINTLN(F(" bytes in config def)"));
+        if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
+            DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
+
+            DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
+            setIntEnabled(0x12);
+
+            DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
+            setRate(4); // 1khz / (1 + 4) = 200 Hz
+
+            DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
+            setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
+
+            DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
+            setDLPFMode(MPU6050_DLPF_BW_42);
+
+            DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
+            setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
+
+            DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
+            setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+
+            DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
+            setDMPConfig1(0x03);
+            setDMPConfig2(0x00);
+
+            DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
+            setOTPBankValid(false);
+
+            DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values..."));
+            setXGyroOffsetTC(xgOffset);
+            setYGyroOffsetTC(ygOffset);
+            setZGyroOffsetTC(zgOffset);
+
+            //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
+            //setXGyroOffset(0);
+            //setYGyroOffset(0);
+            //setZGyroOffset(0);
+
+            DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)..."));
+            uint8_t dmpUpdate[16], j;
+            uint16_t pos = 0;
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            resetFIFO();
+
+            DEBUG_PRINTLN(F("Reading FIFO count..."));
+            uint8_t fifoCount = getFIFOCount();
+
+            DEBUG_PRINT(F("Current FIFO count="));
+            DEBUG_PRINTLN(fifoCount);
+            uint8_t fifoBuffer[128];
+            //getFIFOBytes(fifoBuffer, fifoCount);
+
+            DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Disabling all standby flags..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00);
+
+            DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00);
+
+            DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
+            setMotionDetectionThreshold(2);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
+            setZeroMotionDetectionThreshold(156);
+
+            DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
+            setMotionDetectionDuration(80);
+
+            DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
+            setZeroMotionDetectionDuration(0);
+
+            DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
+            //mag -> setMode(1);
+            I2Cdev::writeByte(0x0E, 0x0A, 0x01);
+
+            // setup AK8975 (0x0E) as Slave 0 in read mode
+            DEBUG_PRINTLN(F("Setting up AK8975 read slave 0..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG,  0x01);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);
+
+            // setup AK8975 (0x0E) as Slave 2 in write mode
+            DEBUG_PRINTLN(F("Setting up AK8975 write slave 2..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG,  0x0A);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO,   0x01);
+
+            // setup I2C timing/delay control
+            DEBUG_PRINTLN(F("Setting up slave access delay..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
+            I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
+
+            // enable interrupts
+            DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00);
+
+            // enable I2C master mode and reset DMP/FIFO
+            DEBUG_PRINTLN(F("Enabling I2C master mode..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
+            DEBUG_PRINTLN(F("Resetting FIFO..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24);
+            DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know"));
+            I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
+            DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO..."));
+            I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8);
+
+            DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            
+            DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            #ifdef DEBUG
+                DEBUG_PRINT(F("Read bytes: "));
+                for (j = 0; j < 4; j++) {
+                    DEBUG_PRINTF(dmpUpdate[3 + j], HEX);
+                    DEBUG_PRINT(" ");
+                }
+                DEBUG_PRINTLN("");
+            #endif
+
+            DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+            DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 46..."));
+            while ((fifoCount = getFIFOCount()) < 46);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+
+            DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
+            while ((fifoCount = getFIFOCount()) < 48);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+            DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
+            while ((fifoCount = getFIFOCount()) < 48);
+            DEBUG_PRINTLN(F("Reading FIFO..."));
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
+            DEBUG_PRINTLN(F("Reading interrupt status..."));
+            getIntStatus();
+
+            DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)..."));
+            for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
+            writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
+
+            DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
+            setDMPEnabled(false);
+
+            DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer..."));
+            dmpPacketSize = 48;
+            /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
+                return 3; // TODO: proper error code for no memory
+            }*/
+
+            DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
+            resetFIFO();
+            getIntStatus();
+        } else {
+            DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
+            return 2; // configuration block loading failed
+        }
+    } else {
+        DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
+        return 1; // main binary block loading failed
+    }
+    return 0; // success
+}
+
+bool MPU6050::dmpPacketAvailable() {
+    return getFIFOCount() >= dmpGetFIFOPacketSize();
+}
+
+// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
+// uint8_t MPU6050::dmpGetFIFORate();
+// uint8_t MPU6050::dmpGetSampleStepSizeMS();
+// uint8_t MPU6050::dmpGetSampleFrequency();
+// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
+
+//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
+//uint8_t MPU6050::dmpRunFIFORateProcesses();
+
+// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]);
+    data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]);
+    data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[34] << 8) | packet[35];
+    data[1] = (packet[38] << 8) | packet[39];
+    data[2] = (packet[42] << 8) | packet[43];
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[34] << 8) | packet[35];
+    v -> y = (packet[38] << 8) | packet[39];
+    v -> z = (packet[42] << 8) | packet[43];
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
+    data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
+    data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
+    data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 8) | packet[1]);
+    data[1] = ((packet[4] << 8) | packet[5]);
+    data[2] = ((packet[8] << 8) | packet[9]);
+    data[3] = ((packet[12] << 8) | packet[13]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    if (status == 0) {
+        q -> w = (float)qI[0] / 16384.0f;
+        q -> x = (float)qI[1] / 16384.0f;
+        q -> y = (float)qI[2] / 16384.0f;
+        q -> z = (float)qI[3] / 16384.0f;
+        return 0;
+    }
+    return status; // int16 return value, indicates error if this line is reached
+}
+// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
+    data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
+    data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[16] << 8) | packet[17];
+    data[1] = (packet[20] << 8) | packet[21];
+    data[2] = (packet[24] << 8) | packet[25];
+    return 0;
+}
+uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[28] << 8) | packet[29];
+    data[1] = (packet[30] << 8) | packet[31];
+    data[2] = (packet[32] << 8) | packet[33];
+    return 0;
+}
+// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
+// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
+    // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
+    v -> x = vRaw -> x - gravity -> x*4096;
+    v -> y = vRaw -> y - gravity -> y*4096;
+    v -> z = vRaw -> z - gravity -> z*4096;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
+    // rotate measured 3D acceleration vector into original state
+    // frame of reference based on orientation quaternion
+    memcpy(v, vReal, sizeof(VectorInt16));
+    v -> rotate(q);
+    return 0;
+}
+// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
+    return status;
+}
+
+uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
+    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
+    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
+    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
+    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
+    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
+    return 0;
+}
+uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
+    return 0;
+}
+
+// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
+    /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
+        if (dmpData[k] < 0x10) Serial.print("0");
+        Serial.print(dmpData[k], HEX);
+        Serial.print(" ");
+    }
+    Serial.print("\n");*/
+    //Serial.println((uint16_t)dmpPacketBuffer);
+    return 0;
+}
+uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
+    uint8_t status;
+    uint8_t buf[dmpPacketSize];
+    for (uint8_t i = 0; i < numPackets; i++) {
+        // read packet from FIFO
+        getFIFOBytes(buf, dmpPacketSize);
+
+        // process packet
+        if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
+        
+        // increment external process count variable, if supplied
+        if (processed != 0) *processed++;
+    }
+    return 0;
+}
+
+// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
+
+// uint8_t MPU6050::dmpInitFIFOParam();
+// uint8_t MPU6050::dmpCloseFIFO();
+// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
+// uint8_t MPU6050::dmpDecodeQuantizedAccel();
+// uint32_t MPU6050::dmpGetGyroSumOfSquare();
+// uint32_t MPU6050::dmpGetAccelSumOfSquare();
+// void MPU6050::dmpOverrideQuaternion(long *q);
+uint16_t MPU6050::dmpGetFIFOPacketSize() {
+    return dmpPacketSize;
+}
+
+#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */
diff --git a/ESP32_ESP-IDF/components/MPU6050/component.mk b/ESP32_ESP-IDF/components/MPU6050/component.mk
new file mode 100644
index 00000000..3d61395f
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/component.mk
@@ -0,0 +1,10 @@
+#
+# Main component makefile.
+#
+# This Makefile can be left empty. By default, it will take the sources in the 
+# src/ directory, compile them and link them into lib(subdirectory_name).a 
+# in the build directory. This behaviour is entirely configurable,
+# please read the ESP-IDF documents if you need to do this.
+#
+
+COMPONENT_ADD_INCLUDEDIRS=.
\ No newline at end of file
diff --git a/ESP32_ESP-IDF/components/MPU6050/helper_3dmath.h b/ESP32_ESP-IDF/components/MPU6050/helper_3dmath.h
new file mode 100644
index 00000000..541f5b95
--- /dev/null
+++ b/ESP32_ESP-IDF/components/MPU6050/helper_3dmath.h
@@ -0,0 +1,218 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg 
+// 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 
+#include 
+
+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/ESP32_ESP-IDF/main/CMakeLists.txt b/ESP32_ESP-IDF/main/CMakeLists.txt
new file mode 100644
index 00000000..b3f17500
--- /dev/null
+++ b/ESP32_ESP-IDF/main/CMakeLists.txt
@@ -0,0 +1,4 @@
+set(COMPONENT_SRCS "main.cpp example.cpp")
+set(COMPONENT_ADD_INCLUDEDIRS ".")
+
+register_component()
diff --git a/ESP32_ESP-IDF/main/component.mk b/ESP32_ESP-IDF/main/component.mk
new file mode 100644
index 00000000..61f8990c
--- /dev/null
+++ b/ESP32_ESP-IDF/main/component.mk
@@ -0,0 +1,8 @@
+#
+# Main component makefile.
+#
+# This Makefile can be left empty. By default, it will take the sources in the 
+# src/ directory, compile them and link them into lib(subdirectory_name).a 
+# in the build directory. This behaviour is entirely configurable,
+# please read the ESP-IDF documents if you need to do this.
+#
diff --git a/ESP32_ESP-IDF/main/example.cpp b/ESP32_ESP-IDF/main/example.cpp
new file mode 100644
index 00000000..1724fa06
--- /dev/null
+++ b/ESP32_ESP-IDF/main/example.cpp
@@ -0,0 +1,87 @@
+/*
+ * Display.c
+ *
+ *  Created on: 14.08.2017
+ *      Author: darek
+ */
+#include 
+#include 
+#include 
+#include 
+#include 
+#include "MPU6050.h"
+#include "MPU6050_6Axis_MotionApps20.h"
+#include "sdkconfig.h"
+
+#define PIN_SDA 21
+#define PIN_CLK 22
+
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+uint16_t packetSize = 42;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+
+void task_initI2C(void *ignore) {
+	i2c_config_t conf;
+	conf.mode = I2C_MODE_MASTER;
+	conf.sda_io_num = (gpio_num_t)PIN_SDA;
+	conf.scl_io_num = (gpio_num_t)PIN_CLK;
+	conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
+	conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
+	conf.master.clk_speed = 400000;
+	ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf));
+	ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0));
+	vTaskDelete(NULL);
+}
+
+void task_display(void*){
+	MPU6050 mpu = MPU6050();
+	mpu.initialize();
+	mpu.dmpInitialize();
+
+	// This need to be setup individually
+	// mpu.setXGyroOffset(220);
+	// mpu.setYGyroOffset(76);
+	// mpu.setZGyroOffset(-85);
+	// mpu.setZAccelOffset(1788);
+    mpu.CalibrateAccel(6);
+    mpu.CalibrateGyro(6);
+
+	mpu.setDMPEnabled(true);
+
+	while(1){
+	    mpuIntStatus = mpu.getIntStatus();
+		// get current FIFO count
+		fifoCount = mpu.getFIFOCount();
+
+	    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+	        // reset so we can continue cleanly
+	        mpu.resetFIFO();
+
+	    // otherwise, check for DMP data ready interrupt frequently)
+	    } else if (mpuIntStatus & 0x02) {
+	        // wait for correct available data length, should be a VERY short wait
+	        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+	        // read a packet from FIFO
+
+	        mpu.getFIFOBytes(fifoBuffer, packetSize);
+	 		mpu.dmpGetQuaternion(&q, fifoBuffer);
+			mpu.dmpGetGravity(&gravity, &q);
+			mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+			printf("YAW: %3.1f, ", ypr[0] * 180/M_PI);
+			printf("PITCH: %3.1f, ", ypr[1] * 180/M_PI);
+			printf("ROLL: %3.1f \n", ypr[2] * 180/M_PI);
+	    }
+
+	    //Best result is to match with DMP refresh rate
+	    // Its last value in components/MPU6050/MPU6050_6Axis_MotionApps20.h file line 310
+	    // Now its 0x13, which means DMP is refreshed with 10Hz rate
+		// vTaskDelay(5/portTICK_PERIOD_MS);
+	}
+
+	vTaskDelete(NULL);
+}
diff --git a/ESP32_ESP-IDF/main/main.cpp b/ESP32_ESP-IDF/main/main.cpp
new file mode 100644
index 00000000..9714a3d8
--- /dev/null
+++ b/ESP32_ESP-IDF/main/main.cpp
@@ -0,0 +1,17 @@
+#include "freertos/FreeRTOS.h"
+#include "sdkconfig.h"
+#include "freertos/task.h"
+
+extern "C" {
+	void app_main(void);
+}
+
+extern void task_initI2C(void*);
+extern void task_display(void*);
+
+void app_main(void)
+{
+    xTaskCreate(&task_initI2C, "mpu_task", 2048, NULL, 5, NULL);
+    vTaskDelay(500/portTICK_PERIOD_MS);
+    xTaskCreate(&task_display, "disp_task", 8192, NULL, 5, NULL);
+}
diff --git a/ESP32_ESP-IDF/sdkconfig b/ESP32_ESP-IDF/sdkconfig
new file mode 100644
index 00000000..12586c85
--- /dev/null
+++ b/ESP32_ESP-IDF/sdkconfig
@@ -0,0 +1,1375 @@
+#
+# Automatically generated file. DO NOT EDIT.
+# Espressif IoT Development Framework (ESP-IDF) Project Configuration
+#
+CONFIG_IDF_CMAKE=y
+CONFIG_IDF_TARGET_ARCH_XTENSA=y
+CONFIG_IDF_TARGET="esp32"
+CONFIG_IDF_TARGET_ESP32=y
+CONFIG_IDF_FIRMWARE_CHIP_ID=0x0000
+
+#
+# SDK tool configuration
+#
+CONFIG_SDK_TOOLPREFIX="xtensa-esp32-elf-"
+# CONFIG_SDK_TOOLCHAIN_SUPPORTS_TIME_WIDE_64_BITS is not set
+# end of SDK tool configuration
+
+#
+# Build type
+#
+CONFIG_APP_BUILD_TYPE_APP_2NDBOOT=y
+# CONFIG_APP_BUILD_TYPE_ELF_RAM is not set
+CONFIG_APP_BUILD_GENERATE_BINARIES=y
+CONFIG_APP_BUILD_BOOTLOADER=y
+CONFIG_APP_BUILD_USE_FLASH_SECTIONS=y
+# end of Build type
+
+#
+# Application manager
+#
+CONFIG_APP_COMPILE_TIME_DATE=y
+# CONFIG_APP_EXCLUDE_PROJECT_VER_VAR is not set
+# CONFIG_APP_EXCLUDE_PROJECT_NAME_VAR is not set
+# CONFIG_APP_PROJECT_VER_FROM_CONFIG is not set
+CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16
+# end of Application manager
+
+#
+# Bootloader config
+#
+CONFIG_BOOTLOADER_OFFSET_IN_FLASH=0x1000
+CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_SIZE=y
+# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_DEBUG is not set
+# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_PERF is not set
+# CONFIG_BOOTLOADER_COMPILER_OPTIMIZATION_NONE is not set
+# CONFIG_BOOTLOADER_LOG_LEVEL_NONE is not set
+# CONFIG_BOOTLOADER_LOG_LEVEL_ERROR is not set
+# CONFIG_BOOTLOADER_LOG_LEVEL_WARN is not set
+# CONFIG_BOOTLOADER_LOG_LEVEL_INFO is not set
+CONFIG_BOOTLOADER_LOG_LEVEL_DEBUG=y
+# CONFIG_BOOTLOADER_LOG_LEVEL_VERBOSE is not set
+CONFIG_BOOTLOADER_LOG_LEVEL=4
+# CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_8V is not set
+CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y
+# CONFIG_BOOTLOADER_FACTORY_RESET is not set
+# CONFIG_BOOTLOADER_APP_TEST is not set
+CONFIG_BOOTLOADER_WDT_ENABLE=y
+# CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE is not set
+CONFIG_BOOTLOADER_WDT_TIME_MS=9000
+# CONFIG_BOOTLOADER_APP_ROLLBACK_ENABLE is not set
+# CONFIG_BOOTLOADER_SKIP_VALIDATE_IN_DEEP_SLEEP is not set
+# CONFIG_BOOTLOADER_SKIP_VALIDATE_ON_POWER_ON is not set
+# CONFIG_BOOTLOADER_SKIP_VALIDATE_ALWAYS is not set
+CONFIG_BOOTLOADER_RESERVE_RTC_SIZE=0
+# CONFIG_BOOTLOADER_CUSTOM_RESERVE_RTC is not set
+CONFIG_BOOTLOADER_FLASH_XMC_SUPPORT=y
+# end of Bootloader config
+
+#
+# Security features
+#
+# CONFIG_SECURE_SIGNED_APPS_NO_SECURE_BOOT is not set
+# CONFIG_SECURE_BOOT is not set
+# CONFIG_SECURE_FLASH_ENC_ENABLED is not set
+# end of Security features
+
+#
+# Serial flasher config
+#
+CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200
+# CONFIG_ESPTOOLPY_NO_STUB is not set
+# CONFIG_ESPTOOLPY_FLASHMODE_QIO is not set
+# CONFIG_ESPTOOLPY_FLASHMODE_QOUT is not set
+CONFIG_ESPTOOLPY_FLASHMODE_DIO=y
+# CONFIG_ESPTOOLPY_FLASHMODE_DOUT is not set
+CONFIG_ESPTOOLPY_FLASH_SAMPLE_MODE_STR=y
+CONFIG_ESPTOOLPY_FLASHMODE="dio"
+# CONFIG_ESPTOOLPY_FLASHFREQ_80M is not set
+CONFIG_ESPTOOLPY_FLASHFREQ_40M=y
+# CONFIG_ESPTOOLPY_FLASHFREQ_26M is not set
+# CONFIG_ESPTOOLPY_FLASHFREQ_20M is not set
+CONFIG_ESPTOOLPY_FLASHFREQ="40m"
+# CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set
+# CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
+# CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set
+# CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set
+CONFIG_ESPTOOLPY_FLASHSIZE="4MB"
+CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y
+CONFIG_ESPTOOLPY_BEFORE_RESET=y
+# CONFIG_ESPTOOLPY_BEFORE_NORESET is not set
+CONFIG_ESPTOOLPY_BEFORE="default_reset"
+CONFIG_ESPTOOLPY_AFTER_RESET=y
+# CONFIG_ESPTOOLPY_AFTER_NORESET is not set
+CONFIG_ESPTOOLPY_AFTER="hard_reset"
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_CONSOLE is not set
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_9600B is not set
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_57600B is not set
+CONFIG_ESPTOOLPY_MONITOR_BAUD_115200B=y
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_230400B is not set
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_921600B is not set
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_2MB is not set
+# CONFIG_ESPTOOLPY_MONITOR_BAUD_OTHER is not set
+CONFIG_ESPTOOLPY_MONITOR_BAUD_OTHER_VAL=115200
+CONFIG_ESPTOOLPY_MONITOR_BAUD=115200
+# end of Serial flasher config
+
+#
+# Partition Table
+#
+CONFIG_PARTITION_TABLE_SINGLE_APP=y
+# CONFIG_PARTITION_TABLE_SINGLE_APP_LARGE is not set
+# CONFIG_PARTITION_TABLE_TWO_OTA is not set
+# CONFIG_PARTITION_TABLE_CUSTOM is not set
+CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
+CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp.csv"
+CONFIG_PARTITION_TABLE_OFFSET=0x8000
+CONFIG_PARTITION_TABLE_MD5=y
+# end of Partition Table
+
+#
+# Compiler options
+#
+CONFIG_COMPILER_OPTIMIZATION_DEFAULT=y
+# CONFIG_COMPILER_OPTIMIZATION_SIZE is not set
+# CONFIG_COMPILER_OPTIMIZATION_PERF is not set
+# CONFIG_COMPILER_OPTIMIZATION_NONE is not set
+CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_ENABLE=y
+# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_SILENT is not set
+# CONFIG_COMPILER_OPTIMIZATION_ASSERTIONS_DISABLE is not set
+CONFIG_COMPILER_OPTIMIZATION_ASSERTION_LEVEL=2
+# CONFIG_COMPILER_OPTIMIZATION_CHECKS_SILENT is not set
+CONFIG_COMPILER_HIDE_PATHS_MACROS=y
+# CONFIG_COMPILER_CXX_EXCEPTIONS is not set
+# CONFIG_COMPILER_CXX_RTTI is not set
+CONFIG_COMPILER_STACK_CHECK_MODE_NONE=y
+# CONFIG_COMPILER_STACK_CHECK_MODE_NORM is not set
+# CONFIG_COMPILER_STACK_CHECK_MODE_STRONG is not set
+# CONFIG_COMPILER_STACK_CHECK_MODE_ALL is not set
+# CONFIG_COMPILER_WARN_WRITE_STRINGS is not set
+# CONFIG_COMPILER_DISABLE_GCC8_WARNINGS is not set
+# CONFIG_COMPILER_DUMP_RTL_FILES is not set
+# end of Compiler options
+
+#
+# Component config
+#
+
+#
+# Application Level Tracing
+#
+# CONFIG_APPTRACE_DEST_JTAG is not set
+CONFIG_APPTRACE_DEST_NONE=y
+CONFIG_APPTRACE_LOCK_ENABLE=y
+# end of Application Level Tracing
+
+#
+# ESP-ASIO
+#
+# CONFIG_ASIO_SSL_SUPPORT is not set
+# end of ESP-ASIO
+
+#
+# Bluetooth
+#
+# CONFIG_BT_ENABLED is not set
+# end of Bluetooth
+
+#
+# CoAP Configuration
+#
+CONFIG_COAP_MBEDTLS_PSK=y
+# CONFIG_COAP_MBEDTLS_PKI is not set
+# CONFIG_COAP_MBEDTLS_DEBUG is not set
+CONFIG_COAP_LOG_DEFAULT_LEVEL=0
+# end of CoAP Configuration
+
+#
+# Driver configurations
+#
+
+#
+# ADC configuration
+#
+# CONFIG_ADC_FORCE_XPD_FSM is not set
+CONFIG_ADC_DISABLE_DAC=y
+# end of ADC configuration
+
+#
+# MCPWM configuration
+#
+# CONFIG_MCPWM_ISR_IN_IRAM is not set
+# end of MCPWM configuration
+
+#
+# SPI configuration
+#
+# CONFIG_SPI_MASTER_IN_IRAM is not set
+CONFIG_SPI_MASTER_ISR_IN_IRAM=y
+# CONFIG_SPI_SLAVE_IN_IRAM is not set
+CONFIG_SPI_SLAVE_ISR_IN_IRAM=y
+# end of SPI configuration
+
+#
+# TWAI configuration
+#
+# CONFIG_TWAI_ISR_IN_IRAM is not set
+# CONFIG_TWAI_ERRATA_FIX_BUS_OFF_REC is not set
+# CONFIG_TWAI_ERRATA_FIX_TX_INTR_LOST is not set
+# CONFIG_TWAI_ERRATA_FIX_RX_FRAME_INVALID is not set
+# CONFIG_TWAI_ERRATA_FIX_RX_FIFO_CORRUPT is not set
+# end of TWAI configuration
+
+#
+# UART configuration
+#
+# CONFIG_UART_ISR_IN_IRAM is not set
+# end of UART configuration
+
+#
+# RTCIO configuration
+#
+# CONFIG_RTCIO_SUPPORT_RTC_GPIO_DESC is not set
+# end of RTCIO configuration
+
+#
+# GPIO Configuration
+#
+# CONFIG_GPIO_ESP32_SUPPORT_SWITCH_SLP_PULL is not set
+# end of GPIO Configuration
+
+#
+# GDMA Configuration
+#
+# CONFIG_GDMA_CTRL_FUNC_IN_IRAM is not set
+# CONFIG_GDMA_ISR_IRAM_SAFE is not set
+# end of GDMA Configuration
+# end of Driver configurations
+
+#
+# eFuse Bit Manager
+#
+# CONFIG_EFUSE_CUSTOM_TABLE is not set
+# CONFIG_EFUSE_VIRTUAL is not set
+# CONFIG_EFUSE_CODE_SCHEME_COMPAT_NONE is not set
+CONFIG_EFUSE_CODE_SCHEME_COMPAT_3_4=y
+# CONFIG_EFUSE_CODE_SCHEME_COMPAT_REPEAT is not set
+CONFIG_EFUSE_MAX_BLK_LEN=192
+# end of eFuse Bit Manager
+
+#
+# ESP-TLS
+#
+CONFIG_ESP_TLS_USING_MBEDTLS=y
+# CONFIG_ESP_TLS_USE_SECURE_ELEMENT is not set
+# CONFIG_ESP_TLS_SERVER is not set
+# CONFIG_ESP_TLS_CLIENT_SESSION_TICKETS is not set
+# CONFIG_ESP_TLS_PSK_VERIFICATION is not set
+# CONFIG_ESP_TLS_INSECURE is not set
+# end of ESP-TLS
+
+#
+# ESP32-specific
+#
+CONFIG_ESP32_REV_MIN_0=y
+# CONFIG_ESP32_REV_MIN_1 is not set
+# CONFIG_ESP32_REV_MIN_2 is not set
+# CONFIG_ESP32_REV_MIN_3 is not set
+CONFIG_ESP32_REV_MIN=0
+# CONFIG_ESP32_DEFAULT_CPU_FREQ_80 is not set
+# CONFIG_ESP32_DEFAULT_CPU_FREQ_160 is not set
+CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y
+CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240
+# CONFIG_ESP32_SPIRAM_SUPPORT is not set
+# CONFIG_ESP32_TRAX is not set
+CONFIG_ESP32_TRACEMEM_RESERVE_DRAM=0x0
+# CONFIG_ESP32_ULP_COPROC_ENABLED is not set
+CONFIG_ESP32_ULP_COPROC_RESERVE_MEM=0
+CONFIG_ESP32_DEBUG_OCDAWARE=y
+CONFIG_ESP32_BROWNOUT_DET=y
+CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_0=y
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_1 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_2 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_3 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_4 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_5 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_6 is not set
+# CONFIG_ESP32_BROWNOUT_DET_LVL_SEL_7 is not set
+CONFIG_ESP32_BROWNOUT_DET_LVL=0
+CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y
+# CONFIG_ESP32_TIME_SYSCALL_USE_RTC is not set
+# CONFIG_ESP32_TIME_SYSCALL_USE_FRC1 is not set
+# CONFIG_ESP32_TIME_SYSCALL_USE_NONE is not set
+CONFIG_ESP32_RTC_CLK_SRC_INT_RC=y
+# CONFIG_ESP32_RTC_CLK_SRC_EXT_CRYS is not set
+# CONFIG_ESP32_RTC_CLK_SRC_EXT_OSC is not set
+# CONFIG_ESP32_RTC_CLK_SRC_INT_8MD256 is not set
+CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024
+CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000
+# CONFIG_ESP32_XTAL_FREQ_40 is not set
+# CONFIG_ESP32_XTAL_FREQ_26 is not set
+CONFIG_ESP32_XTAL_FREQ_AUTO=y
+CONFIG_ESP32_XTAL_FREQ=0
+# CONFIG_ESP32_DISABLE_BASIC_ROM_CONSOLE is not set
+# CONFIG_ESP32_NO_BLOBS is not set
+# CONFIG_ESP32_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set
+# CONFIG_ESP32_COMPATIBLE_PRE_V3_1_BOOTLOADERS is not set
+# CONFIG_ESP32_RTCDATA_IN_FAST_MEM is not set
+# CONFIG_ESP32_USE_FIXED_STATIC_RAM_SIZE is not set
+CONFIG_ESP32_DPORT_DIS_INTERRUPT_LVL=5
+# CONFIG_ESP32_IRAM_AS_8BIT_ACCESSIBLE_MEMORY is not set
+# end of ESP32-specific
+
+#
+# ADC-Calibration
+#
+CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y
+CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y
+CONFIG_ADC_CAL_LUT_ENABLE=y
+# end of ADC-Calibration
+
+#
+# Common ESP-related
+#
+CONFIG_ESP_ERR_TO_NAME_LOOKUP=y
+# end of Common ESP-related
+
+#
+# Ethernet
+#
+CONFIG_ETH_ENABLED=y
+CONFIG_ETH_USE_ESP32_EMAC=y
+CONFIG_ETH_PHY_INTERFACE_RMII=y
+CONFIG_ETH_RMII_CLK_INPUT=y
+# CONFIG_ETH_RMII_CLK_OUTPUT is not set
+CONFIG_ETH_RMII_CLK_IN_GPIO=0
+CONFIG_ETH_DMA_BUFFER_SIZE=512
+CONFIG_ETH_DMA_RX_BUFFER_NUM=10
+CONFIG_ETH_DMA_TX_BUFFER_NUM=10
+CONFIG_ETH_USE_SPI_ETHERNET=y
+# CONFIG_ETH_SPI_ETHERNET_DM9051 is not set
+# CONFIG_ETH_SPI_ETHERNET_W5500 is not set
+# CONFIG_ETH_SPI_ETHERNET_KSZ8851SNL is not set
+# CONFIG_ETH_USE_OPENETH is not set
+# end of Ethernet
+
+#
+# Event Loop Library
+#
+# CONFIG_ESP_EVENT_LOOP_PROFILING is not set
+CONFIG_ESP_EVENT_POST_FROM_ISR=y
+CONFIG_ESP_EVENT_POST_FROM_IRAM_ISR=y
+# end of Event Loop Library
+
+#
+# GDB Stub
+#
+CONFIG_ESP_GDBSTUB_ENABLED=y
+CONFIG_ESP_GDBSTUB_SUPPORT_TASKS=y
+CONFIG_ESP_GDBSTUB_MAX_TASKS=32
+# end of GDB Stub
+
+#
+# ESP HTTP client
+#
+CONFIG_ESP_HTTP_CLIENT_ENABLE_HTTPS=y
+# CONFIG_ESP_HTTP_CLIENT_ENABLE_BASIC_AUTH is not set
+CONFIG_ESP_HTTP_CLIENT_ENABLE_DIGEST_AUTH=y
+# end of ESP HTTP client
+
+#
+# HTTP Server
+#
+CONFIG_HTTPD_MAX_REQ_HDR_LEN=512
+CONFIG_HTTPD_MAX_URI_LEN=512
+CONFIG_HTTPD_ERR_RESP_NO_DELAY=y
+CONFIG_HTTPD_PURGE_BUF_LEN=32
+# CONFIG_HTTPD_LOG_PURGE_DATA is not set
+# CONFIG_HTTPD_WS_SUPPORT is not set
+# end of HTTP Server
+
+#
+# ESP HTTPS OTA
+#
+# CONFIG_OTA_ALLOW_HTTP is not set
+# end of ESP HTTPS OTA
+
+#
+# ESP HTTPS server
+#
+# CONFIG_ESP_HTTPS_SERVER_ENABLE is not set
+# end of ESP HTTPS server
+
+#
+# Hardware Settings
+#
+
+#
+# MAC Config
+#
+CONFIG_ESP_MAC_ADDR_UNIVERSE_WIFI_STA=y
+CONFIG_ESP_MAC_ADDR_UNIVERSE_WIFI_AP=y
+CONFIG_ESP_MAC_ADDR_UNIVERSE_BT=y
+CONFIG_ESP_MAC_ADDR_UNIVERSE_ETH=y
+# CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_TWO is not set
+CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES_FOUR=y
+CONFIG_ESP32_UNIVERSAL_MAC_ADDRESSES=4
+# end of MAC Config
+
+#
+# Sleep Config
+#
+CONFIG_ESP_SLEEP_POWER_DOWN_FLASH=y
+CONFIG_ESP_SLEEP_RTC_BUS_ISO_WORKAROUND=y
+# CONFIG_ESP_SLEEP_GPIO_RESET_WORKAROUND is not set
+# CONFIG_ESP_SLEEP_FLASH_LEAKAGE_WORKAROUND is not set
+# end of Sleep Config
+
+#
+# RTC Clock Config
+#
+# end of RTC Clock Config
+# end of Hardware Settings
+
+#
+# IPC (Inter-Processor Call)
+#
+CONFIG_ESP_IPC_TASK_STACK_SIZE=1024
+# end of IPC (Inter-Processor Call)
+
+#
+# LCD and Touch Panel
+#
+
+#
+# LCD Peripheral Configuration
+#
+CONFIG_LCD_PANEL_IO_FORMAT_BUF_SIZE=32
+# end of LCD Peripheral Configuration
+# end of LCD and Touch Panel
+
+#
+# ESP NETIF Adapter
+#
+CONFIG_ESP_NETIF_IP_LOST_TIMER_INTERVAL=120
+CONFIG_ESP_NETIF_TCPIP_LWIP=y
+# CONFIG_ESP_NETIF_LOOPBACK is not set
+CONFIG_ESP_NETIF_TCPIP_ADAPTER_COMPATIBLE_LAYER=y
+# end of ESP NETIF Adapter
+
+#
+# PHY
+#
+CONFIG_ESP_PHY_CALIBRATION_AND_DATA_STORAGE=y
+# CONFIG_ESP_PHY_INIT_DATA_IN_PARTITION is not set
+CONFIG_ESP_PHY_MAX_WIFI_TX_POWER=20
+CONFIG_ESP_PHY_MAX_TX_POWER=20
+CONFIG_ESP_PHY_REDUCE_TX_POWER=y
+# end of PHY
+
+#
+# Power Management
+#
+# CONFIG_PM_ENABLE is not set
+# end of Power Management
+
+#
+# ESP System Settings
+#
+# CONFIG_ESP_SYSTEM_PANIC_PRINT_HALT is not set
+# CONFIG_ESP_SYSTEM_PANIC_PRINT_REBOOT is not set
+# CONFIG_ESP_SYSTEM_PANIC_SILENT_REBOOT is not set
+CONFIG_ESP_SYSTEM_PANIC_GDBSTUB=y
+# CONFIG_ESP_SYSTEM_GDBSTUB_RUNTIME is not set
+CONFIG_ESP_SYSTEM_SINGLE_CORE_MODE=y
+CONFIG_ESP_SYSTEM_RTC_FAST_MEM_AS_HEAP_DEPCHECK=y
+CONFIG_ESP_SYSTEM_ALLOW_RTC_FAST_MEM_AS_HEAP=y
+
+#
+# Memory protection
+#
+# end of Memory protection
+
+CONFIG_ESP_SYSTEM_EVENT_QUEUE_SIZE=32
+CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=4096
+CONFIG_ESP_MAIN_TASK_AFFINITY_CPU0=y
+# CONFIG_ESP_MAIN_TASK_AFFINITY_NO_AFFINITY is not set
+CONFIG_ESP_MAIN_TASK_AFFINITY=0x0
+CONFIG_ESP_MINIMAL_SHARED_STACK_SIZE=2048
+CONFIG_ESP_CONSOLE_UART_DEFAULT=y
+# CONFIG_ESP_CONSOLE_UART_CUSTOM is not set
+# CONFIG_ESP_CONSOLE_NONE is not set
+CONFIG_ESP_CONSOLE_UART=y
+CONFIG_ESP_CONSOLE_MULTIPLE_UART=y
+CONFIG_ESP_CONSOLE_UART_NUM=0
+CONFIG_ESP_CONSOLE_UART_BAUDRATE=115200
+CONFIG_ESP_INT_WDT=y
+CONFIG_ESP_INT_WDT_TIMEOUT_MS=300
+CONFIG_ESP_TASK_WDT=y
+# CONFIG_ESP_TASK_WDT_PANIC is not set
+CONFIG_ESP_TASK_WDT_TIMEOUT_S=5
+CONFIG_ESP_TASK_WDT_CHECK_IDLE_TASK_CPU0=y
+# CONFIG_ESP_PANIC_HANDLER_IRAM is not set
+# CONFIG_ESP_DEBUG_STUBS_ENABLE is not set
+# CONFIG_ESP_SYSTEM_CHECK_INT_LEVEL_5 is not set
+CONFIG_ESP_SYSTEM_CHECK_INT_LEVEL_4=y
+# end of ESP System Settings
+
+#
+# High resolution timer (esp_timer)
+#
+# CONFIG_ESP_TIMER_PROFILING is not set
+CONFIG_ESP_TIME_FUNCS_USE_RTC_TIMER=y
+CONFIG_ESP_TIME_FUNCS_USE_ESP_TIMER=y
+CONFIG_ESP_TIMER_TASK_STACK_SIZE=3584
+CONFIG_ESP_TIMER_INTERRUPT_LEVEL=1
+# CONFIG_ESP_TIMER_SUPPORTS_ISR_DISPATCH_METHOD is not set
+# CONFIG_ESP_TIMER_IMPL_FRC2 is not set
+CONFIG_ESP_TIMER_IMPL_TG0_LAC=y
+# end of High resolution timer (esp_timer)
+
+#
+# Wi-Fi
+#
+CONFIG_ESP32_WIFI_ENABLED=y
+CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10
+CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32
+# CONFIG_ESP32_WIFI_STATIC_TX_BUFFER is not set
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y
+CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32
+# CONFIG_ESP32_WIFI_CSI_ENABLED is not set
+CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y
+CONFIG_ESP32_WIFI_TX_BA_WIN=6
+CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y
+CONFIG_ESP32_WIFI_RX_BA_WIN=6
+CONFIG_ESP32_WIFI_NVS_ENABLED=y
+CONFIG_ESP32_WIFI_SOFTAP_BEACON_MAX_LEN=752
+CONFIG_ESP32_WIFI_MGMT_SBUF_NUM=32
+CONFIG_ESP32_WIFI_IRAM_OPT=y
+CONFIG_ESP32_WIFI_RX_IRAM_OPT=y
+CONFIG_ESP32_WIFI_ENABLE_WPA3_SAE=y
+# CONFIG_ESP_WIFI_SLP_IRAM_OPT is not set
+# CONFIG_ESP_WIFI_STA_DISCONNECTED_PM_ENABLE is not set
+# CONFIG_ESP_WIFI_GMAC_SUPPORT is not set
+CONFIG_ESP_WIFI_SOFTAP_SUPPORT=y
+# end of Wi-Fi
+
+#
+# Core dump
+#
+# CONFIG_ESP_COREDUMP_ENABLE_TO_FLASH is not set
+# CONFIG_ESP_COREDUMP_ENABLE_TO_UART is not set
+CONFIG_ESP_COREDUMP_ENABLE_TO_NONE=y
+# end of Core dump
+
+#
+# FAT Filesystem support
+#
+# CONFIG_FATFS_CODEPAGE_DYNAMIC is not set
+CONFIG_FATFS_CODEPAGE_437=y
+# CONFIG_FATFS_CODEPAGE_720 is not set
+# CONFIG_FATFS_CODEPAGE_737 is not set
+# CONFIG_FATFS_CODEPAGE_771 is not set
+# CONFIG_FATFS_CODEPAGE_775 is not set
+# CONFIG_FATFS_CODEPAGE_850 is not set
+# CONFIG_FATFS_CODEPAGE_852 is not set
+# CONFIG_FATFS_CODEPAGE_855 is not set
+# CONFIG_FATFS_CODEPAGE_857 is not set
+# CONFIG_FATFS_CODEPAGE_860 is not set
+# CONFIG_FATFS_CODEPAGE_861 is not set
+# CONFIG_FATFS_CODEPAGE_862 is not set
+# CONFIG_FATFS_CODEPAGE_863 is not set
+# CONFIG_FATFS_CODEPAGE_864 is not set
+# CONFIG_FATFS_CODEPAGE_865 is not set
+# CONFIG_FATFS_CODEPAGE_866 is not set
+# CONFIG_FATFS_CODEPAGE_869 is not set
+# CONFIG_FATFS_CODEPAGE_932 is not set
+# CONFIG_FATFS_CODEPAGE_936 is not set
+# CONFIG_FATFS_CODEPAGE_949 is not set
+# CONFIG_FATFS_CODEPAGE_950 is not set
+CONFIG_FATFS_CODEPAGE=437
+CONFIG_FATFS_LFN_NONE=y
+# CONFIG_FATFS_LFN_HEAP is not set
+# CONFIG_FATFS_LFN_STACK is not set
+CONFIG_FATFS_FS_LOCK=0
+CONFIG_FATFS_TIMEOUT_MS=10000
+CONFIG_FATFS_PER_FILE_CACHE=y
+# CONFIG_FATFS_USE_FASTSEEK is not set
+# end of FAT Filesystem support
+
+#
+# Modbus configuration
+#
+CONFIG_FMB_COMM_MODE_TCP_EN=y
+CONFIG_FMB_TCP_PORT_DEFAULT=502
+CONFIG_FMB_TCP_PORT_MAX_CONN=5
+CONFIG_FMB_TCP_CONNECTION_TOUT_SEC=20
+CONFIG_FMB_COMM_MODE_RTU_EN=y
+CONFIG_FMB_COMM_MODE_ASCII_EN=y
+CONFIG_FMB_MASTER_TIMEOUT_MS_RESPOND=150
+CONFIG_FMB_MASTER_DELAY_MS_CONVERT=200
+CONFIG_FMB_QUEUE_LENGTH=20
+CONFIG_FMB_PORT_TASK_STACK_SIZE=4096
+CONFIG_FMB_SERIAL_BUF_SIZE=256
+CONFIG_FMB_SERIAL_ASCII_BITS_PER_SYMB=8
+CONFIG_FMB_SERIAL_ASCII_TIMEOUT_RESPOND_MS=1000
+CONFIG_FMB_PORT_TASK_PRIO=10
+CONFIG_FMB_PORT_TASK_AFFINITY=0x7FFFFFFF
+CONFIG_FMB_CONTROLLER_SLAVE_ID_SUPPORT=y
+CONFIG_FMB_CONTROLLER_SLAVE_ID=0x00112233
+CONFIG_FMB_CONTROLLER_NOTIFY_TIMEOUT=20
+CONFIG_FMB_CONTROLLER_NOTIFY_QUEUE_SIZE=20
+CONFIG_FMB_CONTROLLER_STACK_SIZE=4096
+CONFIG_FMB_EVENT_QUEUE_TIMEOUT=20
+# CONFIG_FMB_TIMER_PORT_ENABLED is not set
+CONFIG_FMB_TIMER_GROUP=0
+CONFIG_FMB_TIMER_INDEX=0
+CONFIG_FMB_MASTER_TIMER_GROUP=0
+CONFIG_FMB_MASTER_TIMER_INDEX=0
+# CONFIG_FMB_TIMER_ISR_IN_IRAM is not set
+# end of Modbus configuration
+
+#
+# FreeRTOS
+#
+CONFIG_FREERTOS_UNICORE=y
+CONFIG_FREERTOS_NO_AFFINITY=0x7FFFFFFF
+CONFIG_FREERTOS_TICK_SUPPORT_CORETIMER=y
+CONFIG_FREERTOS_CORETIMER_0=y
+# CONFIG_FREERTOS_CORETIMER_1 is not set
+CONFIG_FREERTOS_SYSTICK_USES_CCOUNT=y
+CONFIG_FREERTOS_OPTIMIZED_SCHEDULER=y
+CONFIG_FREERTOS_HZ=1000
+CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y
+# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set
+# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set
+CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y
+# CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK is not set
+CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y
+CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1
+# CONFIG_FREERTOS_ASSERT_FAIL_ABORT is not set
+CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE=y
+# CONFIG_FREERTOS_ASSERT_DISABLE is not set
+CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1024
+CONFIG_FREERTOS_ISR_STACKSIZE=1536
+# CONFIG_FREERTOS_LEGACY_HOOKS is not set
+CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16
+CONFIG_FREERTOS_SUPPORT_STATIC_ALLOCATION=y
+# CONFIG_FREERTOS_ENABLE_STATIC_TASK_CLEAN_UP is not set
+CONFIG_FREERTOS_TIMER_TASK_PRIORITY=1
+CONFIG_FREERTOS_TIMER_TASK_STACK_DEPTH=2048
+CONFIG_FREERTOS_TIMER_QUEUE_LENGTH=10
+CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0
+# CONFIG_FREERTOS_USE_TRACE_FACILITY is not set
+# CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS is not set
+CONFIG_FREERTOS_TASK_FUNCTION_WRAPPER=y
+CONFIG_FREERTOS_CHECK_MUTEX_GIVEN_BY_OWNER=y
+# CONFIG_FREERTOS_CHECK_PORT_CRITICAL_COMPLIANCE is not set
+# CONFIG_FREERTOS_PLACE_FUNCTIONS_INTO_FLASH is not set
+CONFIG_FREERTOS_DEBUG_OCDAWARE=y
+# CONFIG_FREERTOS_FPU_IN_ISR is not set
+CONFIG_FREERTOS_ENABLE_TASK_SNAPSHOT=y
+# CONFIG_FREERTOS_PLACE_SNAPSHOT_FUNS_INTO_FLASH is not set
+# end of FreeRTOS
+
+#
+# Hardware Abstraction Layer (HAL) and Low Level (LL)
+#
+CONFIG_HAL_ASSERTION_EQUALS_SYSTEM=y
+# CONFIG_HAL_ASSERTION_DISABLE is not set
+# CONFIG_HAL_ASSERTION_SILIENT is not set
+# CONFIG_HAL_ASSERTION_ENABLE is not set
+CONFIG_HAL_DEFAULT_ASSERTION_LEVEL=2
+# end of Hardware Abstraction Layer (HAL) and Low Level (LL)
+
+#
+# Heap memory debugging
+#
+CONFIG_HEAP_POISONING_DISABLED=y
+# CONFIG_HEAP_POISONING_LIGHT is not set
+# CONFIG_HEAP_POISONING_COMPREHENSIVE is not set
+CONFIG_HEAP_TRACING_OFF=y
+# CONFIG_HEAP_TRACING_STANDALONE is not set
+# CONFIG_HEAP_TRACING_TOHOST is not set
+# CONFIG_HEAP_ABORT_WHEN_ALLOCATION_FAILS is not set
+# end of Heap memory debugging
+
+#
+# jsmn
+#
+# CONFIG_JSMN_PARENT_LINKS is not set
+# CONFIG_JSMN_STRICT is not set
+# end of jsmn
+
+#
+# libsodium
+#
+# end of libsodium
+
+#
+# Log output
+#
+# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set
+# CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set
+# CONFIG_LOG_DEFAULT_LEVEL_WARN is not set
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+# CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set
+# CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set
+CONFIG_LOG_DEFAULT_LEVEL=3
+CONFIG_LOG_MAXIMUM_EQUALS_DEFAULT=y
+# CONFIG_LOG_MAXIMUM_LEVEL_DEBUG is not set
+# CONFIG_LOG_MAXIMUM_LEVEL_VERBOSE is not set
+CONFIG_LOG_MAXIMUM_LEVEL=3
+CONFIG_LOG_COLORS=y
+CONFIG_LOG_TIMESTAMP_SOURCE_RTOS=y
+# CONFIG_LOG_TIMESTAMP_SOURCE_SYSTEM is not set
+# end of Log output
+
+#
+# LWIP
+#
+CONFIG_LWIP_LOCAL_HOSTNAME="espressif"
+# CONFIG_LWIP_NETIF_API is not set
+# CONFIG_LWIP_TCPIP_CORE_LOCKING is not set
+CONFIG_LWIP_DNS_SUPPORT_MDNS_QUERIES=y
+# CONFIG_LWIP_L2_TO_L3_COPY is not set
+# CONFIG_LWIP_IRAM_OPTIMIZATION is not set
+CONFIG_LWIP_TIMERS_ONDEMAND=y
+CONFIG_LWIP_MAX_SOCKETS=10
+# CONFIG_LWIP_USE_ONLY_LWIP_SELECT is not set
+# CONFIG_LWIP_SO_LINGER is not set
+# CONFIG_LWIP_SO_REUSE is not set
+# CONFIG_LWIP_SO_RCVBUF is not set
+# CONFIG_LWIP_NETBUF_RECVINFO is not set
+CONFIG_LWIP_IP4_FRAG=y
+CONFIG_LWIP_IP6_FRAG=y
+# CONFIG_LWIP_IP4_REASSEMBLY is not set
+# CONFIG_LWIP_IP6_REASSEMBLY is not set
+# CONFIG_LWIP_IP_FORWARD is not set
+# CONFIG_LWIP_STATS is not set
+# CONFIG_LWIP_ETHARP_TRUST_IP_MAC is not set
+CONFIG_LWIP_ESP_GRATUITOUS_ARP=y
+CONFIG_LWIP_GARP_TMR_INTERVAL=60
+CONFIG_LWIP_TCPIP_RECVMBOX_SIZE=32
+CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y
+# CONFIG_LWIP_DHCP_DISABLE_CLIENT_ID is not set
+CONFIG_LWIP_DHCP_DISABLE_VENDOR_CLASS_ID=y
+# CONFIG_LWIP_DHCP_RESTORE_LAST_IP is not set
+CONFIG_LWIP_DHCP_OPTIONS_LEN=68
+
+#
+# DHCP server
+#
+CONFIG_LWIP_DHCPS=y
+CONFIG_LWIP_DHCPS_LEASE_UNIT=60
+CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8
+# end of DHCP server
+
+# CONFIG_LWIP_AUTOIP is not set
+CONFIG_LWIP_IPV6=y
+# CONFIG_LWIP_IPV6_AUTOCONFIG is not set
+CONFIG_LWIP_IPV6_NUM_ADDRESSES=3
+# CONFIG_LWIP_IPV6_FORWARD is not set
+# CONFIG_LWIP_NETIF_STATUS_CALLBACK is not set
+CONFIG_LWIP_NETIF_LOOPBACK=y
+CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8
+
+#
+# TCP
+#
+CONFIG_LWIP_MAX_ACTIVE_TCP=16
+CONFIG_LWIP_MAX_LISTENING_TCP=16
+CONFIG_LWIP_TCP_HIGH_SPEED_RETRANSMISSION=y
+CONFIG_LWIP_TCP_MAXRTX=12
+CONFIG_LWIP_TCP_SYNMAXRTX=6
+CONFIG_LWIP_TCP_MSS=1436
+CONFIG_LWIP_TCP_TMR_INTERVAL=250
+CONFIG_LWIP_TCP_MSL=60000
+CONFIG_LWIP_TCP_SND_BUF_DEFAULT=5744
+CONFIG_LWIP_TCP_WND_DEFAULT=5744
+CONFIG_LWIP_TCP_RECVMBOX_SIZE=6
+CONFIG_LWIP_TCP_QUEUE_OOSEQ=y
+# CONFIG_LWIP_TCP_SACK_OUT is not set
+# CONFIG_LWIP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set
+CONFIG_LWIP_TCP_OVERSIZE_MSS=y
+# CONFIG_LWIP_TCP_OVERSIZE_QUARTER_MSS is not set
+# CONFIG_LWIP_TCP_OVERSIZE_DISABLE is not set
+CONFIG_LWIP_TCP_RTO_TIME=1500
+# end of TCP
+
+#
+# UDP
+#
+CONFIG_LWIP_MAX_UDP_PCBS=16
+CONFIG_LWIP_UDP_RECVMBOX_SIZE=6
+# end of UDP
+
+#
+# Checksums
+#
+# CONFIG_LWIP_CHECKSUM_CHECK_IP is not set
+# CONFIG_LWIP_CHECKSUM_CHECK_UDP is not set
+CONFIG_LWIP_CHECKSUM_CHECK_ICMP=y
+# end of Checksums
+
+CONFIG_LWIP_TCPIP_TASK_STACK_SIZE=2560
+CONFIG_LWIP_TCPIP_TASK_AFFINITY_NO_AFFINITY=y
+# CONFIG_LWIP_TCPIP_TASK_AFFINITY_CPU0 is not set
+CONFIG_LWIP_TCPIP_TASK_AFFINITY=0x7FFFFFFF
+# CONFIG_LWIP_PPP_SUPPORT is not set
+CONFIG_LWIP_IPV6_MEMP_NUM_ND6_QUEUE=3
+CONFIG_LWIP_IPV6_ND6_NUM_NEIGHBORS=5
+# CONFIG_LWIP_SLIP_SUPPORT is not set
+
+#
+# ICMP
+#
+CONFIG_LWIP_ICMP=y
+# CONFIG_LWIP_MULTICAST_PING is not set
+# CONFIG_LWIP_BROADCAST_PING is not set
+# end of ICMP
+
+#
+# LWIP RAW API
+#
+CONFIG_LWIP_MAX_RAW_PCBS=16
+# end of LWIP RAW API
+
+#
+# SNTP
+#
+CONFIG_LWIP_SNTP_MAX_SERVERS=1
+# CONFIG_LWIP_DHCP_GET_NTP_SRV is not set
+CONFIG_LWIP_SNTP_UPDATE_DELAY=3600000
+# end of SNTP
+
+CONFIG_LWIP_ESP_LWIP_ASSERT=y
+
+#
+# Hooks
+#
+# CONFIG_LWIP_HOOK_TCP_ISN_NONE is not set
+CONFIG_LWIP_HOOK_TCP_ISN_DEFAULT=y
+# CONFIG_LWIP_HOOK_TCP_ISN_CUSTOM is not set
+CONFIG_LWIP_HOOK_IP6_ROUTE_NONE=y
+# CONFIG_LWIP_HOOK_IP6_ROUTE_DEFAULT is not set
+# CONFIG_LWIP_HOOK_IP6_ROUTE_CUSTOM is not set
+CONFIG_LWIP_HOOK_ND6_GET_GW_NONE=y
+# CONFIG_LWIP_HOOK_ND6_GET_GW_DEFAULT is not set
+# CONFIG_LWIP_HOOK_ND6_GET_GW_CUSTOM is not set
+CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_NONE=y
+# CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_DEFAULT is not set
+# CONFIG_LWIP_HOOK_NETCONN_EXT_RESOLVE_CUSTOM is not set
+# end of Hooks
+
+# CONFIG_LWIP_DEBUG is not set
+# end of LWIP
+
+#
+# mbedTLS
+#
+CONFIG_MBEDTLS_INTERNAL_MEM_ALLOC=y
+# CONFIG_MBEDTLS_DEFAULT_MEM_ALLOC is not set
+# CONFIG_MBEDTLS_CUSTOM_MEM_ALLOC is not set
+CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN=y
+CONFIG_MBEDTLS_SSL_IN_CONTENT_LEN=16384
+CONFIG_MBEDTLS_SSL_OUT_CONTENT_LEN=4096
+# CONFIG_MBEDTLS_DYNAMIC_BUFFER is not set
+# CONFIG_MBEDTLS_DEBUG is not set
+
+#
+# mbedTLS v2.28.x related
+#
+# CONFIG_MBEDTLS_SSL_VARIABLE_BUFFER_LENGTH is not set
+# CONFIG_MBEDTLS_X509_TRUSTED_CERT_CALLBACK is not set
+# CONFIG_MBEDTLS_SSL_CONTEXT_SERIALIZATION is not set
+CONFIG_MBEDTLS_SSL_KEEP_PEER_CERTIFICATE=y
+# end of mbedTLS v2.28.x related
+
+#
+# Certificate Bundle
+#
+CONFIG_MBEDTLS_CERTIFICATE_BUNDLE=y
+CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_FULL=y
+# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_CMN is not set
+# CONFIG_MBEDTLS_CERTIFICATE_BUNDLE_DEFAULT_NONE is not set
+# CONFIG_MBEDTLS_CUSTOM_CERTIFICATE_BUNDLE is not set
+# end of Certificate Bundle
+
+# CONFIG_MBEDTLS_ECP_RESTARTABLE is not set
+# CONFIG_MBEDTLS_CMAC_C is not set
+CONFIG_MBEDTLS_HARDWARE_AES=y
+CONFIG_MBEDTLS_HARDWARE_MPI=y
+CONFIG_MBEDTLS_HARDWARE_SHA=y
+CONFIG_MBEDTLS_ROM_MD5=y
+# CONFIG_MBEDTLS_ATCA_HW_ECDSA_SIGN is not set
+# CONFIG_MBEDTLS_ATCA_HW_ECDSA_VERIFY is not set
+CONFIG_MBEDTLS_HAVE_TIME=y
+# CONFIG_MBEDTLS_HAVE_TIME_DATE is not set
+CONFIG_MBEDTLS_ECDSA_DETERMINISTIC=y
+CONFIG_MBEDTLS_SHA512_C=y
+CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y
+# CONFIG_MBEDTLS_TLS_SERVER_ONLY is not set
+# CONFIG_MBEDTLS_TLS_CLIENT_ONLY is not set
+# CONFIG_MBEDTLS_TLS_DISABLED is not set
+CONFIG_MBEDTLS_TLS_SERVER=y
+CONFIG_MBEDTLS_TLS_CLIENT=y
+CONFIG_MBEDTLS_TLS_ENABLED=y
+
+#
+# TLS Key Exchange Methods
+#
+# CONFIG_MBEDTLS_PSK_MODES is not set
+CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y
+# end of TLS Key Exchange Methods
+
+CONFIG_MBEDTLS_SSL_RENEGOTIATION=y
+# CONFIG_MBEDTLS_SSL_PROTO_SSL3 is not set
+CONFIG_MBEDTLS_SSL_PROTO_TLS1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y
+# CONFIG_MBEDTLS_SSL_PROTO_GMTSSL1_1 is not set
+# CONFIG_MBEDTLS_SSL_PROTO_DTLS is not set
+CONFIG_MBEDTLS_SSL_ALPN=y
+CONFIG_MBEDTLS_CLIENT_SSL_SESSION_TICKETS=y
+CONFIG_MBEDTLS_X509_CHECK_KEY_USAGE=y
+CONFIG_MBEDTLS_X509_CHECK_EXTENDED_KEY_USAGE=y
+CONFIG_MBEDTLS_SERVER_SSL_SESSION_TICKETS=y
+
+#
+# Symmetric Ciphers
+#
+CONFIG_MBEDTLS_AES_C=y
+# CONFIG_MBEDTLS_CAMELLIA_C is not set
+# CONFIG_MBEDTLS_DES_C is not set
+CONFIG_MBEDTLS_RC4_DISABLED=y
+# CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT is not set
+# CONFIG_MBEDTLS_RC4_ENABLED is not set
+# CONFIG_MBEDTLS_BLOWFISH_C is not set
+# CONFIG_MBEDTLS_XTEA_C is not set
+CONFIG_MBEDTLS_CCM_C=y
+CONFIG_MBEDTLS_GCM_C=y
+# CONFIG_MBEDTLS_NIST_KW_C is not set
+# end of Symmetric Ciphers
+
+# CONFIG_MBEDTLS_RIPEMD160_C is not set
+
+#
+# Certificates
+#
+CONFIG_MBEDTLS_PEM_PARSE_C=y
+CONFIG_MBEDTLS_PEM_WRITE_C=y
+CONFIG_MBEDTLS_X509_CRL_PARSE_C=y
+CONFIG_MBEDTLS_X509_CSR_PARSE_C=y
+# end of Certificates
+
+CONFIG_MBEDTLS_ECP_C=y
+CONFIG_MBEDTLS_ECDH_C=y
+CONFIG_MBEDTLS_ECDSA_C=y
+# CONFIG_MBEDTLS_ECJPAKE_C is not set
+CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y
+CONFIG_MBEDTLS_ECP_NIST_OPTIM=y
+# CONFIG_MBEDTLS_POLY1305_C is not set
+# CONFIG_MBEDTLS_CHACHA20_C is not set
+# CONFIG_MBEDTLS_HKDF_C is not set
+# CONFIG_MBEDTLS_THREADING_C is not set
+# CONFIG_MBEDTLS_LARGE_KEY_SOFTWARE_MPI is not set
+# CONFIG_MBEDTLS_SECURITY_RISKS is not set
+# end of mbedTLS
+
+#
+# mDNS
+#
+CONFIG_MDNS_MAX_SERVICES=10
+CONFIG_MDNS_TASK_PRIORITY=1
+CONFIG_MDNS_TASK_STACK_SIZE=4096
+# CONFIG_MDNS_TASK_AFFINITY_NO_AFFINITY is not set
+CONFIG_MDNS_TASK_AFFINITY_CPU0=y
+CONFIG_MDNS_TASK_AFFINITY=0x0
+CONFIG_MDNS_SERVICE_ADD_TIMEOUT_MS=2000
+# CONFIG_MDNS_STRICT_MODE is not set
+CONFIG_MDNS_TIMER_PERIOD_MS=100
+# CONFIG_MDNS_NETWORKING_SOCKET is not set
+CONFIG_MDNS_MULTIPLE_INSTANCE=y
+# end of mDNS
+
+#
+# ESP-MQTT Configurations
+#
+CONFIG_MQTT_PROTOCOL_311=y
+CONFIG_MQTT_TRANSPORT_SSL=y
+CONFIG_MQTT_TRANSPORT_WEBSOCKET=y
+CONFIG_MQTT_TRANSPORT_WEBSOCKET_SECURE=y
+# CONFIG_MQTT_MSG_ID_INCREMENTAL is not set
+# CONFIG_MQTT_SKIP_PUBLISH_IF_DISCONNECTED is not set
+# CONFIG_MQTT_REPORT_DELETED_MESSAGES is not set
+# CONFIG_MQTT_USE_CUSTOM_CONFIG is not set
+# CONFIG_MQTT_TASK_CORE_SELECTION_ENABLED is not set
+# CONFIG_MQTT_CUSTOM_OUTBOX is not set
+# end of ESP-MQTT Configurations
+
+#
+# Newlib
+#
+CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y
+# CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF is not set
+# CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR is not set
+# CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF is not set
+# CONFIG_NEWLIB_STDIN_LINE_ENDING_LF is not set
+CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y
+# CONFIG_NEWLIB_NANO_FORMAT is not set
+# end of Newlib
+
+#
+# NVS
+#
+# end of NVS
+
+#
+# OpenSSL
+#
+# CONFIG_OPENSSL_DEBUG is not set
+CONFIG_OPENSSL_ERROR_STACK=y
+CONFIG_OPENSSL_ASSERT_DO_NOTHING=y
+# CONFIG_OPENSSL_ASSERT_EXIT is not set
+# end of OpenSSL
+
+#
+# OpenThread
+#
+# CONFIG_OPENTHREAD_ENABLED is not set
+# end of OpenThread
+
+#
+# PThreads
+#
+CONFIG_PTHREAD_TASK_PRIO_DEFAULT=5
+CONFIG_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072
+CONFIG_PTHREAD_STACK_MIN=768
+CONFIG_PTHREAD_TASK_CORE_DEFAULT=-1
+CONFIG_PTHREAD_TASK_NAME_DEFAULT="pthread"
+# end of PThreads
+
+#
+# SPI Flash driver
+#
+# CONFIG_SPI_FLASH_VERIFY_WRITE is not set
+# CONFIG_SPI_FLASH_ENABLE_COUNTERS is not set
+CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y
+CONFIG_SPI_FLASH_DANGEROUS_WRITE_ABORTS=y
+# CONFIG_SPI_FLASH_DANGEROUS_WRITE_FAILS is not set
+# CONFIG_SPI_FLASH_DANGEROUS_WRITE_ALLOWED is not set
+# CONFIG_SPI_FLASH_USE_LEGACY_IMPL is not set
+# CONFIG_SPI_FLASH_SHARE_SPI1_BUS is not set
+# CONFIG_SPI_FLASH_BYPASS_BLOCK_ERASE is not set
+CONFIG_SPI_FLASH_YIELD_DURING_ERASE=y
+CONFIG_SPI_FLASH_ERASE_YIELD_DURATION_MS=20
+CONFIG_SPI_FLASH_ERASE_YIELD_TICKS=1
+CONFIG_SPI_FLASH_WRITE_CHUNK_SIZE=8192
+# CONFIG_SPI_FLASH_SIZE_OVERRIDE is not set
+# CONFIG_SPI_FLASH_CHECK_ERASE_TIMEOUT_DISABLED is not set
+# CONFIG_SPI_FLASH_OVERRIDE_CHIP_DRIVER_LIST is not set
+
+#
+# Auto-detect flash chips
+#
+CONFIG_SPI_FLASH_SUPPORT_ISSI_CHIP=y
+CONFIG_SPI_FLASH_SUPPORT_MXIC_CHIP=y
+CONFIG_SPI_FLASH_SUPPORT_GD_CHIP=y
+CONFIG_SPI_FLASH_SUPPORT_WINBOND_CHIP=y
+# CONFIG_SPI_FLASH_SUPPORT_BOYA_CHIP is not set
+# CONFIG_SPI_FLASH_SUPPORT_TH_CHIP is not set
+# end of Auto-detect flash chips
+
+CONFIG_SPI_FLASH_ENABLE_ENCRYPTED_READ_WRITE=y
+# end of SPI Flash driver
+
+#
+# SPIFFS Configuration
+#
+CONFIG_SPIFFS_MAX_PARTITIONS=3
+
+#
+# SPIFFS Cache Configuration
+#
+CONFIG_SPIFFS_CACHE=y
+CONFIG_SPIFFS_CACHE_WR=y
+# CONFIG_SPIFFS_CACHE_STATS is not set
+# end of SPIFFS Cache Configuration
+
+CONFIG_SPIFFS_PAGE_CHECK=y
+CONFIG_SPIFFS_GC_MAX_RUNS=10
+# CONFIG_SPIFFS_GC_STATS is not set
+CONFIG_SPIFFS_PAGE_SIZE=256
+CONFIG_SPIFFS_OBJ_NAME_LEN=32
+# CONFIG_SPIFFS_FOLLOW_SYMLINKS is not set
+CONFIG_SPIFFS_USE_MAGIC=y
+CONFIG_SPIFFS_USE_MAGIC_LENGTH=y
+CONFIG_SPIFFS_META_LENGTH=4
+CONFIG_SPIFFS_USE_MTIME=y
+
+#
+# Debug Configuration
+#
+# CONFIG_SPIFFS_DBG is not set
+# CONFIG_SPIFFS_API_DBG is not set
+# CONFIG_SPIFFS_GC_DBG is not set
+# CONFIG_SPIFFS_CACHE_DBG is not set
+# CONFIG_SPIFFS_CHECK_DBG is not set
+# CONFIG_SPIFFS_TEST_VISUALISATION is not set
+# end of Debug Configuration
+# end of SPIFFS Configuration
+
+#
+# TCP Transport
+#
+
+#
+# Websocket
+#
+CONFIG_WS_TRANSPORT=y
+CONFIG_WS_BUFFER_SIZE=1024
+# end of Websocket
+# end of TCP Transport
+
+#
+# Unity unit testing library
+#
+CONFIG_UNITY_ENABLE_FLOAT=y
+CONFIG_UNITY_ENABLE_DOUBLE=y
+# CONFIG_UNITY_ENABLE_64BIT is not set
+# CONFIG_UNITY_ENABLE_COLOR is not set
+CONFIG_UNITY_ENABLE_IDF_TEST_RUNNER=y
+# CONFIG_UNITY_ENABLE_FIXTURE is not set
+# CONFIG_UNITY_ENABLE_BACKTRACE_ON_FAIL is not set
+# end of Unity unit testing library
+
+#
+# Virtual file system
+#
+CONFIG_VFS_SUPPORT_IO=y
+CONFIG_VFS_SUPPORT_DIR=y
+CONFIG_VFS_SUPPORT_SELECT=y
+CONFIG_VFS_SUPPRESS_SELECT_DEBUG_OUTPUT=y
+CONFIG_VFS_SUPPORT_TERMIOS=y
+
+#
+# Host File System I/O (Semihosting)
+#
+CONFIG_VFS_SEMIHOSTFS_MAX_MOUNT_POINTS=1
+CONFIG_VFS_SEMIHOSTFS_HOST_PATH_MAX_LEN=128
+# end of Host File System I/O (Semihosting)
+# end of Virtual file system
+
+#
+# Wear Levelling
+#
+# CONFIG_WL_SECTOR_SIZE_512 is not set
+CONFIG_WL_SECTOR_SIZE_4096=y
+CONFIG_WL_SECTOR_SIZE=4096
+# end of Wear Levelling
+
+#
+# Wi-Fi Provisioning Manager
+#
+CONFIG_WIFI_PROV_SCAN_MAX_ENTRIES=16
+CONFIG_WIFI_PROV_AUTOSTOP_TIMEOUT=30
+# end of Wi-Fi Provisioning Manager
+
+#
+# Supplicant
+#
+CONFIG_WPA_MBEDTLS_CRYPTO=y
+# CONFIG_WPA_WAPI_PSK is not set
+# CONFIG_WPA_SUITE_B_192 is not set
+# CONFIG_WPA_DEBUG_PRINT is not set
+# CONFIG_WPA_TESTING_OPTIONS is not set
+# CONFIG_WPA_WPS_STRICT is not set
+# CONFIG_WPA_11KV_SUPPORT is not set
+# end of Supplicant
+# end of Component config
+
+#
+# Compatibility options
+#
+# CONFIG_LEGACY_INCLUDE_COMMON_HEADERS is not set
+# end of Compatibility options
+
+# Deprecated options for backward compatibility
+CONFIG_TOOLPREFIX="xtensa-esp32-elf-"
+# CONFIG_LOG_BOOTLOADER_LEVEL_NONE is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_ERROR is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_WARN is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_INFO is not set
+CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG=y
+# CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE is not set
+CONFIG_LOG_BOOTLOADER_LEVEL=4
+# CONFIG_APP_ROLLBACK_ENABLE is not set
+# CONFIG_FLASH_ENCRYPTION_ENABLED is not set
+# CONFIG_FLASHMODE_QIO is not set
+# CONFIG_FLASHMODE_QOUT is not set
+CONFIG_FLASHMODE_DIO=y
+# CONFIG_FLASHMODE_DOUT is not set
+# CONFIG_MONITOR_BAUD_9600B is not set
+# CONFIG_MONITOR_BAUD_57600B is not set
+CONFIG_MONITOR_BAUD_115200B=y
+# CONFIG_MONITOR_BAUD_230400B is not set
+# CONFIG_MONITOR_BAUD_921600B is not set
+# CONFIG_MONITOR_BAUD_2MB is not set
+# CONFIG_MONITOR_BAUD_OTHER is not set
+CONFIG_MONITOR_BAUD_OTHER_VAL=115200
+CONFIG_MONITOR_BAUD=115200
+CONFIG_COMPILER_OPTIMIZATION_LEVEL_DEBUG=y
+# CONFIG_COMPILER_OPTIMIZATION_LEVEL_RELEASE is not set
+CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y
+# CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set
+# CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED is not set
+CONFIG_OPTIMIZATION_ASSERTION_LEVEL=2
+# CONFIG_CXX_EXCEPTIONS is not set
+CONFIG_STACK_CHECK_NONE=y
+# CONFIG_STACK_CHECK_NORM is not set
+# CONFIG_STACK_CHECK_STRONG is not set
+# CONFIG_STACK_CHECK_ALL is not set
+# CONFIG_WARN_WRITE_STRINGS is not set
+# CONFIG_DISABLE_GCC8_WARNINGS is not set
+# CONFIG_ESP32_APPTRACE_DEST_TRAX is not set
+CONFIG_ESP32_APPTRACE_DEST_NONE=y
+CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y
+CONFIG_ADC2_DISABLE_DAC=y
+# CONFIG_SPIRAM_SUPPORT is not set
+CONFIG_TRACEMEM_RESERVE_DRAM=0x0
+# CONFIG_ULP_COPROC_ENABLED is not set
+CONFIG_ULP_COPROC_RESERVE_MEM=0
+CONFIG_BROWNOUT_DET=y
+CONFIG_BROWNOUT_DET_LVL_SEL_0=y
+# CONFIG_BROWNOUT_DET_LVL_SEL_1 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_2 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_3 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_4 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_5 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_6 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_7 is not set
+CONFIG_BROWNOUT_DET_LVL=0
+CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y
+# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL is not set
+# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_OSC is not set
+# CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_8MD256 is not set
+# CONFIG_DISABLE_BASIC_ROM_CONSOLE is not set
+# CONFIG_NO_BLOBS is not set
+# CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS is not set
+# CONFIG_EVENT_LOOP_PROFILING is not set
+CONFIG_POST_EVENTS_FROM_ISR=y
+CONFIG_POST_EVENTS_FROM_IRAM_ISR=y
+CONFIG_GDBSTUB_SUPPORT_TASKS=y
+CONFIG_GDBSTUB_MAX_TASKS=32
+# CONFIG_TWO_UNIVERSAL_MAC_ADDRESS is not set
+CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y
+CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4
+CONFIG_ESP_SYSTEM_PD_FLASH=y
+# CONFIG_ESP32C3_LIGHTSLEEP_GPIO_RESET_WORKAROUND is not set
+CONFIG_IPC_TASK_STACK_SIZE=1024
+CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y
+# CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set
+CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20
+CONFIG_ESP32_PHY_MAX_TX_POWER=20
+CONFIG_ESP32_REDUCE_PHY_TX_POWER=y
+# CONFIG_ESP32S2_PANIC_PRINT_HALT is not set
+# CONFIG_ESP32S2_PANIC_PRINT_REBOOT is not set
+# CONFIG_ESP32S2_PANIC_SILENT_REBOOT is not set
+CONFIG_ESP32S2_PANIC_GDBSTUB=y
+CONFIG_ESP32S2_ALLOW_RTC_FAST_MEM_AS_HEAP=y
+CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32
+CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4096
+CONFIG_MAIN_TASK_STACK_SIZE=4096
+CONFIG_CONSOLE_UART_DEFAULT=y
+# CONFIG_CONSOLE_UART_CUSTOM is not set
+# CONFIG_ESP_CONSOLE_UART_NONE is not set
+CONFIG_CONSOLE_UART=y
+CONFIG_CONSOLE_UART_NUM=0
+CONFIG_CONSOLE_UART_BAUDRATE=115200
+CONFIG_INT_WDT=y
+CONFIG_INT_WDT_TIMEOUT_MS=300
+CONFIG_TASK_WDT=y
+# CONFIG_TASK_WDT_PANIC is not set
+CONFIG_TASK_WDT_TIMEOUT_S=5
+CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y
+# CONFIG_ESP32_DEBUG_STUBS_ENABLE is not set
+CONFIG_TIMER_TASK_STACK_SIZE=3584
+# CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH is not set
+# CONFIG_ESP32_ENABLE_COREDUMP_TO_UART is not set
+CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y
+CONFIG_MB_MASTER_TIMEOUT_MS_RESPOND=150
+CONFIG_MB_MASTER_DELAY_MS_CONVERT=200
+CONFIG_MB_QUEUE_LENGTH=20
+CONFIG_MB_SERIAL_TASK_STACK_SIZE=4096
+CONFIG_MB_SERIAL_BUF_SIZE=256
+CONFIG_MB_SERIAL_TASK_PRIO=10
+CONFIG_MB_CONTROLLER_SLAVE_ID_SUPPORT=y
+CONFIG_MB_CONTROLLER_SLAVE_ID=0x00112233
+CONFIG_MB_CONTROLLER_NOTIFY_TIMEOUT=20
+CONFIG_MB_CONTROLLER_NOTIFY_QUEUE_SIZE=20
+CONFIG_MB_CONTROLLER_STACK_SIZE=4096
+CONFIG_MB_EVENT_QUEUE_TIMEOUT=20
+# CONFIG_MB_TIMER_PORT_ENABLED is not set
+CONFIG_MB_TIMER_GROUP=0
+CONFIG_MB_TIMER_INDEX=0
+# CONFIG_ENABLE_STATIC_TASK_CLEAN_UP_HOOK is not set
+CONFIG_TIMER_TASK_PRIORITY=1
+CONFIG_TIMER_TASK_STACK_DEPTH=2048
+CONFIG_TIMER_QUEUE_LENGTH=10
+# CONFIG_L2_TO_L3_COPY is not set
+# CONFIG_USE_ONLY_LWIP_SELECT is not set
+CONFIG_ESP_GRATUITOUS_ARP=y
+CONFIG_GARP_TMR_INTERVAL=60
+CONFIG_TCPIP_RECVMBOX_SIZE=32
+CONFIG_TCP_MAXRTX=12
+CONFIG_TCP_SYNMAXRTX=6
+CONFIG_TCP_MSS=1436
+CONFIG_TCP_MSL=60000
+CONFIG_TCP_SND_BUF_DEFAULT=5744
+CONFIG_TCP_WND_DEFAULT=5744
+CONFIG_TCP_RECVMBOX_SIZE=6
+CONFIG_TCP_QUEUE_OOSEQ=y
+# CONFIG_ESP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES is not set
+CONFIG_TCP_OVERSIZE_MSS=y
+# CONFIG_TCP_OVERSIZE_QUARTER_MSS is not set
+# CONFIG_TCP_OVERSIZE_DISABLE is not set
+CONFIG_UDP_RECVMBOX_SIZE=6
+CONFIG_TCPIP_TASK_STACK_SIZE=2560
+CONFIG_TCPIP_TASK_AFFINITY_NO_AFFINITY=y
+# CONFIG_TCPIP_TASK_AFFINITY_CPU0 is not set
+CONFIG_TCPIP_TASK_AFFINITY=0x7FFFFFFF
+# CONFIG_PPP_SUPPORT is not set
+CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5
+CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072
+CONFIG_ESP32_PTHREAD_STACK_MIN=768
+CONFIG_ESP32_PTHREAD_TASK_CORE_DEFAULT=-1
+CONFIG_ESP32_PTHREAD_TASK_NAME_DEFAULT="pthread"
+CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y
+# CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS is not set
+# CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED is not set
+CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y
+CONFIG_SUPPORT_TERMIOS=y
+CONFIG_SEMIHOSTFS_MAX_MOUNT_POINTS=1
+CONFIG_SEMIHOSTFS_HOST_PATH_MAX_LEN=128
+# End of deprecated options
diff --git a/ESP32_ESP-IDF/sdkconfig.old b/ESP32_ESP-IDF/sdkconfig.old
new file mode 100644
index 00000000..9d0442cc
--- /dev/null
+++ b/ESP32_ESP-IDF/sdkconfig.old
@@ -0,0 +1,433 @@
+#
+# Automatically generated file; DO NOT EDIT.
+# Espressif IoT Development Framework Configuration
+#
+
+#
+# SDK tool configuration
+#
+CONFIG_TOOLPREFIX="xtensa-esp32-elf-"
+CONFIG_PYTHON="python"
+
+#
+# Bootloader config
+#
+# CONFIG_LOG_BOOTLOADER_LEVEL_NONE is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_ERROR is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_WARN is not set
+# CONFIG_LOG_BOOTLOADER_LEVEL_INFO is not set
+CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG=y
+# CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE is not set
+CONFIG_LOG_BOOTLOADER_LEVEL=4
+
+#
+# Security features
+#
+# CONFIG_SECURE_BOOT_ENABLED is not set
+# CONFIG_FLASH_ENCRYPTION_ENABLED is not set
+
+#
+# Serial flasher config
+#
+CONFIG_ESPTOOLPY_PORT="com3"
+# CONFIG_ESPTOOLPY_BAUD_115200B is not set
+# CONFIG_ESPTOOLPY_BAUD_230400B is not set
+CONFIG_ESPTOOLPY_BAUD_921600B=y
+# CONFIG_ESPTOOLPY_BAUD_2MB is not set
+# CONFIG_ESPTOOLPY_BAUD_OTHER is not set
+CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200
+CONFIG_ESPTOOLPY_BAUD=921600
+# CONFIG_ESPTOOLPY_COMPRESSED is not set
+# CONFIG_FLASHMODE_QIO is not set
+# CONFIG_FLASHMODE_QOUT is not set
+CONFIG_FLASHMODE_DIO=y
+# CONFIG_FLASHMODE_DOUT is not set
+CONFIG_ESPTOOLPY_FLASHMODE="dio"
+# CONFIG_ESPTOOLPY_FLASHFREQ_80M is not set
+CONFIG_ESPTOOLPY_FLASHFREQ_40M=y
+# CONFIG_ESPTOOLPY_FLASHFREQ_26M is not set
+# CONFIG_ESPTOOLPY_FLASHFREQ_20M is not set
+CONFIG_ESPTOOLPY_FLASHFREQ="40m"
+# CONFIG_ESPTOOLPY_FLASHSIZE_1MB is not set
+# CONFIG_ESPTOOLPY_FLASHSIZE_2MB is not set
+CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y
+# CONFIG_ESPTOOLPY_FLASHSIZE_8MB is not set
+# CONFIG_ESPTOOLPY_FLASHSIZE_16MB is not set
+CONFIG_ESPTOOLPY_FLASHSIZE="4MB"
+CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y
+CONFIG_ESPTOOLPY_BEFORE_RESET=y
+# CONFIG_ESPTOOLPY_BEFORE_NORESET is not set
+CONFIG_ESPTOOLPY_BEFORE="default_reset"
+CONFIG_ESPTOOLPY_AFTER_RESET=y
+# CONFIG_ESPTOOLPY_AFTER_NORESET is not set
+CONFIG_ESPTOOLPY_AFTER="hard_reset"
+# CONFIG_MONITOR_BAUD_9600B is not set
+# CONFIG_MONITOR_BAUD_57600B is not set
+CONFIG_MONITOR_BAUD_115200B=y
+# CONFIG_MONITOR_BAUD_230400B is not set
+# CONFIG_MONITOR_BAUD_921600B is not set
+# CONFIG_MONITOR_BAUD_2MB is not set
+# CONFIG_MONITOR_BAUD_OTHER is not set
+CONFIG_MONITOR_BAUD_OTHER_VAL=115200
+CONFIG_MONITOR_BAUD=115200
+
+#
+# Partition Table
+#
+CONFIG_PARTITION_TABLE_SINGLE_APP=y
+# CONFIG_PARTITION_TABLE_TWO_OTA is not set
+# CONFIG_PARTITION_TABLE_CUSTOM is not set
+CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv"
+CONFIG_PARTITION_TABLE_CUSTOM_APP_BIN_OFFSET=0x10000
+CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp.csv"
+CONFIG_APP_OFFSET=0x10000
+
+#
+# Compiler options
+#
+CONFIG_OPTIMIZATION_LEVEL_DEBUG=y
+# CONFIG_OPTIMIZATION_LEVEL_RELEASE is not set
+CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y
+# CONFIG_OPTIMIZATION_ASSERTIONS_SILENT is not set
+# CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED is not set
+
+#
+# Component config
+#
+
+#
+# SSD1331 configuration
+#
+CONFIG_HW_LCD_MOSI_GPIO=23
+CONFIG_HW_LCD_CLK_GPIO=18
+CONFIG_HW_LCD_CS_GPIO=5
+CONFIG_HW_LCD_DC_GPIO=16
+CONFIG_HW_LCD_RESET_GPIO=17
+
+#
+# Application Level Tracing
+#
+# CONFIG_ESP32_APPTRACE_DEST_TRAX is not set
+CONFIG_ESP32_APPTRACE_DEST_NONE=y
+# CONFIG_ESP32_APPTRACE_ENABLE is not set
+CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y
+
+#
+# FreeRTOS SystemView Tracing
+#
+# CONFIG_AWS_IOT_SDK is not set
+# CONFIG_BT_ENABLED is not set
+CONFIG_BT_RESERVE_DRAM=0
+
+#
+# ESP32-specific
+#
+# CONFIG_ESP32_DEFAULT_CPU_FREQ_80 is not set
+# CONFIG_ESP32_DEFAULT_CPU_FREQ_160 is not set
+CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y
+CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240
+# CONFIG_MEMMAP_SMP is not set
+# CONFIG_MEMMAP_TRACEMEM is not set
+# CONFIG_MEMMAP_TRACEMEM_TWOBANKS is not set
+# CONFIG_ESP32_TRAX is not set
+CONFIG_TRACEMEM_RESERVE_DRAM=0x0
+# CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH is not set
+# CONFIG_ESP32_ENABLE_COREDUMP_TO_UART is not set
+CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y
+# CONFIG_ESP32_ENABLE_COREDUMP is not set
+# CONFIG_TWO_UNIVERSAL_MAC_ADDRESS is not set
+CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y
+CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4
+CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32
+CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4096
+CONFIG_MAIN_TASK_STACK_SIZE=4096
+CONFIG_IPC_TASK_STACK_SIZE=1024
+CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y
+# CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF is not set
+# CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR is not set
+# CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF is not set
+# CONFIG_NEWLIB_STDIN_LINE_ENDING_LF is not set
+CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y
+# CONFIG_NEWLIB_NANO_FORMAT is not set
+CONFIG_CONSOLE_UART_DEFAULT=y
+# CONFIG_CONSOLE_UART_CUSTOM is not set
+# CONFIG_CONSOLE_UART_NONE is not set
+CONFIG_CONSOLE_UART_NUM=0
+CONFIG_CONSOLE_UART_BAUDRATE=115200
+# CONFIG_ULP_COPROC_ENABLED is not set
+CONFIG_ULP_COPROC_RESERVE_MEM=0
+# CONFIG_ESP32_PANIC_PRINT_HALT is not set
+# CONFIG_ESP32_PANIC_PRINT_REBOOT is not set
+# CONFIG_ESP32_PANIC_SILENT_REBOOT is not set
+CONFIG_ESP32_PANIC_GDBSTUB=y
+CONFIG_ESP32_DEBUG_OCDAWARE=y
+CONFIG_INT_WDT=y
+CONFIG_INT_WDT_TIMEOUT_MS=300
+CONFIG_TASK_WDT=y
+# CONFIG_TASK_WDT_PANIC is not set
+CONFIG_TASK_WDT_TIMEOUT_S=5
+CONFIG_TASK_WDT_CHECK_IDLE_TASK=y
+CONFIG_BROWNOUT_DET=y
+CONFIG_BROWNOUT_DET_LVL_SEL_0=y
+# CONFIG_BROWNOUT_DET_LVL_SEL_1 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_2 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_3 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_4 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_5 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_6 is not set
+# CONFIG_BROWNOUT_DET_LVL_SEL_7 is not set
+CONFIG_BROWNOUT_DET_LVL=0
+# CONFIG_ESP32_TIME_SYSCALL_USE_RTC is not set
+CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y
+# CONFIG_ESP32_TIME_SYSCALL_USE_FRC1 is not set
+# CONFIG_ESP32_TIME_SYSCALL_USE_NONE is not set
+CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y
+# CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL is not set
+CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024
+CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000
+# CONFIG_ESP32_XTAL_FREQ_40 is not set
+# CONFIG_ESP32_XTAL_FREQ_26 is not set
+CONFIG_ESP32_XTAL_FREQ_AUTO=y
+CONFIG_ESP32_XTAL_FREQ=0
+# CONFIG_NO_BLOBS is not set
+
+#
+# Wi-Fi
+#
+CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10
+CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32
+# CONFIG_ESP32_WIFI_STATIC_TX_BUFFER is not set
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y
+CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1
+CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32
+CONFIG_ESP32_WIFI_AMPDU_ENABLED=y
+CONFIG_ESP32_WIFI_TX_BA_WIN=6
+CONFIG_ESP32_WIFI_RX_BA_WIN=6
+CONFIG_ESP32_WIFI_NVS_ENABLED=y
+
+#
+# PHY
+#
+CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y
+# CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION is not set
+CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20
+CONFIG_ESP32_PHY_MAX_TX_POWER=20
+
+#
+# ETHERNET
+#
+CONFIG_DMA_RX_BUF_NUM=10
+CONFIG_DMA_TX_BUF_NUM=10
+# CONFIG_EMAC_L2_TO_L3_RX_BUF_MODE is not set
+CONFIG_EMAC_TASK_PRIORITY=20
+
+#
+# FAT Filesystem support
+#
+CONFIG_FATFS_CODEPAGE_ASCII=y
+# CONFIG_FATFS_CODEPAGE_437 is not set
+# CONFIG_FATFS_CODEPAGE_720 is not set
+# CONFIG_FATFS_CODEPAGE_737 is not set
+# CONFIG_FATFS_CODEPAGE_771 is not set
+# CONFIG_FATFS_CODEPAGE_775 is not set
+# CONFIG_FATFS_CODEPAGE_850 is not set
+# CONFIG_FATFS_CODEPAGE_852 is not set
+# CONFIG_FATFS_CODEPAGE_855 is not set
+# CONFIG_FATFS_CODEPAGE_857 is not set
+# CONFIG_FATFS_CODEPAGE_860 is not set
+# CONFIG_FATFS_CODEPAGE_861 is not set
+# CONFIG_FATFS_CODEPAGE_862 is not set
+# CONFIG_FATFS_CODEPAGE_863 is not set
+# CONFIG_FATFS_CODEPAGE_864 is not set
+# CONFIG_FATFS_CODEPAGE_865 is not set
+# CONFIG_FATFS_CODEPAGE_866 is not set
+# CONFIG_FATFS_CODEPAGE_869 is not set
+# CONFIG_FATFS_CODEPAGE_932 is not set
+# CONFIG_FATFS_CODEPAGE_936 is not set
+# CONFIG_FATFS_CODEPAGE_949 is not set
+# CONFIG_FATFS_CODEPAGE_950 is not set
+CONFIG_FATFS_CODEPAGE=1
+CONFIG_FATFS_MAX_LFN=255
+
+#
+# FreeRTOS
+#
+CONFIG_FREERTOS_UNICORE=y
+CONFIG_FREERTOS_CORETIMER_0=y
+# CONFIG_FREERTOS_CORETIMER_1 is not set
+CONFIG_FREERTOS_HZ=1000
+CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y
+# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE is not set
+# CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL is not set
+CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y
+# CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK is not set
+CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1
+# CONFIG_FREERTOS_ASSERT_FAIL_ABORT is not set
+CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE=y
+# CONFIG_FREERTOS_ASSERT_DISABLE is not set
+# CONFIG_FREERTOS_BREAK_ON_SCHEDULER_START_JTAG is not set
+# CONFIG_ENABLE_MEMORY_DEBUG is not set
+CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1024
+CONFIG_FREERTOS_ISR_STACKSIZE=1536
+# CONFIG_FREERTOS_LEGACY_HOOKS is not set
+CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16
+# CONFIG_SUPPORT_STATIC_ALLOCATION is not set
+CONFIG_TIMER_TASK_PRIORITY=1
+CONFIG_TIMER_TASK_STACK_DEPTH=2048
+CONFIG_TIMER_QUEUE_LENGTH=10
+# CONFIG_FREERTOS_DEBUG_INTERNALS is not set
+
+#
+# Log output
+#
+# CONFIG_LOG_DEFAULT_LEVEL_NONE is not set
+# CONFIG_LOG_DEFAULT_LEVEL_ERROR is not set
+# CONFIG_LOG_DEFAULT_LEVEL_WARN is not set
+CONFIG_LOG_DEFAULT_LEVEL_INFO=y
+# CONFIG_LOG_DEFAULT_LEVEL_DEBUG is not set
+# CONFIG_LOG_DEFAULT_LEVEL_VERBOSE is not set
+CONFIG_LOG_DEFAULT_LEVEL=3
+CONFIG_LOG_COLORS=y
+
+#
+# LWIP
+#
+# CONFIG_L2_TO_L3_COPY is not set
+CONFIG_LWIP_MAX_SOCKETS=10
+CONFIG_LWIP_THREAD_LOCAL_STORAGE_INDEX=0
+# CONFIG_LWIP_SO_REUSE is not set
+# CONFIG_LWIP_SO_RCVBUF is not set
+CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1
+# CONFIG_LWIP_IP_FRAG is not set
+# CONFIG_LWIP_IP_REASSEMBLY is not set
+
+#
+# TCP
+#
+CONFIG_TCP_MAXRTX=12
+CONFIG_TCP_SYNMAXRTX=6
+CONFIG_TCP_MSS=1436
+CONFIG_TCP_SND_BUF_DEFAULT=5744
+CONFIG_TCP_WND_DEFAULT=5744
+CONFIG_TCP_RECVMBOX_SIZE=6
+CONFIG_TCP_QUEUE_OOSEQ=y
+CONFIG_TCP_OVERSIZE_MSS=y
+# CONFIG_TCP_OVERSIZE_QUARTER_MSS is not set
+# CONFIG_TCP_OVERSIZE_DISABLE is not set
+
+#
+# UDP
+#
+CONFIG_UDP_RECVMBOX_SIZE=6
+CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y
+CONFIG_TCPIP_TASK_STACK_SIZE=2560
+# CONFIG_PPP_SUPPORT is not set
+
+#
+# ICMP
+#
+# CONFIG_LWIP_MULTICAST_PING is not set
+# CONFIG_LWIP_BROADCAST_PING is not set
+
+#
+# mbedTLS
+#
+CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384
+# CONFIG_MBEDTLS_DEBUG is not set
+CONFIG_MBEDTLS_HARDWARE_AES=y
+CONFIG_MBEDTLS_HARDWARE_MPI=y
+CONFIG_MBEDTLS_MPI_USE_INTERRUPT=y
+CONFIG_MBEDTLS_HARDWARE_SHA=y
+CONFIG_MBEDTLS_HAVE_TIME=y
+# CONFIG_MBEDTLS_HAVE_TIME_DATE is not set
+CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y
+# CONFIG_MBEDTLS_TLS_SERVER_ONLY is not set
+# CONFIG_MBEDTLS_TLS_CLIENT_ONLY is not set
+# CONFIG_MBEDTLS_TLS_DISABLED is not set
+CONFIG_MBEDTLS_TLS_SERVER=y
+CONFIG_MBEDTLS_TLS_CLIENT=y
+CONFIG_MBEDTLS_TLS_ENABLED=y
+
+#
+# TLS Key Exchange Methods
+#
+# CONFIG_MBEDTLS_PSK_MODES is not set
+CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y
+CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y
+CONFIG_MBEDTLS_SSL_RENEGOTIATION=y
+# CONFIG_MBEDTLS_SSL_PROTO_SSL3 is not set
+CONFIG_MBEDTLS_SSL_PROTO_TLS1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y
+CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y
+# CONFIG_MBEDTLS_SSL_PROTO_DTLS is not set
+CONFIG_MBEDTLS_SSL_ALPN=y
+CONFIG_MBEDTLS_SSL_SESSION_TICKETS=y
+
+#
+# Symmetric Ciphers
+#
+CONFIG_MBEDTLS_AES_C=y
+# CONFIG_MBEDTLS_CAMELLIA_C is not set
+# CONFIG_MBEDTLS_DES_C is not set
+CONFIG_MBEDTLS_RC4_DISABLED=y
+# CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT is not set
+# CONFIG_MBEDTLS_RC4_ENABLED is not set
+# CONFIG_MBEDTLS_BLOWFISH_C is not set
+# CONFIG_MBEDTLS_XTEA_C is not set
+CONFIG_MBEDTLS_CCM_C=y
+CONFIG_MBEDTLS_GCM_C=y
+# CONFIG_MBEDTLS_RIPEMD160_C is not set
+
+#
+# Certificates
+#
+CONFIG_MBEDTLS_PEM_PARSE_C=y
+CONFIG_MBEDTLS_PEM_WRITE_C=y
+CONFIG_MBEDTLS_X509_CRL_PARSE_C=y
+CONFIG_MBEDTLS_X509_CSR_PARSE_C=y
+CONFIG_MBEDTLS_ECP_C=y
+CONFIG_MBEDTLS_ECDH_C=y
+CONFIG_MBEDTLS_ECDSA_C=y
+CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y
+CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y
+CONFIG_MBEDTLS_ECP_NIST_OPTIM=y
+
+#
+# OpenSSL
+#
+# CONFIG_OPENSSL_DEBUG is not set
+CONFIG_OPENSSL_ASSERT_DO_NOTHING=y
+# CONFIG_OPENSSL_ASSERT_EXIT is not set
+
+#
+# SPI Flash driver
+#
+# CONFIG_SPI_FLASH_ENABLE_COUNTERS is not set
+CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y
+
+#
+# tcpip adapter
+#
+CONFIG_IP_LOST_TIMER_INTERVAL=120
+
+#
+# Wear Levelling
+#
+# CONFIG_WL_SECTOR_SIZE_512 is not set
+CONFIG_WL_SECTOR_SIZE_4096=y
+CONFIG_WL_SECTOR_SIZE=4096
diff --git a/Jennic/I2Cdev/I2Cdev.c b/Jennic/I2Cdev/I2Cdev.c
new file mode 100644
index 00000000..c7619c92
--- /dev/null
+++ b/Jennic/I2Cdev/I2Cdev.c
@@ -0,0 +1,882 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+// 24/06/2015 by Grégoire Surrel 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+//     2015-06-24 - ported to Jennic peripheral library from PIC18 code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2015 Grégoire Surrel
+
+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 "I2Cdev.h"
+#include 
+#include 
+#include "os.h"
+#include "os_gen.h"
+#include "dbg.h"
+#include "dbg_uart.h"
+
+//#define I2CDEV_SERIAL_DEBUG
+
+#define COMMAND_WRITE FALSE
+#define COMMAND_READ TRUE
+
+#define SEND_ACK E_AHI_SI_SEND_ACK
+#define SEND_NACK E_AHI_SI_SEND_NACK
+#define SEND_CONTINUE E_AHI_SI_NO_STOP_BIT
+#define SEND_STOP E_AHI_SI_STOP_BIT
+
+// Helper variables
+bool_t I2Cdev_enabled = FALSE;//Tracks whether the I2C interface is enabled or not
+
+bool_t debug = FALSE;
+uint8 i2cdev_debug_depth = 0;
+
+// Helper functions
+
+/** Active wait .
+ * @return Status of wait (timeout or succeed)
+ */
+bool_t IdleI2C() {
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth++; int z; for(z=0;z 010 shifted
+	uint8 count, b;
+	if ((count = I2Cdev_readByte(devAddr, regAddr, &b)) != 0) {
+		uint8 mask = ((1 << length) - 1) << (bitStart - length + 1);
+		b &= mask;
+		b >>= (bitStart - length + 1);
+		*data = b;
+	}
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth--;
+#endif
+
+	return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8 I2Cdev_readBitsW(uint8 devAddr, uint8 regAddr, uint8 bitStart, uint8 length, uint16 *data) {
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth++; int z; for(z=0;z 010 shifted
+	uint8 count;
+	uint16 w;
+	if ((count = I2Cdev_readWord(devAddr, regAddr, &w)) != 0) {
+		uint16 mask = ((1 << length) - 1) << (bitStart - length + 1);
+		w &= mask;
+		w >>= (bitStart - length + 1);
+		*data = w;
+	}
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth--;
+#endif
+
+	return count;
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (TRUE = success)
+ */
+uint8 I2Cdev_writeBytes(uint8 devAddr, uint8 regAddr, uint8 length, uint8* data) {
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth++; int z; for(z=0;z> 8, SEND_CONTINUE);
+
+		// Send LSB
+		if (count == length - 1) {
+			// Stop
+			WriteI2C(data[count] & 0xFF, SEND_STOP);
+		} else {
+			// continue
+			WriteI2C(data[count] & 0xFF, SEND_CONTINUE);
+		}
+	}
+
+	// Stop I2C
+	StopI2C();
+
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth--;
+#endif
+
+	return TRUE;
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (TRUE = success)
+ */
+uint8 I2Cdev_writeWord(uint8 devAddr, uint8 regAddr, uint16 data) {
+
+#ifdef I2CDEV_SERIAL_DEBUG
+	i2cdev_debug_depth++; int z; for(z=0;z
+// 11/28/2014 by Marton Sebok 
+// 24/06/2015 by Grégoire Surrel 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2015 Grégoire Surrel
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+
+int8 I2Cdev_readBit(uint8 devAddr, uint8 regAddr, uint8 bitNum, uint8 *data);
+int8 I2Cdev_readBitW(uint8 devAddr, uint8 regAddr, uint8 bitNum, uint16 *data);
+int8 I2Cdev_readBits(uint8 devAddr, uint8 regAddr, uint8 bitStart, uint8 length, uint8 *data);
+int8 I2Cdev_readBitsW(uint8 devAddr, uint8 regAddr, uint8 bitStart, uint8 length, uint16 *data);
+int8 I2Cdev_readByte(uint8 devAddr, uint8 regAddr, uint8 *data);
+int8 I2Cdev_readWord(uint8 devAddr, uint8 regAddr, uint16 *data);
+int8 I2Cdev_readBytes(uint8 devAddr, uint8 regAddr, uint8 length, uint8 *data);
+int8 I2Cdev_readWords(uint8 devAddr, uint8 regAddr, uint8 length, uint16 *data);
+
+uint8 I2Cdev_writeBit(uint8 devAddr, uint8 regAddr, uint8 bitNum, uint8 data);
+uint8 I2Cdev_writeBitW(uint8 devAddr, uint8 regAddr, uint8 bitNum, uint16 data);
+uint8 I2Cdev_writeBits(uint8 devAddr, uint8 regAddr, uint8 bitStart, uint8 length, uint8 data);
+uint8 I2Cdev_writeBitsW(uint8 devAddr, uint8 regAddr, uint8 bitStart, uint8 length, uint16 data);
+uint8 I2Cdev_writeByte(uint8 devAddr, uint8 regAddr, uint8 data);
+uint8 I2Cdev_writeWord(uint8 devAddr, uint8 regAddr, uint16 data);
+uint8 I2Cdev_writeBytes(uint8 devAddr, uint8 regAddr, uint8 length, uint8 *data);
+uint8 I2Cdev_writeWords(uint8 devAddr, uint8 regAddr, uint8 length, uint16 *data);
+
+#endif /* _I2CDEV_H_ */
diff --git a/Jennic/MPU6050/MPU6050.c b/Jennic/MPU6050/MPU6050.c
new file mode 100644
index 00000000..dea18889
--- /dev/null
+++ b/Jennic/MPU6050/MPU6050.c
@@ -0,0 +1,3422 @@
+// 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 
+// 11/28/2014 by Marton Sebok 
+// 24/06/2015 by Grégoire Surrel 
+// 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
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2015 Grégoire Surrel
+
+ 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.
+ ===============================================
+ */
+
+#if defined (__GNUC__) && defined (__AVR__)
+#include 
+#else
+#include 
+#define PROGMEM /* empty */
+#define pgm_read_byte(x) (*(x))
+#define pgm_read_word(x) (*(x))
+#define pgm_read_float(x) (*(x))
+#endif
+
+#include "MPU6050.h"
+
+#include "dbg.h"
+#include "dbg_uart.h"
+
+// Dirty dirty code!
+uint8 MPU6050_devAddr = 0;
+uint8 MPU6050_buffer[14];
+
+// Helper functions
+int8 memcmpOwn(const void *Ptr1, const void *Ptr2, size_t Count) {
+	int8 v = 0;
+	char *p1 = (char *) Ptr1;
+	char *p2 = (char *) Ptr2;
+
+	while (Count-- > 0 && v == 0) {
+		v = *(p1++) - *(p2++);
+	}
+
+	return v;
+}
+
+/** Power on and prepare for general usage.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ * 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(uint8 address) {
+	DBG_vPrintf(TRACE_APP, "\tSet address\n");
+	MPU6050_devAddr = address;
+	DBG_vPrintf(TRACE_APP, "\tSet clock source to XGyro\n");
+	MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+	DBG_vPrintf(TRACE_APP, "\tSet gyro range to 250�/s\n");
+	MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+	DBG_vPrintf(TRACE_APP, "\tSet accel range to 2G\n");
+	MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+	DBG_vPrintf(TRACE_APP, "\tSet sleep disable\n");
+	MPU6050_setSleepEnabled(FALSE); // thanks to Jack Elston for pointing this one out!
+	DBG_vPrintf(TRACE_APP, "\tSet filters mode 5: Acc {10Hz | 13.8ms} Gyr {10Hz | 13.4ms} Sampling 1kHz\n");
+	MPU6050_setDLPFMode(5);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, FALSE otherwise
+ */
+uint8 MPU6050_testConnection() {
+	return MPU6050_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 MPU6050_getAuxVDDIOLevel() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_YG_OFFS_TC,
+			MPU6050_TC_PWR_MODE_BIT, MPU6050_buffer);
+	return MPU6050_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 level) {
+	I2Cdev_writeBit(MPU6050_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 MPU6050_getRate() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_SMPLRT_DIV, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Set gyroscope sample rate divider.
+ * @param rate New sample rate divider
+ * @see getRate()
+ * @see MPU6050_RA_SMPLRT_DIV
+ */
+void MPU6050_setRate(uint8 rate) {
+	I2Cdev_writeByte(MPU6050_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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8 MPU6050_getExternalFrameSync() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_CONFIG,
+			MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Set external FSYNC configuration.
+ * @see getExternalFrameSync()
+ * @see MPU6050_RA_CONFIG
+ * @param sync New FSYNC configuration value
+ */
+void MPU6050_setExternalFrameSync(uint8 sync) {
+	I2Cdev_writeBits(MPU6050_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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @return DLFP configuration
+ * @see MPU6050_RA_CONFIG
+ * @see MPU6050_CFG_DLPF_CFG_BIT
+ * @see MPU6050_CFG_DLPF_CFG_LENGTH
+ */
+uint8 MPU6050_getDLPFMode() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_CONFIG,
+			MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 mode) {
+	I2Cdev_writeBits(MPU6050_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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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 MPU6050_getFullScaleGyroRange() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_GYRO_CONFIG,
+			MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 range) {
+	I2Cdev_writeBits(MPU6050_devAddr, MPU6050_RA_GYRO_CONFIG,
+			MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// ACCEL_CONFIG register
+
+/** Get self-test enabled setting for accelerometer X axis.
+ * @return Self-test enabled value
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8 MPU6050_getAccelXSelfTest() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_ACCEL_CONFIG,
+			MPU6050_ACONFIG_XA_ST_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getAccelYSelfTest() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_ACCEL_CONFIG,
+			MPU6050_ACONFIG_YA_ST_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getAccelZSelfTest() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_ACCEL_CONFIG,
+			MPU6050_ACONFIG_ZA_ST_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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 MPU6050_getFullScaleAccelRange() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_ACCEL_CONFIG,
+			MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Set full-scale accelerometer range.
+ * @param range New full-scale accelerometer range setting
+ * @see getFullScaleAccelRange()
+ */
+void MPU6050_setFullScaleAccelRange(uint8 range) {
+	I2Cdev_writeBits(MPU6050_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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8 MPU6050_getDHPFMode() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_ACCEL_CONFIG,
+			MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 bandwidth) {
+	I2Cdev_writeBits(MPU6050_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 MPU6050_getFreefallDetectionThreshold() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_FF_THR, MPU6050_buffer);
+	return MPU6050_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 threshold) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getFreefallDetectionDuration() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_FF_DUR, MPU6050_buffer);
+	return MPU6050_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 duration) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getMotionDetectionThreshold() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_MOT_THR, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Set free-fall event acceleration threshold.
+ * @param threshold New motion detection acceleration threshold value (LSB = 2mg)
+ * @see getMotionDetectionThreshold()
+ * @see MPU6050_RA_MOT_THR
+ */
+void MPU6050_setMotionDetectionThreshold(uint8 threshold) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getMotionDetectionDuration() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_MOT_DUR, MPU6050_buffer);
+	return MPU6050_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 duration) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getZeroMotionDetectionThreshold() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_ZRMOT_THR, MPU6050_buffer);
+	return MPU6050_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 threshold) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getZeroMotionDetectionDuration() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_ZRMOT_DUR, MPU6050_buffer);
+	return MPU6050_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 duration) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getTempFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN,
+			MPU6050_TEMP_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getXGyroFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT,
+			MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getYGyroFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT,
+			MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getZGyroFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT,
+			MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getAccelFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN,
+			MPU6050_ACCEL_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getSlave2FIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN,
+			MPU6050_SLV2_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getSlave1FIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN,
+			MPU6050_SLV1_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+uint8 MPU6050_getSlave0FIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_FIFO_EN,
+			MPU6050_SLV0_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getMultiMasterEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_CTRL,
+			MPU6050_MULT_MST_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getWaitForExternalSensorEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_CTRL,
+			MPU6050_WAIT_FOR_ES_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+uint8 MPU6050_getSlave3FIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_CTRL,
+			MPU6050_SLV_3_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getSlaveReadWriteTransitionEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_CTRL,
+			MPU6050_I2C_MST_P_NSR_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8 MPU6050_getMasterClockSpeed() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_I2C_MST_CTRL,
+			MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Set I2C master clock speed.
+ * @reparam speed Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050_setMasterClockSpeed(uint8 speed) {
+	I2Cdev_writeBits(MPU6050_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 MPU6050_getSlaveAddress(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3,
+			MPU6050_buffer);
+	return MPU6050_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 num, uint8 address) {
+	if (num > 3)
+		return;
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getSlaveRegister(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3,
+			MPU6050_buffer);
+	return MPU6050_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 num, uint8 reg) {
+	if (num > 3)
+		return;
+	I2Cdev_writeByte(MPU6050_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
+ */
+uint8 MPU6050_getSlaveEnabled(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+			MPU6050_I2C_SLV_EN_BIT, MPU6050_buffer);
+	return MPU6050_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 num, bool enabled) {
+	if (num > 3)
+		return;
+	I2Cdev_writeBit(MPU6050_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
+ */
+uint8 MPU6050_getSlaveWordByteSwap(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+			MPU6050_I2C_SLV_BYTE_SW_BIT, MPU6050_buffer);
+	return MPU6050_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 num, bool enabled) {
+	if (num > 3)
+		return;
+	I2Cdev_writeBit(MPU6050_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
+ */
+uint8 MPU6050_getSlaveWriteMode(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+			MPU6050_I2C_SLV_REG_DIS_BIT, MPU6050_buffer);
+	return MPU6050_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 num, bool mode) {
+	if (num > 3)
+		return;
+	I2Cdev_writeBit(MPU6050_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
+ */
+uint8 MPU6050_getSlaveWordGroupOffset(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+			MPU6050_I2C_SLV_GRP_BIT, MPU6050_buffer);
+	return MPU6050_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 num, bool enabled) {
+	if (num > 3)
+		return;
+	I2Cdev_writeBit(MPU6050_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 MPU6050_getSlaveDataLength(uint8 num) {
+	if (num > 3)
+		return 0;
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+			MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, MPU6050_buffer);
+	return MPU6050_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 num, uint8 length) {
+	if (num > 3)
+		return;
+	I2Cdev_writeBits(MPU6050_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 MPU6050_getSlave4Address() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_ADDR, MPU6050_buffer);
+	return MPU6050_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 address) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getSlave4Register() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_REG, MPU6050_buffer);
+	return MPU6050_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 reg) {
+	I2Cdev_writeByte(MPU6050_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 data) {
+	I2Cdev_writeByte(MPU6050_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
+ */
+uint8 MPU6050_getSlave4Enabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+			MPU6050_I2C_SLV4_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getSlave4InterruptEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+			MPU6050_I2C_SLV4_INT_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getSlave4WriteMode() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+			MPU6050_I2C_SLV4_REG_DIS_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_getSlave4MasterDelay() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+			MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 delay) {
+	I2Cdev_writeBits(MPU6050_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 MPU6050_getSlate4InputByte() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_I2C_SLV4_DI, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getPassthroughStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_PASS_THROUGH_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave4IsDone() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV4_DONE_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getLostArbitration() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_LOST_ARB_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave4Nack() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV4_NACK_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave3Nack() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV3_NACK_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave2Nack() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV2_NACK_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave1Nack() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV1_NACK_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getSlave0Nack() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_STATUS,
+			MPU6050_MST_I2C_SLV0_NACK_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getInterruptMode() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_INT_LEVEL_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getInterruptDrive() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_INT_OPEN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getInterruptLatch() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_LATCH_INT_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getInterruptLatchClear() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_INT_RD_CLEAR_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getFSyncInterruptLevel() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getFSyncInterruptEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_FSYNC_INT_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getI2CBypassEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_I2C_BYPASS_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getClockOutputEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_PIN_CFG,
+			MPU6050_INTCFG_CLKOUT_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_getIntEnabled() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_INT_ENABLE, MPU6050_buffer);
+	return MPU6050_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 enabled) {
+	I2Cdev_writeByte(MPU6050_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
+ **/
+uint8 MPU6050_getIntFreefallEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_FF_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ **/
+uint8 MPU6050_getIntMotionEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_MOT_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ **/
+uint8 MPU6050_getIntZeroMotionEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_ZMOT_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ **/
+uint8 MPU6050_getIntFIFOBufferOverflowEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_FIFO_OFLOW_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ **/
+uint8 MPU6050_getIntI2CMasterEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_I2C_MST_INT_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getIntDataReadyEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_DATA_RDY_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_getIntStatus() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_INT_STATUS, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntFreefallStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_FF_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntMotionStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_MOT_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntZeroMotionStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_ZMOT_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntFIFOBufferOverflowStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_FIFO_OFLOW_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntI2CMasterStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_I2C_MST_INT_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getIntDataReadyStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_DATA_RDY_BIT, MPU6050_buffer);
+	return MPU6050_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* ax, int16* ay, int16* az, int16* gx, int16* gy,
+		int16* gz, int16* mx, int16* my, int16* mz) {
+	MPU6050_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* ax, int16* ay, int16* az, int16* gx, int16* gy,
+		int16* gz) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ACCEL_XOUT_H, 14,
+			MPU6050_buffer);
+	*ax = (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+	*ay = (((int16) MPU6050_buffer[2]) << 8) | MPU6050_buffer[3];
+	*az = (((int16) MPU6050_buffer[4]) << 8) | MPU6050_buffer[5];
+	*gx = (((int16) MPU6050_buffer[8]) << 8) | MPU6050_buffer[9];
+	*gy = (((int16) MPU6050_buffer[10]) << 8) | MPU6050_buffer[11];
+	*gz = (((int16) MPU6050_buffer[12]) << 8) | MPU6050_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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* x, int16* y, int16* z) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ACCEL_XOUT_H, 6,
+			MPU6050_buffer);
+	*x = (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+	*y = (((int16) MPU6050_buffer[2]) << 8) | MPU6050_buffer[3];
+	*z = (((int16) MPU6050_buffer[4]) << 8) | MPU6050_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 MPU6050_getAccelerationX() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ACCEL_XOUT_H, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getAccelerationY() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ACCEL_YOUT_H, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getAccelerationZ() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getTemperature() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_TEMP_OUT_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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* x, int16* y, int16* z) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_GYRO_XOUT_H, 6, MPU6050_buffer);
+	*x = (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+	*y = (((int16) MPU6050_buffer[2]) << 8) | MPU6050_buffer[3];
+	*z = (((int16) MPU6050_buffer[4]) << 8) | MPU6050_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 MPU6050_getRotationX() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_GYRO_XOUT_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getRotationY() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_GYRO_YOUT_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getRotationZ() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getExternalSensorByte(int position) {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position,
+			MPU6050_buffer);
+	return MPU6050_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 MPU6050_getExternalSensorWord(int position) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position,
+			2, MPU6050_buffer);
+	return (((uint16) MPU6050_buffer[0]) << 8) | MPU6050_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 MPU6050_getExternalSensorDWord(int position) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position,
+			4, MPU6050_buffer);
+	return (((uint32) MPU6050_buffer[0]) << 24) | (((uint32) MPU6050_buffer[1])
+			<< 16) | (((uint16) MPU6050_buffer[2]) << 8) | MPU6050_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 MPU6050_getMotionStatus() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getXNegMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_XNEG_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getXPosMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_XPOS_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getYNegMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_YNEG_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getYPosMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_YPOS_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getZNegMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_ZNEG_BIT, MPU6050_buffer);
+	return MPU6050_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
+ */
+uint8 MPU6050_getZPosMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_ZPOS_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+/** Get zero motion detection interrupt status.
+ * @return Motion detection status
+ * @see MPU6050_RA_MOT_DETECT_STATUS
+ * @see MPU6050_MOTION_MOT_ZRMOT_BIT
+ */
+uint8 MPU6050_getZeroMotionDetected() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+			MPU6050_MOTION_MOT_ZRMOT_BIT, MPU6050_buffer);
+	return MPU6050_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 num, uint8 data) {
+	if (num > 3)
+		return;
+	I2Cdev_writeByte(MPU6050_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
+ */
+uint8 MPU6050_getExternalShadowDelayEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
+			MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getSlaveDelayEnabled(uint8 num) {
+	// MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
+	if (num > 4)
+		return 0;
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num,
+			MPU6050_buffer);
+	return MPU6050_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 num, bool enabled) {
+	I2Cdev_writeBit(MPU6050_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(MPU6050_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(MPU6050_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(MPU6050_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 MPU6050_getAccelerometerPowerOnDelay() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+			MPU6050_DETECT_ACCEL_ON_DELAY_BIT,
+			MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, MPU6050_buffer);
+	return MPU6050_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 delay) {
+	I2Cdev_writeBits(MPU6050_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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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 MPU6050_getFreefallDetectionCounterDecrement() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+			MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 decrement) {
+	I2Cdev_writeBits(MPU6050_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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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 MPU6050_getMotionDetectionCounterDecrement() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+			MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 decrement) {
+	I2Cdev_writeBits(MPU6050_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 MPU6050_buffer is disabled. The FIFO MPU6050_buffer
+ * cannot be written to or read from while disabled. The FIFO MPU6050_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
+ */
+uint8 MPU6050_getFIFOEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_FIFO_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getI2CMasterModeEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_I2C_MST_EN_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO MPU6050_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(MPU6050_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(MPU6050_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(MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getSleepEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_1,
+			MPU6050_PWR1_SLEEP_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getWakeCycleEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_1,
+			MPU6050_PWR1_CYCLE_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getTempSensorEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_1,
+			MPU6050_PWR1_TEMP_DIS_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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 MPU6050_getClockSource() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_1,
+			MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_buffer);
+	return MPU6050_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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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 source) {
+	I2Cdev_writeBits(MPU6050_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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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 MPU6050_getWakeFrequency() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
+			MPU6050_buffer);
+	return MPU6050_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 frequency) {
+	I2Cdev_writeBits(MPU6050_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
+ */
+uint8 MPU6050_getStandbyXAccelEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_XA_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getStandbyYAccelEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_YA_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getStandbyZAccelEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_ZA_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getStandbyXGyroEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_XG_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getStandbyYGyroEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_YG_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_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
+ */
+uint8 MPU6050_getStandbyZGyroEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_ZG_BIT, MPU6050_buffer);
+	return MPU6050_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(MPU6050_devAddr, MPU6050_RA_PWR_MGMT_2,
+			MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO MPU6050_buffer size.
+ * This value indicates the number of bytes stored in the FIFO MPU6050_buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO MPU6050_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 MPU6050_buffer size
+ */
+uint16 MPU6050_getFIFOCount() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_FIFO_COUNTH, 2, MPU6050_buffer);
+	return (((uint16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO MPU6050_buffer.
+ * This register is used to read and write data from the FIFO MPU6050_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 MPU6050_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 MPU6050_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 MPU6050_buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO MPU6050_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 MPU6050_buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO MPU6050_buffer
+ */
+uint8 MPU6050_getFIFOByte() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_FIFO_R_W, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8 *data, uint8 length) {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO MPU6050_buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_setFIFOByte(uint8 data) {
+	I2Cdev_writeByte(MPU6050_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 MPU6050_getDeviceID() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT,
+			MPU6050_WHO_AM_I_LENGTH, MPU6050_buffer);
+	return MPU6050_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 id) {
+	I2Cdev_writeBits(MPU6050_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 MPU6050_getOTPBankValid() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_XG_OFFS_TC,
+			MPU6050_TC_OTP_BNK_VLD_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setOTPBankValid(bool enabled) {
+	I2Cdev_writeBit(MPU6050_devAddr, MPU6050_RA_XG_OFFS_TC,
+			MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8 MPU6050_getXGyroOffsetTC() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_XG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setXGyroOffsetTC(int8 offset) {
+	I2Cdev_writeBits(MPU6050_devAddr, MPU6050_RA_XG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// YG_OFFS_TC register
+
+int8 MPU6050_getYGyroOffsetTC() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_YG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setYGyroOffsetTC(int8 offset) {
+	I2Cdev_writeBits(MPU6050_devAddr, MPU6050_RA_YG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// ZG_OFFS_TC register
+
+int8 MPU6050_getZGyroOffsetTC() {
+	I2Cdev_readBits(MPU6050_devAddr, MPU6050_RA_ZG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setZGyroOffsetTC(int8 offset) {
+	I2Cdev_writeBits(MPU6050_devAddr, MPU6050_RA_ZG_OFFS_TC,
+			MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, offset);
+}
+
+// X_FINE_GAIN register
+
+int8 MPU6050_getXFineGain() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_X_FINE_GAIN, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setXFineGain(int8 gain) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8 MPU6050_getYFineGain() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_Y_FINE_GAIN, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setYFineGain(int8 gain) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8 MPU6050_getZFineGain() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_Z_FINE_GAIN, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setZFineGain(int8 gain) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16 MPU6050_getXAccelOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_XA_OFFS_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setXAccelOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16 MPU6050_getYAccelOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_YA_OFFS_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setYAccelOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16 MPU6050_getZAccelOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ZA_OFFS_H, 2, MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setZAccelOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16 MPU6050_getXGyroOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_XG_OFFS_USRH, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setXGyroOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16 MPU6050_getYGyroOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_YG_OFFS_USRH, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setYGyroOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16 MPU6050_getZGyroOffset() {
+	I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_ZG_OFFS_USRH, 2,
+			MPU6050_buffer);
+	return (((int16) MPU6050_buffer[0]) << 8) | MPU6050_buffer[1];
+}
+void MPU6050_setZGyroOffset(int16 offset) {
+	I2Cdev_writeWord(MPU6050_devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+uint8 MPU6050_getIntPLLReadyEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_PLL_RDY_INT_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setIntPLLReadyEnabled(bool enabled) {
+	I2Cdev_writeBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+uint8 MPU6050_getIntDMPEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_DMP_INT_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setIntDMPEnabled(bool enabled) {
+	I2Cdev_writeBit(MPU6050_devAddr, MPU6050_RA_INT_ENABLE,
+			MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+uint8 MPU6050_getDMPInt5Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_5_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getDMPInt4Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_4_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getDMPInt3Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_3_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getDMPInt2Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_2_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getDMPInt1Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_1_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getDMPInt0Status() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_DMP_INT_STATUS,
+			MPU6050_DMPINT_0_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+uint8 MPU6050_getIntPLLReadyStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_PLL_RDY_INT_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+uint8 MPU6050_getIntDMPStatus() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_INT_STATUS,
+			MPU6050_INTERRUPT_DMP_INT_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+uint8 MPU6050_getDMPEnabled() {
+	I2Cdev_readBit(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_DMP_EN_BIT, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setDMPEnabled(bool enabled) {
+	I2Cdev_writeBit(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_resetDMP() {
+	I2Cdev_writeBit(MPU6050_devAddr, MPU6050_RA_USER_CTRL,
+			MPU6050_USERCTRL_DMP_RESET_BIT, TRUE);
+}
+
+// BANK_SEL register
+
+void MPU6050_setMemoryBank(uint8 bank, bool prefetchEnabled, bool userBank) {
+	bank &= 0x1F;
+	if (userBank)
+		bank |= 0x20;
+	if (prefetchEnabled)
+		bank |= 0x40;
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_setMemoryStartAddress(uint8 address) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8 MPU6050_readMemoryByte() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_MEM_R_W, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_writeMemoryByte(uint8 data) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_readMemoryBlock(uint8 *data, uint16 dataSize, uint8 bank,
+		uint8 address) {
+	MPU6050_setMemoryBank(bank, FALSE, FALSE);
+	MPU6050_setMemoryStartAddress(address);
+	uint8 chunkSize;
+	uint16 i;
+	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;
+
+		// read the chunk of data as specified
+		I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_MEM_R_W, chunkSize, data
+				+ i);
+
+		// increase byte index by [chunkSize]
+		i += chunkSize;
+
+		// uint8 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++;
+			MPU6050_setMemoryBank(bank, FALSE, FALSE);
+			MPU6050_setMemoryStartAddress(address);
+		}
+	}
+}
+uint8 MPU6050_writeMemoryBlock(const uint8 *data, uint16 dataSize, uint8 bank,
+		uint8 address, bool verify, bool useProgMem) {
+	MPU6050_setMemoryBank(bank, FALSE, FALSE);
+	MPU6050_setMemoryStartAddress(address);
+	uint8 chunkSize;
+	uint8 verifyBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
+	uint8 progBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
+	uint16 i;
+	uint8 j;
+	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 (TRUE || useProgMem) { // Else case broken, always taking the first condition to copy the data
+			// 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
+			// BROKEN, let's simply copy the data as using the loop before progBuffer = (uint8 *)data + i;
+		}
+
+		I2Cdev_writeBytes(MPU6050_devAddr, MPU6050_RA_MEM_R_W, chunkSize,
+				progBuffer);
+
+		// verify data if needed
+		if (verify && verifyBuffer) {
+			MPU6050_setMemoryBank(bank, FALSE, FALSE);
+			MPU6050_setMemoryStartAddress(address);
+			I2Cdev_readBytes(MPU6050_devAddr, MPU6050_RA_MEM_R_W, chunkSize,
+					verifyBuffer);
+			if (memcmpOwn(progBuffer, verifyBuffer, chunkSize) != 0) {
+				/*Serial.print("Block write verification error, bank ");
+				 Serial.print(bank, DEC);
+				 Serial.print(", address ");
+				 Serial.print(address, DEC);
+				 Serial.print("!\nExpected:");
+				 for (j = 0; j < chunkSize; j++) {
+				 Serial.print(" 0x");
+				 if (progBuffer[j] < 16) Serial.print("0");
+				 Serial.print(progBuffer[j], HEX);
+				 }
+				 Serial.print("\nReceived:");
+				 for (uint8 j = 0; j < chunkSize; j++) {
+				 Serial.print(" 0x");
+				 if (verifyBuffer[i + j] < 16) Serial.print("0");
+				 Serial.print(verifyBuffer[i + j], HEX);
+				 }
+				 Serial.print("\n");*/
+				return FALSE; // uh oh.
+			}
+		}
+
+		// increase byte index by [chunkSize]
+		i += chunkSize;
+
+		// uint8 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++;
+			MPU6050_setMemoryBank(bank, FALSE, FALSE);
+			MPU6050_setMemoryStartAddress(address);
+		}
+	}
+	return TRUE;
+}
+uint8 MPU6050_writeProgMemoryBlock(const uint8 *data, uint16 dataSize,
+		uint8 bank, uint8 address, bool verify) {
+	return MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, TRUE);
+}
+uint8 MPU6050_writeDMPConfigurationSet(const uint8 *data, uint16 dataSize,
+		bool useProgMem) {
+	uint8 progBuffer[8], success, special;
+	uint16 i, j;
+
+	// config set data is a long string of blocks with the following structure:
+	// [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]]
+	uint8 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 (TRUE || useProgMem) { // Modified condition because of broken ELSE
+				if (sizeof(progBuffer) < length) { //progBuffer = (uint8 *)realloc(progBuffer, length);
+					DBG_vPrintf(TRACE_APP,
+							"MPU error on line 'if (sizeof(progBuffer) < length) {'\r\n");
+					return FALSE;
+				}
+				for (j = 0; j < length; j++)
+					progBuffer[j] = pgm_read_byte(data + i + j);
+			} else {
+				// BROKEN line progBuffer = (uint8 *)data + i;
+			}
+			success = MPU6050_writeMemoryBlock(progBuffer, length, bank,
+					offset, TRUE, FALSE);
+			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(MPU6050_devAddr, MPU6050_RA_INT_ENABLE, 0x32); // single operation
+
+				success = TRUE;
+			} else {
+				// unknown special command
+				success = FALSE;
+			}
+		}
+
+		if (!success) {
+			return FALSE; // uh oh
+		}
+	}
+
+	return TRUE;
+}
+uint8 MPU6050_writeProgDMPConfigurationSet(const uint8 *data, uint16 dataSize) {
+	return MPU6050_writeDMPConfigurationSet(data, dataSize, TRUE);
+}
+
+// DMP_CFG_1 register
+
+uint8 MPU6050_getDMPConfig1() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_DMP_CFG_1, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setDMPConfig1(uint8 config) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8 MPU6050_getDMPConfig2() {
+	I2Cdev_readByte(MPU6050_devAddr, MPU6050_RA_DMP_CFG_2, MPU6050_buffer);
+	return MPU6050_buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8 config) {
+	I2Cdev_writeByte(MPU6050_devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
diff --git a/Jennic/MPU6050/MPU6050.h b/Jennic/MPU6050/MPU6050.h
new file mode 100644
index 00000000..11b65bbe
--- /dev/null
+++ b/Jennic/MPU6050/MPU6050.h
@@ -0,0 +1,982 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 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 "I2Cdev.h"
+
+#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_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_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
+
+// Helper functions
+int8 memcmpOwn(const void *Ptr1, const void *Ptr2, size_t Count);
+
+// note: DMP code memory blocks defined at end of header file
+void MPU6050_initialize(uint8 address);
+uint8 MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8 MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8 level);
+
+// SMPLRT_DIV register
+uint8 MPU6050_getRate();
+void MPU6050_setRate(uint8 rate);
+
+// CONFIG register
+uint8 MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8 sync);
+uint8 MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8 bandwidth);
+
+// GYRO_CONFIG register
+uint8 MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8 range);
+
+// ACCEL_CONFIG register
+uint8 MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+uint8 MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+uint8 MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8 MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8 range);
+uint8 MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8 mode);
+
+// FF_THR register
+uint8 MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8 threshold);
+
+// FF_DUR register
+uint8 MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8 duration);
+
+// MOT_THR register
+uint8 MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8 threshold);
+
+// MOT_DUR register
+uint8 MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8 duration);
+
+// ZRMOT_THR register
+uint8 MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8 threshold);
+
+// ZRMOT_DUR register
+uint8 MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8 duration);
+
+// FIFO_EN register
+uint8 MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+uint8 MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+uint8 MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+uint8 MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+uint8 MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+uint8 MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+uint8 MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+uint8 MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+uint8 MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+uint8 MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+uint8 MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+uint8 MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8 MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8 speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8 MPU6050_getSlaveAddress(uint8 num);
+void MPU6050_setSlaveAddress(uint8 num, uint8 address);
+uint8 MPU6050_getSlaveRegister(uint8 num);
+void MPU6050_setSlaveRegister(uint8 num, uint8 reg);
+uint8 MPU6050_getSlaveEnabled(uint8 num);
+void MPU6050_setSlaveEnabled(uint8 num, bool enabled);
+uint8 MPU6050_getSlaveWordByteSwap(uint8 num);
+void MPU6050_setSlaveWordByteSwap(uint8 num, bool enabled);
+uint8 MPU6050_getSlaveWriteMode(uint8 num);
+void MPU6050_setSlaveWriteMode(uint8 num, bool mode);
+uint8 MPU6050_getSlaveWordGroupOffset(uint8 num);
+void MPU6050_setSlaveWordGroupOffset(uint8 num, bool enabled);
+uint8 MPU6050_getSlaveDataLength(uint8 num);
+void MPU6050_setSlaveDataLength(uint8 num, uint8 length);
+
+// I2C_SLV* registers (Slave 4)
+uint8 MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8 address);
+uint8 MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8 reg);
+void MPU6050_setSlave4OutputByte(uint8 data);
+uint8 MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+uint8 MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+uint8 MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8 MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8 delay);
+uint8 MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+uint8 MPU6050_getPassthroughStatus();
+uint8 MPU6050_getSlave4IsDone();
+uint8 MPU6050_getLostArbitration();
+uint8 MPU6050_getSlave4Nack();
+uint8 MPU6050_getSlave3Nack();
+uint8 MPU6050_getSlave2Nack();
+uint8 MPU6050_getSlave1Nack();
+uint8 MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+uint8 MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+uint8 MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+uint8 MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+uint8 MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+uint8 MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+uint8 MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+uint8 MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+uint8 MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8 MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8 enabled);
+uint8 MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+uint8 MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+uint8 MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+uint8 MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+uint8 MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+uint8 MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8 MPU6050_getIntStatus();
+uint8 MPU6050_getIntFreefallStatus();
+uint8 MPU6050_getIntMotionStatus();
+uint8 MPU6050_getIntZeroMotionStatus();
+uint8 MPU6050_getIntFIFOBufferOverflowStatus();
+uint8 MPU6050_getIntI2CMasterStatus();
+uint8 MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16* ax, int16* ay, int16* az, int16* gx, int16* gy, int16* gz, int16* mx, int16* my, int16* mz);
+void MPU6050_getMotion6(int16* ax, int16* ay, int16* az, int16* gx, int16* gy, int16* gz);
+void MPU6050_getAcceleration(int16* x, int16* y, int16* z);
+int16 MPU6050_getAccelerationX();
+int16 MPU6050_getAccelerationY();
+int16 MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16 MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16* x, int16* y, int16* z);
+int16 MPU6050_getRotationX();
+int16 MPU6050_getRotationY();
+int16 MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8 MPU6050_getExternalSensorByte(int position);
+uint16 MPU6050_getExternalSensorWord(int position);
+uint32 MPU6050_getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+uint8 MPU6050_getMotionStatus();
+uint8 MPU6050_getXNegMotionDetected();
+uint8 MPU6050_getXPosMotionDetected();
+uint8 MPU6050_getYNegMotionDetected();
+uint8 MPU6050_getYPosMotionDetected();
+uint8 MPU6050_getZNegMotionDetected();
+uint8 MPU6050_getZPosMotionDetected();
+uint8 MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8 num, uint8 data);
+
+// I2C_MST_DELAY_CTRL register
+uint8 MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+uint8 MPU6050_getSlaveDelayEnabled(uint8 num);
+void MPU6050_setSlaveDelayEnabled(uint8 num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8 MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8 delay);
+uint8 MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8 decrement);
+uint8 MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8 decrement);
+
+// USER_CTRL register
+uint8 MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+uint8 MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+uint8 MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+uint8 MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+uint8 MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8 MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8 source);
+
+// PWR_MGMT_2 register
+uint8 MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8 frequency);
+uint8 MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+uint8 MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+uint8 MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+uint8 MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+uint8 MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+uint8 MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16 MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8 MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8 data);
+void MPU6050_getFIFOBytes(uint8 *data, uint8 length);
+
+// WHO_AM_I register
+uint8 MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8 id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8 MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8 MPU6050_getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8 offset);
+
+// YG_OFFS_TC register
+int8 MPU6050_getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8 offset);
+
+// ZG_OFFS_TC register
+int8 MPU6050_getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8 offset);
+
+// X_FINE_GAIN register
+int8 MPU6050_getXFineGain();
+void MPU6050_setXFineGain(int8 gain);
+
+// Y_FINE_GAIN register
+int8 MPU6050_getYFineGain();
+void MPU6050_setYFineGain(int8 gain);
+
+// Z_FINE_GAIN register
+int8 MPU6050_getZFineGain();
+void MPU6050_setZFineGain(int8 gain);
+
+// XA_OFFS_* registers
+int16 MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16 offset);
+
+// YA_OFFS_* register
+int16 MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16 offset);
+
+// ZA_OFFS_* register
+int16 MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16 offset);
+
+// XG_OFFS_USR* registers
+int16 MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16 offset);
+
+// YG_OFFS_USR* register
+int16 MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16 offset);
+
+// ZG_OFFS_USR* register
+int16 MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16 offset);
+
+// INT_ENABLE register (DMP functions)
+uint8 MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+uint8 MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+uint8 MPU6050_getDMPInt5Status();
+uint8 MPU6050_getDMPInt4Status();
+uint8 MPU6050_getDMPInt3Status();
+uint8 MPU6050_getDMPInt2Status();
+uint8 MPU6050_getDMPInt1Status();
+uint8 MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+uint8 MPU6050_getIntPLLReadyStatus();
+uint8 MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+uint8 MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8 bank, bool prefetchEnabled, bool userBank); // mandatory, false, false
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8 address);
+
+// MEM_R_W register
+uint8 MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8 data);
+void MPU6050_readMemoryBlock(uint8 *data, uint16 dataSize, uint8 bank, uint8 address); // mandatory, mandatory, 0, 0
+uint8 MPU6050_writeMemoryBlock(const uint8 *data, uint16 dataSize, uint8 bank, uint8 address, bool verify, bool useProgMem); // Mandatory, mandatory, 0, 0, true, false
+uint8 writeProgMemoryBlock(const uint8 *data, uint16 dataSize, uint8 bank, uint8 address, bool verify); // Mandatory, mandatory, 0, 0, true
+
+uint8 MPU6050_writeDMPConfigurationSet(const uint8 *data, uint16 dataSize, bool useProgMem); // Mandatory, mandatory, false
+uint8 MPU6050_writeProgDMPConfigurationSet(const uint8 *data, uint16 dataSize);
+
+// DMP_CFG_1 register
+uint8 MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8 config);
+
+// DMP_CFG_2 register
+uint8 MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8 config);
+
+// special methods for MotionApps 2.0 implementation
+#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
+#include "helper_3dmath.h"
+
+uint8 *MPU6050_dmpPacketBuffer;
+uint16 MPU6050_dmpPacketSize;
+
+uint8 MPU6050_dmpInitialize();
+bool MPU6050_dmpPacketAvailable();
+
+uint8 MPU6050_dmpSetFIFORate(uint8 fifoRate);
+uint8 MPU6050_dmpGetFIFORate();
+uint8 MPU6050_dmpGetSampleStepSizeMS();
+uint8 MPU6050_dmpGetSampleFrequency();
+int32 MPU6050_dmpDecodeTemperature(int8 tempReg);
+
+// Register callbacks after a packet of FIFO data is processed
+//uint8 dmpRegisterFIFORateProcess(inv_obj_func func, int16 priority);
+//uint8 dmpUnregisterFIFORateProcess(inv_obj_func func);
+uint8 MPU6050_dmpRunFIFORateProcesses();
+
+// Setup FIFO for various output
+uint8 MPU6050_dmpSendQuaternion(uint16 accuracy);
+uint8 MPU6050_dmpSendGyro(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendLinearAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendLinearAccelInWorld(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendControlData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendSensorData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendExternalSensorData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendGravity(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendPacketNumber(uint16 accuracy);
+uint8 MPU6050_dmpSendQuantizedAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendEIS(uint16 elements, uint16 accuracy);
+
+// Get Fixed Point data from FIFO
+uint8 MPU6050_dmpGetAccel32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetAccel16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetAccelVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuaternion32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuaternion16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuaternionQuat(Quaternion *q, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGet6AxisQuaternion32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGet6AxisQuaternion16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGet6AxisQuaternionQuat(Quaternion *q, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetRelativeQuaternion32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetRelativeQuaternion16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetRelativeQuaternionQuat(Quaternion *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyro32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyro16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpSetLinearAccelFilterCoefficient(float coef);
+uint8 MPU6050_dmpGetLinearAccel32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccel16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccelVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccelVecVec(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+uint8 MPU6050_dmpGetLinearAccelInWorld32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccelInWorld16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccelInWorldVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetLinearAccelInWorldVecQuat(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+uint8 MPU6050_dmpGetGyroAndAccelSensor32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroAndAccelSensor16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroAndAccelSensorVec(VectorInt16 *g, VectorInt16 *a, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroSensor32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroSensor16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGyroSensorVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetControlData(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetTemperature(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGravity32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGravity16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGravityVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetGravityVecQuat(VectorFloat *v, Quaternion *q);
+uint8 MPU6050_dmpGetUnquantizedAccel32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetUnquantizedAccel16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetUnquantizedAccelVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuantizedAccel32(int32 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuantizedAccel16(int16 *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuantizedAccelVec(VectorInt16 *v, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetExternalSensorData(int32 *data, uint16 size, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetEIS(int32 *data, const uint8* packet); // Default packet=0
+
+uint8 MPU6050_dmpGetEuler(float *data, Quaternion *q);
+uint8 MPU6050_dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+// Get Floating Point data from FIFO
+uint8 MPU6050_dmpGetAccelFloat(float *data, const uint8* packet); // Default packet=0
+uint8 MPU6050_dmpGetQuaternionFloat(float *data, const uint8* packet); // Default packet=0
+
+uint8 MPU6050_dmpProcessFIFOPacket(const unsigned char *dmpData);
+uint8 MPU6050_dmpReadAndProcessFIFOPacket(uint8 numPackets, uint8 *processed); // Default processed = NULL
+
+uint8 MPU6050_dmpSetFIFOProcessedCallback(void (*func) (void));
+
+uint8 MPU6050_dmpInitFIFOParam();
+uint8 MPU6050_dmpCloseFIFO();
+uint8 MPU6050_dmpSetGyroDataSource(uint8 source);
+uint8 MPU6050_dmpDecodeQuantizedAccel();
+uint32 MPU6050_dmpGetGyroSumOfSquare();
+uint32 MPU6050_dmpGetAccelSumOfSquare();
+void MPU6050_dmpOverrideQuaternion(long *q);
+uint16 MPU6050_dmpGetFIFOPacketSize();
+
+uint8 MPU6050_writeProgMemoryBlock(const uint8 *data, uint16 dataSize,
+		uint8 bank, uint8 address, bool verify);
+#endif
+
+// special methods for MotionApps 4.1 implementation
+#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
+uint8 *MPU6050_dmpPacketBuffer;
+uint16 MPU6050_dmpPacketSize;
+
+uint8 MPU6050_dmpInitialize();
+bool MPU6050_dmpPacketAvailable();
+
+uint8 MPU6050_dmpSetFIFORate(uint8 fifoRate);
+uint8 MPU6050_dmpGetFIFORate();
+uint8 MPU6050_dmpGetSampleStepSizeMS();
+uint8 MPU6050_dmpGetSampleFrequency();
+int32 MPU6050_dmpDecodeTemperature(int8 tempReg);
+
+// Register callbacks after a packet of FIFO data is processed
+//uint8 dmpRegisterFIFORateProcess(inv_obj_func func, int16 priority);
+//uint8 dmpUnregisterFIFORateProcess(inv_obj_func func);
+uint8 MPU6050_dmpRunFIFORateProcesses();
+
+// Setup FIFO for various output
+uint8 MPU6050_dmpSendQuaternion(uint16 accuracy);
+uint8 MPU6050_dmpSendGyro(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendLinearAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendLinearAccelInWorld(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendControlData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendSensorData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendExternalSensorData(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendGravity(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendPacketNumber(uint16 accuracy);
+uint8 MPU6050_dmpSendQuantizedAccel(uint16 elements, uint16 accuracy);
+uint8 MPU6050_dmpSendEIS(uint16 elements, uint16 accuracy);
+
+// Get Fixed Point data from FIFO
+uint8 MPU6050_dmpGetAccel(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetAccel(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetAccel(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuaternion(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuaternion(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuaternion(Quaternion *q, const uint8* packet=0);
+uint8 MPU6050_dmpGet6AxisQuaternion(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGet6AxisQuaternion(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGet6AxisQuaternion(Quaternion *q, const uint8* packet=0);
+uint8 MPU6050_dmpGetRelativeQuaternion(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetRelativeQuaternion(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetRelativeQuaternion(Quaternion *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyro(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyro(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyro(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetMag(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpSetLinearAccelFilterCoefficient(float coef);
+uint8 MPU6050_dmpGetLinearAccel(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccel(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccel(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+uint8 MPU6050_dmpGetLinearAccelInWorld(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccelInWorld(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+uint8 MPU6050_dmpGetGyroAndAccelSensor(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyroAndAccelSensor(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyroSensor(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyroSensor(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGyroSensor(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetControlData(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetTemperature(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGravity(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGravity(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetGravity(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetGravity(VectorFloat *v, Quaternion *q);
+uint8 MPU6050_dmpGetUnquantizedAccel(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetUnquantizedAccel(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetUnquantizedAccel(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuantizedAccel(int32 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuantizedAccel(int16 *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuantizedAccel(VectorInt16 *v, const uint8* packet=0);
+uint8 MPU6050_dmpGetExternalSensorData(int32 *data, uint16 size, const uint8* packet=0);
+uint8 MPU6050_dmpGetEIS(int32 *data, const uint8* packet=0);
+
+uint8 MPU6050_dmpGetEuler(float *data, Quaternion *q);
+uint8 MPU6050_dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+// Get Floating Point data from FIFO
+uint8 MPU6050_dmpGetAccelFloat(float *data, const uint8* packet=0);
+uint8 MPU6050_dmpGetQuaternionFloat(float *data, const uint8* packet=0);
+
+uint8 MPU6050_dmpProcessFIFOPacket(const unsigned char *dmpData);
+uint8 MPU6050_dmpReadAndProcessFIFOPacket(uint8 numPackets, uint8 *processed=NULL);
+
+uint8 MPU6050_dmpSetFIFOProcessedCallback(void (*func) (void));
+
+uint8 MPU6050_dmpInitFIFOParam();
+uint8 MPU6050_dmpCloseFIFO();
+uint8 MPU6050_dmpSetGyroDataSource(uint8 source);
+uint8 MPU6050_dmpDecodeQuantizedAccel();
+uint32 MPU6050_dmpGetGyroSumOfSquare();
+uint32 MPU6050_dmpGetAccelSumOfSquare();
+void MPU6050_dmpOverrideQuaternion(long *q);
+uint16 MPU6050_dmpGetFIFOPacketSize();
+#endif
+
+#endif /* _MPU6050_H_ */
diff --git a/Jennic/README.md b/Jennic/README.md
new file mode 100644
index 00000000..33cc83df
--- /dev/null
+++ b/Jennic/README.md
@@ -0,0 +1,14 @@
+# I2C Device Library for Jennic
+
+## I2Cdev
+We use JenOS peripheral libraries.
+
+## Devices
+Currently only MPU6050 is supported without DMP functions. There is no address set dynamically so currently only one MPU6050 can be read (or address must be changed manually each time before reading/writing)
+
+Adding more functions and devices should be straighforward after reading the source code.
+
+## Licence
+I2Cdev device library code is placed under the MIT license.
+
+_Copyright (c) 2011 Jeff Rowberg. Copyright (c) 2014 Marton Sebok. Copyright (c) 2015 Grégoire Surrel._
diff --git a/LinuxI2CDev/I2Cdev/I2Cdev.cpp b/LinuxI2CDev/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..600bf2fc
--- /dev/null
+++ b/LinuxI2CDev/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,391 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 2013-06-05 by Jeff Rowberg 
+//
+// Changelog:
+//      2021-09-28 - allow custom Wire object as transaction function argument
+//      2020-01-20 - hardija : complete support for Teensy 3.x
+//      2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
+//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+//      2011-10-03 - added automatic Arduino version detection for ease of use
+//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+//      2011-08-02 - added support for 16-bit registers
+//                 - fixed incorrect Doxygen comments on some methods
+//                 - added timeout value for read operations (thanks mem @ Arduino forums)
+//      2011-07-30 - changed read/write function structures to return success or byte counts
+//                 - made all methods static for multi-device memory savings
+//      2011-07-28 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 "I2Cdev.h"
+
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+#include 
+
+static const char* i2cDev = NULL;
+/** Default constructor.
+ */
+I2Cdev::I2Cdev() {
+}
+
+void I2Cdev::initialize(const char* devPath) {
+	i2cDev = devPath;
+}
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
+    uint16_t b;
+    uint8_t count = readWord(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
+    return readWords(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
+    int fd = open(i2cDev, O_RDWR);
+    if (fd < 0) {
+        fprintf(stderr, "Failed to open i2c device %s: %s\n", i2cDev, strerror(errno));
+        return -1;
+    }
+    if (ioctl(fd, I2C_SLAVE, (unsigned long) devAddr) < 0) {
+        fprintf(stderr, "Failed to set i2c address to %u: %s\n", devAddr, strerror(errno));
+        close(fd);
+        return -1;
+    }
+    if (timeout != 0 && ioctl(fd, I2C_TIMEOUT, (unsigned long) timeout / 100) < 0) {
+        fprintf(stderr, "Failed to set i2c timeout: %s\n", strerror(errno));
+    }
+    if (write(fd, ®Addr, 1) != 1) {
+        fprintf(stderr, "Failed to write reg: %s\n", strerror(errno));
+    }
+    if (read(fd, data, length) != length) {
+        fprintf(stderr, "Failed to read reg: %s\n", strerror(errno));
+        close(fd);
+        return -1;
+    }
+    close(fd);
+    return length;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
+    uint8_t buff[length * 2];
+
+    if (readBytes(devAddr, regAddr, length * 2, buff, timeout) > 0)
+    {
+        for (int i = 0; i < length; i++)
+        {
+            data[i] = (buff[i * 2] << 8) | buff[i * 2 + 1];
+        }
+        return length;
+    }
+
+    return -1;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    readByte(devAddr, regAddr, &b, I2Cdev::readTimeout);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
+    uint16_t w;
+    readWord(devAddr, regAddr, &w, I2Cdev::readTimeout);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b, I2Cdev::readTimeout) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (readWord(devAddr, regAddr, &w, I2Cdev::readTimeout) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return writeWord(devAddr, regAddr, w);
+    } else {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    return writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
+    int fd = open(i2cDev, O_RDWR);
+    if (fd < 0) {
+        fprintf(stderr, "Failed to open i2c device %s: %s\n", i2cDev, strerror(errno));
+        return -1;
+    }
+    if (ioctl(fd, I2C_SLAVE, (unsigned long) devAddr) < 0) {
+        fprintf(stderr, "Failed to set i2c address to %u: %s\n", devAddr, strerror(errno));
+        close(fd);
+        return -1;
+    }
+    uint16_t buff_length = length + 1;
+    uint8_t buff[buff_length];
+    buff[0] = regAddr;
+
+    memcpy(&buff[1], data, length);
+
+    if (write(fd, buff, buff_length) != buff_length)
+    {
+        fprintf(stderr, "Failed to write reg: %s\n", strerror(errno));
+        close(fd);
+        return false;
+    }
+    close(fd);
+    return true;
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
+    uint8_t buff[length * 2];
+
+    for (int i = 0; i < length; i++)
+    {
+        buff[2 * i] = (uint8_t)(data[i] >> 8);     //MSByte
+        buff[1 + 2 * i] = (uint8_t)(data[i] >> 0); //LSByte
+    }
+
+    return writeBytes(devAddr, regAddr, length * 2, buff);
+}
+
+/** Default timeout value for read operations.
+ * Set this to 0 to disable timeout detection.
+ */
+uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
+
diff --git a/LinuxI2CDev/I2Cdev/I2Cdev.h b/LinuxI2CDev/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..79fd3f01
--- /dev/null
+++ b/LinuxI2CDev/I2Cdev/I2Cdev.h
@@ -0,0 +1,100 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 2013-06-05 by Jeff Rowberg 
+//
+// Changelog:
+//      2021-09-28 - allow custom Wire object as transaction function argument
+//      2020-01-20 - hardija : complete support for Teensy 3.x
+//      2015-10-30 - simondlevy : support i2c_t3 for Teensy3.1
+//      2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications
+//      2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan)
+//      2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
+//                 - add compiler warnings when using outdated or IDE or limited I2Cdev implementation
+//      2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
+//      2011-10-03 - added automatic Arduino version detection for ease of use
+//      2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
+//      2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
+//      2011-08-03 - added optional timeout parameter to read* methods to easily change from default
+//      2011-08-02 - added support for 16-bit registers
+//                 - fixed incorrect Doxygen comments on some methods
+//                 - added timeout value for read operations (thanks mem @ Arduino forums)
+//      2011-07-30 - changed read/write function structures to return success or byte counts
+//                 - made all methods static for multi-device memory savings
+//      2011-07-28 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+
+#ifndef I2CDEVLIB_WIRE_BUFFER_LENGTH
+    #if defined(I2C_BUFFER_LENGTH)
+        // Arduino ESP32 core Wire uses this
+        #define I2CDEVLIB_WIRE_BUFFER_LENGTH I2C_BUFFER_LENGTH
+    #elif defined(BUFFER_LENGTH)
+        // Arduino AVR core Wire and many others use this
+        #define I2CDEVLIB_WIRE_BUFFER_LENGTH BUFFER_LENGTH
+    #elif defined(SERIAL_BUFFER_SIZE)
+        // Arduino SAMD core Wire uses this
+        #define I2CDEVLIB_WIRE_BUFFER_LENGTH SERIAL_BUFFER_SIZE
+    #else
+        // should be a safe fallback, though possibly inefficient
+        #define I2CDEVLIB_WIRE_BUFFER_LENGTH 32
+    #endif
+#endif
+
+// -----------------------------------------------------------------------------
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static void initialize(const char* i2cDev);
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+};
+
+#endif /* _I2CDEV_H_ */
diff --git a/LinuxI2CDev/MPU6050/MPU6050.cpp b/LinuxI2CDev/MPU6050/MPU6050.cpp
new file mode 100644
index 00000000..6ea4a676
--- /dev/null
+++ b/LinuxI2CDev/MPU6050/MPU6050.cpp
@@ -0,0 +1,3413 @@
+// 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 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//  2021-09-27 - split implementations out of header files, finally
+//  2019-07-08 - Added Auto Calibration routine
+//     ... - ongoing debug release
+
+// 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 
+#include 
+#include 
+#include 
+#include 
+#include 
+
+/** Specific address constructor.
+ * @param address I2C address, uses default I2C address if none is specified
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+MPU6050_Base::MPU6050_Base(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_Base::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_Base::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_Base::getAuxVDDIOLevel() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getRate() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SMPLRT_DIV, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set gyroscope sample rate divider.
+ * @param rate New sample rate divider
+ * @see getRate()
+ * @see MPU6050_RA_SMPLRT_DIV
+ */
+void MPU6050_Base::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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050_Base::getExternalFrameSync() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set external FSYNC configuration.
+ * @see getExternalFrameSync()
+ * @see MPU6050_RA_CONFIG
+ * @param sync New FSYNC configuration value
+ */
+void MPU6050_Base::setExternalFrameSync(uint8_t sync) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, sync);
+}
+/** 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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @return DLFP configuration
+ * @see MPU6050_RA_CONFIG
+ * @see MPU6050_CFG_DLPF_CFG_BIT
+ * @see MPU6050_CFG_DLPF_CFG_LENGTH
+ */
+uint8_t MPU6050_Base::getDLPFMode() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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_Base::getFullScaleGyroRange() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getAccelXSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, &buffer[0], I2Cdev::readTimeout);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout);	
+    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_Base::getAccelYSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, &buffer[0], I2Cdev::readTimeout);
+	I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_A, &buffer[1], I2Cdev::readTimeout);	
+    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_Base::getAccelZSelfTestFactoryTrim() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_SELF_TEST_Z, 2, buffer, I2Cdev::readTimeout);	
+    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_Base::getGyroXSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_X, buffer, I2Cdev::readTimeout);	
+    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_Base::getGyroYSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Y, buffer, I2Cdev::readTimeout);	
+    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_Base::getGyroZSelfTestFactoryTrim() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_SELF_TEST_Z, buffer, I2Cdev::readTimeout);	
+    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_Base::getAccelXSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getAccelYSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getAccelZSelfTest() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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_Base::getFullScaleAccelRange() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set full-scale accelerometer range.
+ * @param range New full-scale accelerometer range setting
+ * @see getFullScaleAccelRange()
+ */
+void MPU6050_Base::setFullScaleAccelRange(uint8_t range) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
+}
+/** 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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050_Base::getDHPFMode() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFreefallDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FF_THR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFreefallDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FF_DUR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_THR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DUR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getZeroMotionDetectionThreshold() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_THR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getZeroMotionDetectionDuration() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_ZRMOT_DUR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getTempFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set temperature FIFO enabled value.
+ * @param enabled New temperature FIFO enabled value
+ * @see getTempFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050_Base::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_Base::getXGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getYGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getZGyroFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getAccelFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set accelerometer FIFO enabled value.
+ * @param enabled New accelerometer FIFO enabled value
+ * @see getAccelFIFOEnabled()
+ * @see MPU6050_RA_FIFO_EN
+ */
+void MPU6050_Base::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_Base::getSlave2FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave1FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave0FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getMultiMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getWaitForExternalSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave3FIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlaveReadWriteTransitionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050_Base::getMasterClockSpeed() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+/** Set I2C master clock speed.
+ * @reparam speed Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+void MPU6050_Base::setMasterClockSpeed(uint8_t speed) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, speed);
+}
+
+// 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_Base::getSlaveAddress(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlaveRegister(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlaveEnabled(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::setSlaveEnabled(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, enabled);
+}
+/** 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_Base::getSlaveWordByteSwap(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::setSlaveWordByteSwap(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, enabled);
+}
+/** 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_Base::getSlaveWriteMode(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::setSlaveWriteMode(uint8_t num, bool mode) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mode);
+}
+/** 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_Base::getSlaveWordGroupOffset(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::setSlaveWordGroupOffset(uint8_t num, bool enabled) {
+    if (num > 3) return;
+    I2Cdev::writeBit(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, enabled);
+}
+/** 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_Base::getSlaveDataLength(uint8_t num) {
+    if (num > 3) return 0;
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::setSlaveDataLength(uint8_t num, uint8_t length) {
+    if (num > 3) return;
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, length);
+}
+
+// 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_Base::getSlave4Address() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_ADDR, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave4Register() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_REG, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::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_Base::getSlave4Enabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave4InterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave4WriteMode() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlave4MasterDelay() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::setSlave4MasterDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, delay);
+}
+/** 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_Base::getSlate4InputByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_I2C_SLV4_DI, buffer, I2Cdev::readTimeout);
+    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_Base::getPassthroughStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave4IsDone() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getLostArbitration() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave4Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave3Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave2Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave1Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getSlave0Nack() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getInterruptMode() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getInterruptDrive() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getInterruptLatch() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getInterruptLatchClear() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFSyncInterruptLevel() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFSyncInterruptEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getI2CBypassEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getClockOutputEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntEnabled() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_INT_ENABLE, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntFreefallEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntZeroMotionEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntFIFOBufferOverflowEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntI2CMasterEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntDataReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getIntStatus() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_INT_STATUS, buffer, I2Cdev::readTimeout);
+    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_Base::getIntFreefallStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getIntMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getIntZeroMotionStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getIntFIFOBufferOverflowStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getIntI2CMasterStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getIntDataReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
+    (void)mx; // unused parameter
+    (void)my; // unused parameter
+    (void)mz; // unused parameter
+    
+    getMotion6(ax, ay, az, gx, gy, gz);
+    // TODO: magnetometer integration
+}
+/** 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_Base::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, buffer, I2Cdev::readTimeout);
+    *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:
+ *
+ * 
+ * AFS_SEL | Full Scale Range | LSB Sensitivity
+ * --------+------------------+----------------
+ * 0       | +/- 2g           | 16384 LSB/mg
+ * 1       | +/- 4g           | 8192 LSB/mg
+ * 2       | +/- 8g           | 4096 LSB/mg
+ * 3       | +/- 16g          | 2048 LSB/mg
+ * 

+ *
+ * @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_Base::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, buffer, I2Cdev::readTimeout);
+    *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_Base::getAccelerationX() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getAccelerationY() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getAccelerationZ() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getTemperature() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_TEMP_OUT_H, 2, buffer, I2Cdev::readTimeout);
+    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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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_Base::getRotation(int16_t* x, int16_t* y, int16_t* z) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 6, buffer, I2Cdev::readTimeout);
+    *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_Base::getRotationX() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_XOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getRotationY() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_YOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getRotationZ() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getExternalSensorByte(int position) {
+    I2Cdev::readByte(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, buffer, I2Cdev::readTimeout);
+    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_Base::getExternalSensorWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getExternalSensorDWord(int position) {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, buffer, I2Cdev::readTimeout);
+    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_Base::getMotionStatus() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MOT_DETECT_STATUS, buffer, I2Cdev::readTimeout);
+    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_Base::getXNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getXPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getYNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getYPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getZNegMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getZPosMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::getZeroMotionDetected() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getExternalShadowDelayEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getSlaveDelayEnabled(uint8_t num) {
+    // MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT is 4, SLV3 is 3, etc.
+    if (num > 4) return 0;
+    I2Cdev::readBit(devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::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_Base::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_Base::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_Base::getAccelerometerPowerOnDelay() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::setAccelerometerPowerOnDelay(uint8_t delay) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, delay);
+}
+/** 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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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_Base::getFreefallDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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_Base::getMotionDetectionCounterDecrement() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFIFOEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getI2CMasterModeEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::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_Base::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_Base::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_Base::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_Base::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_Base::getSleepEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getWakeCycleEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getTempSensorEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getClockSource() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, buffer, I2Cdev::readTimeout);
+    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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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_Base::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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 20 Hz
+ * 3            | 40 Hz
+ * 

+ *
+ * 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_Base::getWakeFrequency() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::setWakeFrequency(uint8_t frequency) {
+    I2Cdev::writeBits(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, frequency);
+}
+
+/** 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_Base::getStandbyXAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getStandbyYAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getStandbyZAccelEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getStandbyXGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getStandbyYGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getStandbyZGyroEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getFIFOCount() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer, I2Cdev::readTimeout);
+    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_Base::getFIFOByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::getFIFOBytes(uint8_t *data, uint8_t length) {
+    if(length > 0){
+        I2Cdev::readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data, I2Cdev::readTimeout);
+    } else {
+    	*data = 0;
+    }
+}
+
+/** Get timeout to get a packet from FIFO buffer.
+ * @return Current timeout to get a packet from FIFO buffer
+ * @see MPU6050_FIFO_DEFAULT_TIMEOUT
+ */
+uint32_t MPU6050_Base::getFIFOTimeout() {
+	return fifoTimeout;
+}
+
+/** Set timeout to get a packet from FIFO buffer.
+ * @param New timeout to get a packet from FIFO buffer
+ * @see MPU6050_FIFO_DEFAULT_TIMEOUT
+ */
+void MPU6050_Base::setFIFOTimeout(uint32_t fifoTimeout) {
+	this->fifoTimeout = fifoTimeout;
+}
+
+/** Replacement for Arduino micros().
+ * @return Number of microseconds since process start
+ */
+static uint32_t micros() {
+    struct timespec ts;
+    clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &ts);
+    return ts.tv_sec * 1000000 + ts.tv_nsec / 1000;
+}
+/** Get latest byte from FIFO buffer no matter how much time has passed.
+ * ===                  GetCurrentFIFOPacket                    ===
+ * ================================================================
+ * Returns 1) when nothing special was done
+ *         2) when recovering from overflow
+ *         0) when no valid data is available
+ * ================================================================ */
+ int8_t MPU6050_Base::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
+     int16_t fifoC;
+     // This section of code is for when we allowed more than 1 packet to be acquired
+     uint32_t BreakTimer = micros();
+     bool packetReceived = false;
+     do {
+         if ((fifoC = getFIFOCount())  > length) {
+
+             if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
+                 resetFIFO(); // Fixes any overflow corruption
+                 fifoC = 0;
+                 while (!(fifoC = getFIFOCount()) && ((micros() - BreakTimer) <= (getFIFOTimeout()))); // Get Next New Packet
+                 } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
+                 uint8_t Trash[I2CDEVLIB_WIRE_BUFFER_LENGTH];
+                 while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
+                     fifoC = fifoC - length; // Save the last packet
+                     uint16_t  RemoveBytes;
+                     while (fifoC) { // fifo count will reach zero so this is safe
+                         RemoveBytes = (fifoC < I2CDEVLIB_WIRE_BUFFER_LENGTH) ? fifoC : I2CDEVLIB_WIRE_BUFFER_LENGTH; // Buffer Length is different than the packet length this will efficiently clear the buffer
+                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
+                         fifoC -= RemoveBytes;
+                     }
+                 }
+             }
+         }
+         if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
+         // We have 1 packet
+         packetReceived = fifoC == length;
+         if (!packetReceived && (micros() - BreakTimer) > (getFIFOTimeout())) return 0;
+     } while (!packetReceived);
+     getFIFOBytes(data, length); //Get 1 packet
+     return 1;
+}
+
+
+/** Write byte to FIFO buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_Base::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_Base::getDeviceID() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer, I2Cdev::readTimeout);
+    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_Base::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_Base::getOTPBankValid() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setOTPBankValid(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050_Base::getXGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::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_Base::getYGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::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_Base::getZGyroOffsetTC() {
+    I2Cdev::readBits(devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::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_Base::getXFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setXFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050_Base::getYFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setYFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050_Base::getZFineGain() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setZFineGain(int8_t gain) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050_Base::getXAccelOffset() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setXAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050_Base::getYAccelOffset() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setYAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050_Base::getZAccelOffset() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer, I2Cdev::readTimeout);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setZAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050_Base::getXGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_XG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setXGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050_Base::getYGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_YG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setYGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050_Base::getZGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, buffer, I2Cdev::readTimeout);
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050_Base::setZGyroOffset(int16_t offset) {
+    I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050_Base::getIntPLLReadyEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050_Base::getIntDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setIntDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050_Base::getDMPInt5Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getDMPInt4Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getDMPInt3Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getDMPInt2Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getDMPInt1Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getDMPInt0Status() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050_Base::getIntPLLReadyStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+bool MPU6050_Base::getIntDMPStatus() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_Base::getDMPEnabled() {
+    I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setDMPEnabled(bool enabled) {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_Base::resetDMP() {
+    I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true);
+}
+
+// BANK_SEL register
+
+void MPU6050_Base::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
+    bank &= 0x1F;
+    if (userBank) bank |= 0x20;
+    if (prefetchEnabled) bank |= 0x40;
+    I2Cdev::writeByte(devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_Base::setMemoryStartAddress(uint8_t address) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050_Base::readMemoryByte() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::writeMemoryByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_Base::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, I2Cdev::readTimeout);
+        
+        // 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_Base::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    setMemoryBank(bank);
+    setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    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] = *(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) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer, I2Cdev::readTimeout);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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_Base::writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
+    return writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_Base::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 = *(data + i++);
+            offset = *(data + i++);
+            length = *(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] = *(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 = *(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_Base::writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize) {
+    return writeDMPConfigurationSet(data, dataSize, true);
+}
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_Base::getDMPConfig1() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setDMPConfig1(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050_Base::getDMPConfig2() {
+    I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer, I2Cdev::readTimeout);
+    return buffer[0];
+}
+void MPU6050_Base::setDMPConfig2(uint8_t config) {
+    I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
+
+/* Replacement for Arduino map()
+ * @see https://www.arduino.cc/reference/en/language/functions/math/map/
+ */
+long map(long x, long in_min, long in_max, long out_min, long out_max) {
+    return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+//***************************************************************************************
+//**********************           Calibration Routines            **********************
+//***************************************************************************************
+/**
+  @brief      Fully calibrate Gyro from ZERO in about 6-7 Loops 600-700 readings
+*/
+void MPU6050_Base::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_Base::CalibrateAccel(uint8_t Loops ) {
+
+	float kP = 0.3;
+	float kI = 20;
+	float x;
+	x = (100 - map(Loops, 1, 5, 20, 0)) * .01;
+	kP *= x;
+	kI *= x;
+	PID( 0x3B, kP, kI,  Loops);
+}
+
+void MPU6050_Base::PID(uint8_t ReadAddress, float kP,float kI, uint8_t Loops){
+	uint8_t SaveAddress = (ReadAddress == 0x3B)?((getDeviceID() < 0x38 )? 0x06:0x77):0x13;
+
+	int16_t  Data;
+	float Reading;
+	int16_t BitZero[3];
+	uint8_t shift =(SaveAddress == 0x77)?3:2;
+	float Error, PTerm, ITerm[3];
+	int16_t eSample;
+	uint32_t eSum;
+	uint16_t gravity = 8192; // prevent uninitialized compiler warning
+	if (ReadAddress == 0x3B) gravity = 16384 >> getFullScaleAccelRange();
+	for (int i = 0; i < 3; i++) {
+		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data, I2Cdev::readTimeout); // reads 1 or more 16 bit integers (Word)
+		Reading = Data;
+		if(SaveAddress != 0x13){
+			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
+			ITerm[i] = ((float)Reading) * 8;
+			} else {
+			ITerm[i] = Reading * 4;
+		}
+	}
+	for (int L = 0; L < Loops; L++) {
+		eSample = 0;
+		for (int c = 0; c < 100; c++) {// 100 PI Calculations
+			eSum = 0;
+			for (int i = 0; i < 3; i++) {
+				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data, I2Cdev::readTimeout); // reads 1 or more 16 bit integers (Word)
+				Reading = Data;
+				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= gravity;	//remove Gravity
+				Error = -Reading;
+				eSum += abs(Reading);
+				PTerm = kP * Error;
+				ITerm[i] += (Error * 0.001) * kI;				// Integral term 1000 Calculations a second = 0.001
+				if(SaveAddress != 0x13){
+					Data = round((PTerm + ITerm[i] ) / 8);		//Compute PID Output
+					Data = ((Data)&0xFFFE) |BitZero[i];			// Insert Bit0 Saved at beginning
+				} else Data = round((PTerm + ITerm[i] ) / 4);	//Compute PID Output
+				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+			}
+			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
+				c = 0;
+			}
+			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
+            usleep(1000);
+		}
+		kP *= .75;
+		kI *= .75;
+		for (int i = 0; i < 3; i++){
+			if(SaveAddress != 0x13) {
+				Data = round((ITerm[i] ) / 8);		//Compute PID Output
+				Data = ((Data)&0xFFFE) |BitZero[i];	// Insert Bit0 Saved at beginning
+			} else Data = round((ITerm[i]) / 4);
+			I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+		}
+	}
+	resetFIFO();
+	resetDMP();
+}
+
+int16_t * MPU6050_Base::GetActiveOffsets() {
+    uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
+    if(AOffsetRegister == 0x06)	I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)offsets, I2Cdev::readTimeout);
+    else {
+        I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)offsets, I2Cdev::readTimeout);
+        I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)(offsets+1), I2Cdev::readTimeout);
+        I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)(offsets+2), I2Cdev::readTimeout);
+    }
+    I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)(offsets+3), I2Cdev::readTimeout);
+    return offsets;
+}
+
+void MPU6050_Base::PrintActiveOffsets() {
+    GetActiveOffsets();
+	//	A_OFFSET_H_READ_A_OFFS(Data);
+    fprintf(stderr, "%.5f,\t", (float)offsets[0]);
+    fprintf(stderr, "%.5f,\t", (float)offsets[1]);
+    fprintf(stderr, "%.5f,\t", (float)offsets[2]);
+	
+	//	XG_OFFSET_H_READ_OFFS_USR(Data);
+    fprintf(stderr, "%.5f,\t", (float)offsets[3]);
+    fprintf(stderr, "%.5f,\t", (float)offsets[4]);
+    fprintf(stderr, "%.5f\n\n", (float)offsets[5]);
+}
diff --git a/LinuxI2CDev/MPU6050/MPU6050.h b/LinuxI2CDev/MPU6050/MPU6050.h
new file mode 100644
index 00000000..12edd61f
--- /dev/null
+++ b/LinuxI2CDev/MPU6050/MPU6050.h
@@ -0,0 +1,836 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//  2021/09/27 - split implementations out of header files, finally
+//     ... - ongoing debug release
+
+// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
+// 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 "I2Cdev.h"
+#include "helper_3dmath.h"
+
+#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
+
+#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000
+
+class MPU6050_Base {
+    public:
+        MPU6050_Base(uint8_t address=MPU6050_DEFAULT_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();
+		int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length);
+        void setFIFOByte(uint8_t data);
+        void getFIFOBytes(uint8_t *data, uint8_t length);
+        void setFIFOTimeout(uint32_t fifoTimeout);
+        uint32_t getFIFOTimeout();
+
+        // WHO_AM_I register
+        uint8_t getDeviceID();
+        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);
+
+		// Calibration Routines
+		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 math
+		void PrintActiveOffsets(); // See the results of the Calibration
+		int16_t * GetActiveOffsets();
+
+    protected:
+        uint8_t devAddr;
+        void *wireObj;
+        uint8_t buffer[14];
+        uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT;
+    
+    private:
+        int16_t offsets[6];
+};
+
+#ifndef I2CDEVLIB_MPU6050_TYPEDEF
+#define I2CDEVLIB_MPU6050_TYPEDEF
+typedef MPU6050_Base MPU6050;
+#endif
+
+#endif /* _MPU6050_H_ */
diff --git a/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.cpp b/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.cpp
new file mode 100644
index 00000000..32e7bc46
--- /dev/null
+++ b/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.cpp
@@ -0,0 +1,563 @@
+// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 5/20/2013 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//  2021/09/27 - split implementations out of header files, finally
+//  2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array
+//             - Simplified dmpInitialize() to accomidate the dmpmemory array alterations
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2021 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.
+===============================================
+*/
+
+// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS20
+
+#include "MPU6050_6Axis_MotionApps20.h"
+#include 
+#include 
+#include 
+#include 
+
+/* Source is from the InvenSense MotionApps v2 demo code. Original source is
+ * unavailable, unless you happen to be amazing as decompiling binary by
+ * hand (in which case, please contact me, and I'm totally serious).
+ *
+ * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
+ * DMP reverse-engineering he did to help make this bit of wizardry
+ * possible.
+ */
+
+// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
+// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
+// after moving string constants to flash memory storage using the F()
+// compiler macro (Arduino IDE 1.0+ required).
+
+//#define DEBUG
+#ifdef DEBUG
+    #define DEBUG_PRINTF(...) fprintf(stderr, __VA_ARGS__)
+#else
+    #define DEBUG_PRINTF(...)
+#endif
+
+#define MPU6050_DMP_CODE_SIZE       1929    // dmpMemory[]
+#define MPU6050_DMP_CONFIG_SIZE     192     // dmpConfig[]
+#define MPU6050_DMP_UPDATES_SIZE    47      // dmpUpdates[]
+
+/* ================================================================================================ *
+ | Default MotionApps v2.0 42-byte FIFO packet structure:                                           |
+ |                                                                                                  |
+ | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
+ |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
+ |                                                                                                  |
+ | [GYRO Z][      ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ]                         |
+ |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41                          |
+ * ================================================================================================ */
+
+// this block of memory gets written to the MPU on start-up, and it seems
+// to be volatile memory, so it has to be done each time (it only takes ~1
+// second though)
+
+// I Only Changed this by applying all the configuration data and capturing it before startup:
+// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it
+static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] = {
+	/* bank # 0 */
+	0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
+	0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
+	0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00,
+	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+	0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
+	0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
+	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
+	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
+	0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
+	0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
+	/* bank # 1 */
+	0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
+	0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
+	0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
+	0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00,
+	0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
+	/* bank # 2 */
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	/* bank # 3 */
+	0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
+	0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
+	0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
+	0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
+	0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
+	0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
+	0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
+	0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C,
+	0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
+	0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
+	0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
+	0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
+	0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
+	0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
+	0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
+	0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
+	/* bank # 4 */
+	0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
+	0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
+	0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
+	0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
+	0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
+	0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
+	0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
+	0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
+	0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
+	0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
+	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
+	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
+	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
+	0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
+	0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
+	/* bank # 5 */
+	0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
+	0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
+	0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
+	0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
+	0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
+	0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
+	0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
+	0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
+	0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
+	0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
+	0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
+	0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
+	0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
+	0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
+	0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
+	0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
+	/* bank # 6 */
+	0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
+	0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
+	0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
+	0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
+	0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
+	0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
+	0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
+	0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
+	0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
+	0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
+	0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
+	0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
+	0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
+	0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
+	0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
+	0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
+	/* bank # 7 */
+	0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
+	0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
+	0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
+	0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
+	0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
+	0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
+	0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38,
+	0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC,
+	0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF,
+
+};
+
+#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR 
+#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default
+#endif
+
+// I Simplified this:
+uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() {
+	// reset device
+	DEBUG_PRINTF("\nResetting MPU6050...\n");
+	reset();
+	usleep(30000); // wait after reset
+
+	// enable sleep mode and wake cycle
+	/*Serial.println("Enabling sleep mode...");
+	setSleepEnabled(true);
+	Serial.println("Enabling wake cycle...");
+	setWakeCycleEnabled(true);*/
+
+	// disable sleep mode
+	setSleepEnabled(false);
+
+	// get MPU hardware revision
+	setMemoryBank(0x10, true, true);
+	setMemoryStartAddress(0x06);
+	DEBUG_PRINTF("Checking hardware revision...\n");
+    DEBUG_PRINTF("Revision @ user[16][6] = %hhx\n", readMemoryByte());
+	DEBUG_PRINTF("Resetting memory bank selection to 0...\n");
+	setMemoryBank(0, false, false);
+
+	// check OTP bank valid
+	DEBUG_PRINTF("Reading OTP bank valid flag...\n");
+	DEBUG_PRINTF("OTP bank is %s\n", getOTPBankValid() ? "valid!" : "invalid!");
+
+	// setup weird slave stuff (?)
+	DEBUG_PRINTF("Setting slave 0 address to 0x7F...\n");
+	setSlaveAddress(0, 0x7F);
+	DEBUG_PRINTF("Disabling I2C Master mode...\n");
+	setI2CMasterModeEnabled(false);
+	DEBUG_PRINTF("Setting slave 0 address to 0x68 (self)...\n");
+	setSlaveAddress(0, 0x68);
+	DEBUG_PRINTF("Resetting I2C Master control...\n");
+	resetI2CMaster();
+	usleep(20000);
+	DEBUG_PRINTF("Setting clock source to Z Gyro...\n");
+	setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
+
+	DEBUG_PRINTF("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
+	setIntEnabled(1<= dmpGetFIFOPacketSize();
+}
+
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFORate(uint8_t fifoRate);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetFIFORate();
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleStepSizeMS();
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleFrequency();
+// int32_t MPU6050_6Axis_MotionApps20::dmpDecodeTemperature(int8_t tempReg);
+
+//uint8_t MPU6050_6Axis_MotionApps20::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050_6Axis_MotionApps20::dmpUnregisterFIFORateProcess(inv_obj_func func);
+//uint8_t MPU6050_6Axis_MotionApps20::dmpRunFIFORateProcesses();
+
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuaternion(uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendPacketNumber(uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]);
+    data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]);
+    data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]);
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[28] << 8) | packet[29];
+    data[1] = (packet[32] << 8) | packet[33];
+    data[2] = (packet[36] << 8) | packet[37];
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[28] << 8) | packet[29];
+    v -> y = (packet[32] << 8) | packet[33];
+    v -> z = (packet[36] << 8) | packet[37];
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
+    data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
+    data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
+    data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 8) | packet[1]);
+    data[1] = ((packet[4] << 8) | packet[5]);
+    data[2] = ((packet[8] << 8) | packet[9]);
+    data[3] = ((packet[12] << 8) | packet[13]);
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    if (status == 0) {
+        q -> w = (float)qI[0] / 16384.0f;
+        q -> x = (float)qI[1] / 16384.0f;
+        q -> y = (float)qI[2] / 16384.0f;
+        q -> z = (float)qI[3] / 16384.0f;
+        return 0;
+    }
+    return status; // int16 return value, indicates error if this line is reached
+}
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
+    data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
+    data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[16] << 8) | packet[17];
+    data[1] = (packet[20] << 8) | packet[21];
+    data[2] = (packet[24] << 8) | packet[25];
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[16] << 8) | packet[17];
+    v -> y = (packet[20] << 8) | packet[21];
+    v -> z = (packet[24] << 8) | packet[25];
+    return 0;
+}
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSetLinearAccelFilterCoefficient(float coef);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(long *data, const uint8_t* packet);
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
+    // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
+    v -> x = vRaw -> x - gravity -> x*8192;
+    v -> y = vRaw -> y - gravity -> y*8192;
+    v -> z = vRaw -> z - gravity -> z*8192;
+    return 0;
+}
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
+    // rotate measured 3D acceleration vector into original state
+    // frame of reference based on orientation quaternion
+    memcpy(v, vReal, sizeof(VectorInt16));
+    v -> rotate(q);
+    return 0;
+}
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetControlData(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetTemperature(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L);
+    return status;
+}
+
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(VectorFloat *v, Quaternion *q) {
+    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
+    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
+    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
+    return 0;
+}
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetEIS(long *data, const uint8_t* packet);
+
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetEuler(float *data, Quaternion *q) {
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
+    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
+    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
+    return 0;
+}
+
+#ifdef USE_OLD_DMPGETYAWPITCHROLL
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
+    return 0;
+}
+#else 
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan2(gravity -> y , gravity -> z);
+    if (gravity -> z < 0) {
+        if(data[1] > 0) {
+            data[1] = M_PI - data[1];
+        } else { 
+            data[1] = -M_PI - data[1];
+        }
+    }
+    return 0;
+}
+#endif
+
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccelFloat(float *data, const uint8_t* packet);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
+
+uint8_t MPU6050_6Axis_MotionApps20::dmpProcessFIFOPacket(const unsigned char *dmpData) {
+    (void)dmpData; // unused parameter
+    /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
+        if (dmpData[k] < 0x10) Serial.print("0");
+        Serial.print(dmpData[k], HEX);
+        Serial.print(" ");
+    }
+    Serial.print("\n");*/
+    //Serial.println((uint16_t)dmpPacketBuffer);
+    return 0;
+}
+uint8_t MPU6050_6Axis_MotionApps20::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
+    uint8_t status;
+    uint8_t buf[dmpPacketSize];
+    for (uint8_t i = 0; i < numPackets; i++) {
+        // read packet from FIFO
+        getFIFOBytes(buf, dmpPacketSize);
+
+        // process packet
+        if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
+        
+        // increment external process count variable, if supplied
+        if (processed != 0) (*processed)++;
+    }
+    return 0;
+}
+
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFOProcessedCallback(void (*func) (void));
+
+// uint8_t MPU6050_6Axis_MotionApps20::dmpInitFIFOParam();
+// uint8_t MPU6050_6Axis_MotionApps20::dmpCloseFIFO();
+// uint8_t MPU6050_6Axis_MotionApps20::dmpSetGyroDataSource(uint_fast8_t source);
+// uint8_t MPU6050_6Axis_MotionApps20::dmpDecodeQuantizedAccel();
+// uint32_t MPU6050_6Axis_MotionApps20::dmpGetGyroSumOfSquare();
+// uint32_t MPU6050_6Axis_MotionApps20::dmpGetAccelSumOfSquare();
+// void MPU6050_6Axis_MotionApps20::dmpOverrideQuaternion(long *q);
+uint16_t MPU6050_6Axis_MotionApps20::dmpGetFIFOPacketSize() {
+    return dmpPacketSize;
+}
+
+
+
+uint8_t MPU6050_6Axis_MotionApps20::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof
+    return(GetCurrentFIFOPacket(data, dmpPacketSize));
+}
diff --git a/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.h b/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.h
new file mode 100644
index 00000000..71a84667
--- /dev/null
+++ b/LinuxI2CDev/MPU6050/MPU6050_6Axis_MotionApps20.h
@@ -0,0 +1,153 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//  2021/09/27 - split implementations out of header files, finally
+//     ... - ongoing debug release
+
+// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
+// 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_6AXIS_MOTIONAPPS20_H_
+#define _MPU6050_6AXIS_MOTIONAPPS20_H_
+
+// take ownership of the "MPU6050" typedef
+#define I2CDEVLIB_MPU6050_TYPEDEF
+
+#include "MPU6050.h"
+
+class MPU6050_6Axis_MotionApps20 : public MPU6050_Base {
+    public:
+        MPU6050_6Axis_MotionApps20(uint8_t address=MPU6050_DEFAULT_ADDRESS) : MPU6050_Base(address) { }
+
+        uint8_t dmpInitialize();
+        bool dmpPacketAvailable();
+
+        uint8_t dmpSetFIFORate(uint8_t fifoRate);
+        uint8_t dmpGetFIFORate();
+        uint8_t dmpGetSampleStepSizeMS();
+        uint8_t dmpGetSampleFrequency();
+        int32_t dmpDecodeTemperature(int8_t tempReg);
+        
+        // Register callbacks after a packet of FIFO data is processed
+        //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+        //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
+        uint8_t dmpRunFIFORateProcesses();
+        
+        // Setup FIFO for various output
+        uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
+        uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
+        uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+        uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+        // Get Fixed Point data from FIFO
+        uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
+        uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
+        uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
+        uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+        uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+        uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
+        uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
+        uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
+        uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+        uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
+        uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
+        
+        uint8_t dmpGetEuler(float *data, Quaternion *q);
+        uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+        // Get Floating Point data from FIFO
+        uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
+        uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
+
+        uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
+        uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
+
+        uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
+
+        uint8_t dmpInitFIFOParam();
+        uint8_t dmpCloseFIFO();
+        uint8_t dmpSetGyroDataSource(uint8_t source);
+        uint8_t dmpDecodeQuantizedAccel();
+        uint32_t dmpGetGyroSumOfSquare();
+        uint32_t dmpGetAccelSumOfSquare();
+        void dmpOverrideQuaternion(long *q);
+        uint16_t dmpGetFIFOPacketSize();
+        uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof
+
+    private:
+        uint8_t *dmpPacketBuffer;
+        uint16_t dmpPacketSize;
+};
+
+typedef MPU6050_6Axis_MotionApps20 MPU6050;
+
+#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
diff --git a/LinuxI2CDev/MPU6050/helper_3dmath.h b/LinuxI2CDev/MPU6050/helper_3dmath.h
new file mode 100644
index 00000000..b8f20d47
--- /dev/null
+++ b/LinuxI2CDev/MPU6050/helper_3dmath.h
@@ -0,0 +1,218 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg 
+// 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 
+
+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/MSP430/AK8975/library.json b/MSP430/AK8975/library.json
new file mode 100644
index 00000000..44c22b94
--- /dev/null
+++ b/MSP430/AK8975/library.json
@@ -0,0 +1,25 @@
+{
+  "name": "I2Cdevlib-AK8975",
+  "version": "1.0.0",
+  "keywords": "compass, sensor, i2cdevlib, i2c",
+  "description": "AK8975 is 3-axis electronic compass IC with high sensitive Hall sensor technology",
+  "include": "MSP430/AK8975",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "dependencies":
+  [
+    {
+      "name": "I2Cdevlib-Core",
+      "platforms": "timsp430"
+    },
+    {
+      "name": "I2Cdevlib-MPU6050",
+      "platforms": "timsp430"
+    }    
+  ],  
+  "frameworks": "energia",
+  "platforms": "timsp430"
+}
diff --git a/MSP430/I2Cdev/I2Cdev.cpp b/MSP430/I2Cdev/I2Cdev.cpp
index 7c37a8ee..cbc0fbe0 100644
--- a/MSP430/I2Cdev/I2Cdev.cpp
+++ b/MSP430/I2Cdev/I2Cdev.cpp
@@ -215,7 +215,7 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
         Serial.print("...");
     #endif
 
-    int8_t count = 0;
+    uint8_t count = 0;
     uint32_t t1 = millis();
 
     #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
@@ -224,15 +224,13 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
             // Arduino v00xx (before v1.0), Wire library
 
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length; k += min(length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.send(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
-                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
-
+                Wire.requestFrom(devAddr, (uint8_t)min(length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
                 for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                     data[count] = Wire.receive();
                     #ifdef I2CDEV_SERIAL_DEBUG
@@ -240,23 +238,19 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
                         if (count + 1 < length) Serial.print(" ");
                     #endif
                 }
-
-                Wire.endTransmission();
             }
         #elif (ARDUINO == 100)
             // Arduino v1.0.0, Wire library
             // Adds standardized write() and read() stream methods instead of send() and receive()
 
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length; k += min(length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.write(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
-                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
-        
+                Wire.requestFrom(devAddr, (uint8_t)min(length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
                 for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                     data[count] = Wire.read();
                     #ifdef I2CDEV_SERIAL_DEBUG
@@ -264,23 +258,19 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
                         if (count + 1 < length) Serial.print(" ");
                     #endif
                 }
-        
-                Wire.endTransmission();
             }
         #elif (ARDUINO > 100)
             // Arduino v1.0.1+, Wire library
             // Adds official support for repeated start condition, yay!
 
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length; k += min(length, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length; k += min(length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.write(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
-                Wire.requestFrom(devAddr, (uint8_t)min(length - k, BUFFER_LENGTH));
-        
+                Wire.requestFrom(devAddr, (uint8_t)min(length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
                 for (; Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
                     data[count] = Wire.read();
                     #ifdef I2CDEV_SERIAL_DEBUG
@@ -288,8 +278,6 @@ int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8
                         if (count + 1 < length) Serial.print(" ");
                     #endif
                 }
-        
-                Wire.endTransmission();
             }
         #endif
 
@@ -342,7 +330,7 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
         Serial.print("...");
     #endif
 
-    int8_t count = 0;
+    uint8_t count = 0;
     uint32_t t1 = millis();
 
     #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE)
@@ -351,13 +339,12 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
             // Arduino v00xx (before v1.0), Wire library
 
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.send(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
                 Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
     
                 bool msb = true; // starts with MSB, then LSB
@@ -376,21 +363,18 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     }
                     msb = !msb;
                 }
-
-                Wire.endTransmission();
             }
         #elif (ARDUINO == 100)
             // Arduino v1.0.0, Wire library
             // Adds standardized write() and read() stream methods instead of send() and receive()
     
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.write(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
                 Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
     
                 bool msb = true; // starts with MSB, then LSB
@@ -409,21 +393,18 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     }
                     msb = !msb;
                 }
-        
-                Wire.endTransmission();
             }
         #elif (ARDUINO > 100)
             // Arduino v1.0.1+, Wire library
             // Adds official support for repeated start condition, yay!
 
             // I2C/TWI subsystem uses internal buffer that breaks with large data requests
-            // so if user requests more than BUFFER_LENGTH bytes, we have to do it in
+            // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in
             // smaller chunks instead of all at once
-            for (uint8_t k = 0; k < length * 2; k += min(length * 2, BUFFER_LENGTH)) {
+            for (uint8_t k = 0; k < length * 2; k += min(length * 2, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
                 Wire.beginTransmission(devAddr);
                 Wire.write(regAddr);
                 Wire.endTransmission();
-                Wire.beginTransmission(devAddr);
                 Wire.requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes
         
                 bool msb = true; // starts with MSB, then LSB
@@ -442,8 +423,6 @@ int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint1
                     }
                     msb = !msb;
                 }
-        
-                Wire.endTransmission();
             }
         #endif
 
@@ -966,7 +945,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
         TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register
         // enable twi module, acks, and twi interrupt
 
-        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
+        TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA);
 
         /* TWEN - TWI Enable Bit
         TWIE - TWI Interrupt Enable
@@ -1035,7 +1014,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
     }
 
     void twii_SetStart() {
-        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTA);
+        TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTA);
     }
 
     void twi_write01() {
@@ -1118,19 +1097,19 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
     void twi_reply(uint8_t ack) {
         // transmit master read ready signal, with or without ack
         if (ack){
-            TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT) | _BV(TWEA);
+            TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT) | (1 << TWEA);
         } else {
-            TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWINT);
+            TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT);
         }
     }
     
     void twi_stop(void) {
         // send stop condition
-        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT) | _BV(TWSTO);
+        TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTO);
     
         // wait for stop condition to be exectued on bus
         // TWINT is not set after a stop condition!
-        while (TWCR & _BV(TWSTO)) {
+        while (TWCR & (1 << TWSTO)) {
             continue;
         }
     
@@ -1140,7 +1119,7 @@ uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
 
     void twi_releaseBus(void) {
         // release bus
-        TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA) | _BV(TWINT);
+        TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT);
     
         // update twi state
         twi_state = TWI_READY;
diff --git a/MSP430/I2Cdev/I2Cdev.h b/MSP430/I2Cdev/I2Cdev.h
index daf01322..c3a32a08 100644
--- a/MSP430/I2Cdev/I2Cdev.h
+++ b/MSP430/I2Cdev/I2Cdev.h
@@ -213,7 +213,7 @@ class I2Cdev {
     
     /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */
     
-    #define TW_STATUS_MASK              (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3))
+    #define TW_STATUS_MASK              ((1 << TWS7)|(1 << TWS6)|(1 << TWS5)|(1 << TWS4)|(1 << TWS3))
     #define TW_STATUS                   (TWSR & TW_STATUS_MASK)
     #define TW_START                    0x08
     #define TW_REP_START                0x10
@@ -248,11 +248,11 @@ class I2Cdev {
     //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr))
     
     #ifndef sbi // set bit
-        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
+        #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= (1 << bit))
     #endif // sbi
     
     #ifndef cbi // clear bit
-        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
+        #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit))
     #endif // cbi
     
     extern TwoWire Wire;
diff --git a/MSP430/I2Cdev/library.json b/MSP430/I2Cdev/library.json
new file mode 100644
index 00000000..99bbece1
--- /dev/null
+++ b/MSP430/I2Cdev/library.json
@@ -0,0 +1,14 @@
+{
+  "name": "I2Cdevlib-Core",
+  "version": "1.0.0",
+  "keywords": "i2cdevlib, i2c",
+  "description": "The I2C Device Library (I2Cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices.",
+  "include": "MSP430/I2Cdev",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "frameworks": "energia",
+  "platforms": "timsp430"
+}
diff --git a/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h b/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h
index 6a8afe27..8c1ee345 100644
--- a/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h
+++ b/MSP430/MPU6050/MPU6050_6Axis_MotionApps20.h
@@ -625,6 +625,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
+    return status;
+}
+
 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
@@ -676,7 +687,7 @@ uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *proces
         if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
         
         // increment external process count variable, if supplied
-        if (processed != 0) *processed++;
+        if (processed != 0) (*processed)++;
     }
     return 0;
 }
diff --git a/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h b/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h
index aa04e1ba..722fabae 100644
--- a/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h
+++ b/MSP430/MPU6050/MPU6050_9Axis_MotionApps41.h
@@ -558,7 +558,7 @@ uint8_t MPU6050::dmpInitialize() {
             DEBUG_PRINTLN(F("Waiting for FIRO count >= 46..."));
             while ((fifoCount = getFIFOCount()) < 46);
             DEBUG_PRINTLN(F("Reading FIFO..."));
-            getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
             DEBUG_PRINTLN(F("Reading interrupt status..."));
             getIntStatus();
 
@@ -569,13 +569,13 @@ uint8_t MPU6050::dmpInitialize() {
             DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
             while ((fifoCount = getFIFOCount()) < 48);
             DEBUG_PRINTLN(F("Reading FIFO..."));
-            getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
             DEBUG_PRINTLN(F("Reading interrupt status..."));
             getIntStatus();
             DEBUG_PRINTLN(F("Waiting for FIRO count >= 48..."));
             while ((fifoCount = getFIFOCount()) < 48);
             DEBUG_PRINTLN(F("Reading FIFO..."));
-            getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+            getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes
             DEBUG_PRINTLN(F("Reading interrupt status..."));
             getIntStatus();
 
@@ -736,6 +736,17 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu
 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
+    return status;
+}
+
 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
@@ -787,7 +798,7 @@ uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *proces
         if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
         
         // increment external process count variable, if supplied
-        if (processed != 0) *processed++;
+        if (processed != 0) (*processed)++;
     }
     return 0;
 }
diff --git a/MSP430/MPU6050/library.json b/MSP430/MPU6050/library.json
new file mode 100644
index 00000000..15e3cfbb
--- /dev/null
+++ b/MSP430/MPU6050/library.json
@@ -0,0 +1,19 @@
+{
+  "name": "I2Cdevlib-MPU6050",
+  "version": "1.0.0",
+  "keywords": "gyroscope, accelerometer, sensor, i2cdevlib, i2c",
+  "description": "The MPU6050 combines a 3-axis gyroscope and a 3-axis accelerometer on the same silicon die together with an onboard Digital Motion Processor(DMP) which processes complex 6-axis MotionFusion algorithms",
+  "include": "MSP430/MPU6050",
+  "repository":
+  {
+    "type": "git",
+    "url": "https://github.com/jrowberg/i2cdevlib.git"
+  },
+  "dependencies":
+  {
+    "name": "I2Cdevlib-Core",
+    "platforms": "timsp430"
+  },
+  "frameworks": "energia",
+  "platforms": "timsp430"
+}
diff --git a/PIC18/I2Cdev/I2Cdev.c b/PIC18/I2Cdev/I2Cdev.c
new file mode 100644
index 00000000..1d865343
--- /dev/null
+++ b/PIC18/I2Cdev/I2Cdev.c
@@ -0,0 +1,421 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+
+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 "I2Cdev.h"
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
+    uint8_t count = 0;
+
+    // S
+    IdleI2C();
+    StartI2C();
+
+    // Device write address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x00);
+
+    // Register address
+    IdleI2C();
+    WriteI2C(regAddr);
+
+    // R
+    IdleI2C();
+    RestartI2C();
+
+    // Device read address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x01);
+
+    for (count = 0; count < length; count++) {
+        // Data byte
+        IdleI2C();
+        data[count] = ReadI2C();
+
+        if (count == length - 1) {
+            // NACK
+            IdleI2C();
+            NotAckI2C();
+        } else {
+            // ACK
+            IdleI2C();
+            AckI2C();
+        }
+    }
+
+    // P
+    IdleI2C();
+    StopI2C();
+
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
+    return I2Cdev_readBytes(devAddr, regAddr, 1, data);
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
+    uint8_t count = 0;
+
+    // S
+    IdleI2C();
+    StartI2C();
+
+    // Device write address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x00);
+
+    // Register address
+    IdleI2C();
+    WriteI2C(regAddr);
+
+    // R
+    IdleI2C();
+    RestartI2C();
+
+    // Device read address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x01);
+
+    for (count = 0; count < length; count++) {
+        // First byte is bits 15-8 (MSb=15)
+        IdleI2C();
+        data[count] = ReadI2C() << 8;
+
+        // NACK
+        IdleI2C();
+        NotAckI2C();
+
+        // Second byte is bits 7-0 (LSb=0)
+        IdleI2C();
+        data[count] |= ReadI2C();
+
+        if (count == length - 1) {
+            // NACK
+            IdleI2C();
+            NotAckI2C();
+        } else {
+            // ACK
+            IdleI2C();
+            AckI2C();
+        }
+    }
+
+    // P
+    IdleI2C();
+    StopI2C();
+
+    return count;
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
+    return I2Cdev_readWords(devAddr, regAddr, 1, data);
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
+    uint8_t b;
+    uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data) {
+    uint16_t b;
+    uint8_t count = I2Cdev_readWord(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = I2Cdev_readByte(devAddr, regAddr, &b)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = I2Cdev_readWord(devAddr, regAddr, &w)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
+    // S
+    IdleI2C();
+    StartI2C();
+
+    // Device write address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x00);
+
+    // Register address
+    IdleI2C();
+    WriteI2C(regAddr);
+
+    for (uint8_t i = 0; i < length; i++) {
+        // Data byte
+        IdleI2C();
+        WriteI2C(data[i]);
+    }
+    
+    // P
+    IdleI2C();
+    StopI2C();
+
+    return true;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    return I2Cdev_writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
+    // S
+    IdleI2C();
+    StartI2C();
+
+    // Device write address
+    IdleI2C();
+    WriteI2C(devAddr << 1 | 0x00);
+
+    // Register address
+    IdleI2C();
+    WriteI2C(regAddr);
+
+    for (uint8_t i = 0; i < length; i++) {
+        // Send MSB
+        IdleI2C();
+        WriteI2C(data[i] >> 8);
+
+        // Send LSB
+        IdleI2C();
+        WriteI2C(data[i] & 0xFF);
+    }
+
+    // P
+    IdleI2C();
+    StopI2C();
+
+    return true;
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return I2Cdev_writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    I2Cdev_readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return I2Cdev_writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
+    uint16_t w;
+    I2Cdev_readWord(devAddr, regAddr, &w);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return I2Cdev_writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (I2Cdev_readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return I2Cdev_writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (I2Cdev_readWord(devAddr, regAddr, &w) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return I2Cdev_writeWord(devAddr, regAddr, w);
+    } else {
+        return false;
+    }
+}
diff --git a/PIC18/I2Cdev/I2Cdev.h b/PIC18/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..f90d5d2e
--- /dev/null
+++ b/PIC18/I2Cdev/I2Cdev.h
@@ -0,0 +1,64 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#ifndef __XC8
+    #error Use XC8 for compiling
+#endif
+
+#include 
+#include 
+#include 
+#include 
+
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+#endif /* _I2CDEV_H_ */
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/Makefile b/PIC18/MPU6050/Examples/MPU6050_raw.X/Makefile
new file mode 100644
index 00000000..fca8e2cc
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/Makefile
@@ -0,0 +1,113 @@
+#
+#  There exist several targets which are by default empty and which can be 
+#  used for execution of your targets. These targets are usually executed 
+#  before and after some main targets. They are: 
+#
+#     .build-pre:              called before 'build' target
+#     .build-post:             called after 'build' target
+#     .clean-pre:              called before 'clean' target
+#     .clean-post:             called after 'clean' target
+#     .clobber-pre:            called before 'clobber' target
+#     .clobber-post:           called after 'clobber' target
+#     .all-pre:                called before 'all' target
+#     .all-post:               called after 'all' target
+#     .help-pre:               called before 'help' target
+#     .help-post:              called after 'help' target
+#
+#  Targets beginning with '.' are not intended to be called on their own.
+#
+#  Main targets can be executed directly, and they are:
+#  
+#     build                    build a specific configuration
+#     clean                    remove built files from a configuration
+#     clobber                  remove all built files
+#     all                      build all configurations
+#     help                     print help mesage
+#  
+#  Targets .build-impl, .clean-impl, .clobber-impl, .all-impl, and
+#  .help-impl are implemented in nbproject/makefile-impl.mk.
+#
+#  Available make variables:
+#
+#     CND_BASEDIR                base directory for relative paths
+#     CND_DISTDIR                default top distribution directory (build artifacts)
+#     CND_BUILDDIR               default top build directory (object files, ...)
+#     CONF                       name of current configuration
+#     CND_ARTIFACT_DIR_${CONF}   directory of build artifact (current configuration)
+#     CND_ARTIFACT_NAME_${CONF}  name of build artifact (current configuration)
+#     CND_ARTIFACT_PATH_${CONF}  path to build artifact (current configuration)
+#     CND_PACKAGE_DIR_${CONF}    directory of package (current configuration)
+#     CND_PACKAGE_NAME_${CONF}   name of package (current configuration)
+#     CND_PACKAGE_PATH_${CONF}   path to package (current configuration)
+#
+# NOCDDL
+
+
+# Environment 
+MKDIR=mkdir
+CP=cp
+CCADMIN=CCadmin
+RANLIB=ranlib
+
+
+# build
+build: .build-post
+
+.build-pre:
+# Add your pre 'build' code here...
+
+.build-post: .build-impl
+# Add your post 'build' code here...
+
+
+# clean
+clean: .clean-post
+
+.clean-pre:
+# Add your pre 'clean' code here...
+# WARNING: the IDE does not call this target since it takes a long time to
+# simply run make. Instead, the IDE removes the configuration directories
+# under build and dist directly without calling make.
+# This target is left here so people can do a clean when running a clean
+# outside the IDE.
+
+.clean-post: .clean-impl
+# Add your post 'clean' code here...
+
+
+# clobber
+clobber: .clobber-post
+
+.clobber-pre:
+# Add your pre 'clobber' code here...
+
+.clobber-post: .clobber-impl
+# Add your post 'clobber' code here...
+
+
+# all
+all: .all-post
+
+.all-pre:
+# Add your pre 'all' code here...
+
+.all-post: .all-impl
+# Add your post 'all' code here...
+
+
+# help
+help: .help-post
+
+.help-pre:
+# Add your pre 'help' code here...
+
+.help-post: .help-impl
+# Add your post 'help' code here...
+
+
+
+# include project implementation makefile
+include nbproject/Makefile-impl.mk
+
+# include project make variables
+include nbproject/Makefile-variables.mk
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/funclist b/PIC18/MPU6050/Examples/MPU6050_raw.X/funclist
new file mode 100644
index 00000000..bf32657d
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/funclist
@@ -0,0 +1,37 @@
+_MPU6050_initialize: CODE, 6334 0 24
+_I2Cdev_writeBytes: CODE, 5202 0 156
+_MPU6050_getDeviceID: CODE, 6310 0 24
+__stringdata: SMALLCONST, 3851 0 193
+_putch: CODE, 6552 0 12
+_MPU6050_getXGyroOffset: CODE, 6188 0 42
+_MPU6050_getYGyroOffset: CODE, 6104 0 42
+_MPU6050_getZGyroOffset: CODE, 6020 0 42
+_dpowers: SMALLCONST, 3841 0 10
+_I2Cdev_readByte: CODE, 6514 0 20
+_I2Cdev_writeBit: CODE, 5474 0 110
+_main: CODE, 4046 0 342
+_I2Cdev_readBits: CODE, 5584 0 108
+_MPU6050_setFullScaleGyroRange: CODE, 6358 0 24
+_MPU6050_setSleepEnabled: CODE, 6474 0 20
+_MPU6050: CODE, 6584 0 8
+_MPU6050_setClockSource: CODE, 6406 0 24
+__initialization: CODE, 6430 0 12
+___lwdiv: CODE, 5884 0 74
+_MPU6050_setFullScaleAccelRange: CODE, 6382 0 24
+_baud1USART: CODE, 6576 0 8
+_Write1USART: CODE, 6534 0 18
+_printf: CODE, 4706 0 294
+___lwmod: CODE, 5958 0 62
+_MPU6050_testConnection: CODE, 6564 0 12
+_ReadI2C1: CODE, 6272 0 38
+_MPU6050_getXAccelOffset: CODE, 6230 0 42
+_MPU6050_getYAccelOffset: CODE, 6146 0 42
+_MPU6050_getZAccelOffset: CODE, 6062 0 42
+_WriteI2C1: CODE, 5692 0 108
+_I2Cdev_writeByte: CODE, 6494 0 20
+_Open1USART: CODE, 5800 0 84
+_I2Cdev_readBytes: CODE, 4388 0 318
+_I2Cdev_writeBits: CODE, 5358 0 116
+_MPU6050_getMotion6: CODE, 5000 0 202
+_OpenI2C1: CODE, 6452 0 22
+Total: 2739
\ No newline at end of file
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/l.obj b/PIC18/MPU6050/Examples/MPU6050_raw.X/l.obj
new file mode 100644
index 00000000..1734cc86
Binary files /dev/null and b/PIC18/MPU6050/Examples/MPU6050_raw.X/l.obj differ
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/main.c b/PIC18/MPU6050/Examples/MPU6050_raw.X/main.c
new file mode 100644
index 00000000..5f29aed4
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/main.c
@@ -0,0 +1,137 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class
+// 10/7/2011 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+
+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.
+===============================================
+*/
+
+/*
+ * The project can be run without modification on Microchip's 8-bit Wireless
+ * Development Kit. Please set the type of your IC in project properties/Conf/
+ * Device and change the code as needed.
+ */
+
+// PLL Prescaler Selection bits (Divide by 2 (8 MHz oscillator input))
+#pragma config PLLDIV = 2
+
+// Extended Instruction Set (Disabled)
+#pragma config XINST = OFF
+
+// CPU System Clock Postscaler (CPU system clock divide by 2)
+#pragma config CPUDIV = OSC2_PLL2
+
+// Oscillator (HS+PLL, USB-HS+PLL)
+#pragma config OSC = HSPLL
+
+#define USE_OR_MASKS
+#define _XTAL_FREQ 24000000
+
+#include 
+#include 
+#include 
+#include 
+#include 
+
+/*
+ * You must add the I2Cdev and MPU6050 directories as include directories in
+ * project properties/Conf/General options/XC8 global option/XC8 compiler/
+ * Preprocessing and messages/Include directories.
+ */
+#include "I2Cdev.h"
+#include "MPU6050.h"
+
+#define LED_PIN LATBbits.LATB1
+
+/**
+ * Write one char to USART
+ * @param data
+ */
+void putch(char data) {
+    while (Busy1USART());
+    Write1USART(data);
+}
+
+/**
+ * Main function
+ */
+int main() {
+    int16_t ax, ay, az, gx, gy, gz;
+    
+    // Initialize system
+    // PLL (24 MHz)
+    OSCTUNEbits.PLLEN = 1;
+    __delay_ms(25);
+    __delay_ms(25);
+
+    // LED
+    LED_PIN = 0;
+    TRISBbits.TRISB1 = 0;
+
+    // Serial (115200 kbps)
+    Open1USART(USART_TX_INT_OFF | USART_RX_INT_OFF | USART_ASYNCH_MODE |
+        USART_EIGHT_BIT | USART_CONT_RX | USART_BRGH_HIGH, 12);
+    baud1USART(BAUD_8_BIT_RATE | BAUD_AUTO_OFF);
+
+    // I2C (400 kHz)
+    OpenI2C(MASTER, SLEW_OFF);
+    SSP1ADD = 14;
+
+    // MPU6050
+    MPU6050(MPU6050_ADDRESS_AD0_LOW);
+
+    // Initialize device
+    printf("Initializing I2C devices...\r\n");
+    MPU6050_initialize();
+
+    // Verify connection
+    printf("Testing device connections...\r\n");
+    printf(MPU6050_testConnection() ? "MPU6050 connection successful\r\n" :
+        "MPU6050 connection failed\r\n");
+
+    printf("Reading internal sensor offsets...\r\n");
+    printf("%d\t", MPU6050_getXAccelOffset());
+    printf("%d\t", MPU6050_getYAccelOffset());
+    printf("%d\t", MPU6050_getZAccelOffset());
+    printf("%d\t", MPU6050_getXGyroOffset());
+    printf("%d\t", MPU6050_getYGyroOffset());
+    printf("%d\t\n", MPU6050_getZGyroOffset());
+
+    while (true) {
+        // Read raw accel/gyro measurements from device
+        MPU6050_getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+        // Display tab-separated accel/gyro x/y/z values
+        printf("a/g:\t%d\t%d\t%d\t%d\t%d\t%d\r\n", ax, ay, az, gx, gy, gz);
+
+        // Blink LED to indicate activity
+        LED_PIN ^= 1;
+
+        __delay_ms(25);
+        __delay_ms(25);
+    }
+}
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-default.mk b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-default.mk
new file mode 100644
index 00000000..1c5f0ec6
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-default.mk
@@ -0,0 +1,172 @@
+#
+# Generated Makefile - do not edit!
+#
+# Edit the Makefile in the project folder instead (../Makefile). Each target
+# has a -pre and a -post target defined where you can add customized code.
+#
+# This makefile implements configuration specific macros and targets.
+
+
+# Include project Makefile
+ifeq "${IGNORE_LOCAL}" "TRUE"
+# do not include local makefile. User is passing all local related variables already
+else
+include Makefile
+# Include makefile containing local settings
+ifeq "$(wildcard nbproject/Makefile-local-default.mk)" "nbproject/Makefile-local-default.mk"
+include nbproject/Makefile-local-default.mk
+endif
+endif
+
+# Environment
+MKDIR=gnumkdir -p
+RM=rm -f 
+MV=mv 
+CP=cp 
+
+# Macros
+CND_CONF=default
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+IMAGE_TYPE=debug
+OUTPUT_SUFFIX=elf
+DEBUGGABLE_SUFFIX=elf
+FINAL_IMAGE=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+else
+IMAGE_TYPE=production
+OUTPUT_SUFFIX=hex
+DEBUGGABLE_SUFFIX=elf
+FINAL_IMAGE=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+endif
+
+# Object Directory
+OBJECTDIR=build/${CND_CONF}/${IMAGE_TYPE}
+
+# Distribution Directory
+DISTDIR=dist/${CND_CONF}/${IMAGE_TYPE}
+
+# Source Files Quoted if spaced
+SOURCEFILES_QUOTED_IF_SPACED=../../MPU6050.c ../../../I2Cdev/I2Cdev.c main.c
+
+# Object Files Quoted if spaced
+OBJECTFILES_QUOTED_IF_SPACED=${OBJECTDIR}/_ext/43898991/MPU6050.p1 ${OBJECTDIR}/_ext/105797386/I2Cdev.p1 ${OBJECTDIR}/main.p1
+POSSIBLE_DEPFILES=${OBJECTDIR}/_ext/43898991/MPU6050.p1.d ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d ${OBJECTDIR}/main.p1.d
+
+# Object Files
+OBJECTFILES=${OBJECTDIR}/_ext/43898991/MPU6050.p1 ${OBJECTDIR}/_ext/105797386/I2Cdev.p1 ${OBJECTDIR}/main.p1
+
+# Source Files
+SOURCEFILES=../../MPU6050.c ../../../I2Cdev/I2Cdev.c main.c
+
+
+CFLAGS=
+ASFLAGS=
+LDLIBSOPTIONS=
+
+############# Tool locations ##########################################
+# If you copy a project from one host to another, the path where the  #
+# compiler is installed may be different.                             #
+# If you open this project with MPLAB X in the new host, this         #
+# makefile will be regenerated and the paths will be corrected.       #
+#######################################################################
+# fixDeps replaces a bunch of sed/cat/printf statements that slow down the build
+FIXDEPS=fixDeps
+
+.build-conf:  ${BUILD_SUBPROJECTS}
+	${MAKE}  -f nbproject/Makefile-default.mk dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+
+MP_PROCESSOR_OPTION=18F46J50
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: compile
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+${OBJECTDIR}/_ext/43898991/MPU6050.p1: ../../MPU6050.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR}/_ext/43898991 
+	@${RM} ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d 
+	@${RM} ${OBJECTDIR}/_ext/43898991/MPU6050.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  -D__DEBUG=1 --debugger=pickit3  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/_ext/43898991/MPU6050.p1  ../../MPU6050.c 
+	@-${MV} ${OBJECTDIR}/_ext/43898991/MPU6050.d ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+${OBJECTDIR}/_ext/105797386/I2Cdev.p1: ../../../I2Cdev/I2Cdev.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR}/_ext/105797386 
+	@${RM} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d 
+	@${RM} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  -D__DEBUG=1 --debugger=pickit3  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/_ext/105797386/I2Cdev.p1  ../../../I2Cdev/I2Cdev.c 
+	@-${MV} ${OBJECTDIR}/_ext/105797386/I2Cdev.d ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+${OBJECTDIR}/main.p1: main.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR} 
+	@${RM} ${OBJECTDIR}/main.p1.d 
+	@${RM} ${OBJECTDIR}/main.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  -D__DEBUG=1 --debugger=pickit3  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/main.p1  main.c 
+	@-${MV} ${OBJECTDIR}/main.d ${OBJECTDIR}/main.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/main.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+else
+${OBJECTDIR}/_ext/43898991/MPU6050.p1: ../../MPU6050.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR}/_ext/43898991 
+	@${RM} ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d 
+	@${RM} ${OBJECTDIR}/_ext/43898991/MPU6050.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/_ext/43898991/MPU6050.p1  ../../MPU6050.c 
+	@-${MV} ${OBJECTDIR}/_ext/43898991/MPU6050.d ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/_ext/43898991/MPU6050.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+${OBJECTDIR}/_ext/105797386/I2Cdev.p1: ../../../I2Cdev/I2Cdev.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR}/_ext/105797386 
+	@${RM} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d 
+	@${RM} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/_ext/105797386/I2Cdev.p1  ../../../I2Cdev/I2Cdev.c 
+	@-${MV} ${OBJECTDIR}/_ext/105797386/I2Cdev.d ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/_ext/105797386/I2Cdev.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+${OBJECTDIR}/main.p1: main.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} ${OBJECTDIR} 
+	@${RM} ${OBJECTDIR}/main.p1.d 
+	@${RM} ${OBJECTDIR}/main.p1 
+	${MP_CC} --pass1 $(MP_EXTRA_CC_PRE) --chip=$(MP_PROCESSOR_OPTION) -Q -G  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"    -o${OBJECTDIR}/main.p1  main.c 
+	@-${MV} ${OBJECTDIR}/main.d ${OBJECTDIR}/main.p1.d 
+	@${FIXDEPS} ${OBJECTDIR}/main.p1.d $(SILENT) -rsi ${MP_CC_DIR}../  
+	
+endif
+
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: assemble
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+else
+endif
+
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: link
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}: ${OBJECTFILES}  nbproject/Makefile-${CND_CONF}.mk    
+	@${MKDIR} dist/${CND_CONF}/${IMAGE_TYPE} 
+	${MP_CC} $(MP_EXTRA_LD_PRE) --chip=$(MP_PROCESSOR_OPTION) -G -mdist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.map  -D__DEBUG=1 --debugger=pickit3  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"        -odist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${DEBUGGABLE_SUFFIX}  ${OBJECTFILES_QUOTED_IF_SPACED}     
+	@${RM} dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.hex 
+	
+else
+dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}: ${OBJECTFILES}  nbproject/Makefile-${CND_CONF}.mk   
+	@${MKDIR} dist/${CND_CONF}/${IMAGE_TYPE} 
+	${MP_CC} $(MP_EXTRA_LD_PRE) --chip=$(MP_PROCESSOR_OPTION) -G -mdist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.map  --double=24 --float=24 --emi=wordwrite --opt=default,+asm,-asmfile,+speed,-space,-debug --addrqual=ignore --mode=pro -P -N255 -I"../../../I2Cdev" -I"../../" --warn=0 --asmlist --summary=default,-psect,-class,+mem,-hex,-file --output=default,-inhx032 --runtime=default,+clear,+init,-keep,-no_startup,-download,+config,+clib,+plib --output=-mcof,+elf:multilocs --stack=compiled:auto:auto:auto "--errformat=%f:%l: error: (%n) %s" "--warnformat=%f:%l: warning: (%n) %s" "--msgformat=%f:%l: advisory: (%n) %s"     -odist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${DEBUGGABLE_SUFFIX}  ${OBJECTFILES_QUOTED_IF_SPACED}     
+	
+endif
+
+
+# Subprojects
+.build-subprojects:
+
+
+# Subprojects
+.clean-subprojects:
+
+# Clean Targets
+.clean-conf: ${CLEAN_SUBPROJECTS}
+	${RM} -r build/default
+	${RM} -r dist/default
+
+# Enable dependency checking
+.dep.inc: .depcheck-impl
+
+DEPFILES=$(shell mplabwildcard ${POSSIBLE_DEPFILES})
+ifneq (${DEPFILES},)
+include ${DEPFILES}
+endif
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-genesis.properties b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-genesis.properties
new file mode 100644
index 00000000..441f9906
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-genesis.properties
@@ -0,0 +1,8 @@
+#
+#Fri Nov 28 22:55:53 CET 2014
+default.languagetoolchain.dir=C\:\\Program Files (x86)\\Microchip\\xc8\\v1.32\\bin
+com-microchip-mplab-nbide-embedded-makeproject-MakeProject.md5=20b996ec0591c93034157100c3d90ac3
+default.languagetoolchain.version=1.32
+host.platform=windows
+conf.ids=default
+default.com-microchip-mplab-nbide-toolchainXC8-XC8LanguageToolchain.md5=533e5c34133c5d768bcb87ffcec948e1
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-impl.mk b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-impl.mk
new file mode 100644
index 00000000..8897f7f2
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-impl.mk
@@ -0,0 +1,69 @@
+#
+# Generated Makefile - do not edit!
+#
+# Edit the Makefile in the project folder instead (../Makefile). Each target
+# has a pre- and a post- target defined where you can add customization code.
+#
+# This makefile implements macros and targets common to all configurations.
+#
+# NOCDDL
+
+
+# Building and Cleaning subprojects are done by default, but can be controlled with the SUB
+# macro. If SUB=no, subprojects will not be built or cleaned. The following macro
+# statements set BUILD_SUB-CONF and CLEAN_SUB-CONF to .build-reqprojects-conf
+# and .clean-reqprojects-conf unless SUB has the value 'no'
+SUB_no=NO
+SUBPROJECTS=${SUB_${SUB}}
+BUILD_SUBPROJECTS_=.build-subprojects
+BUILD_SUBPROJECTS_NO=
+BUILD_SUBPROJECTS=${BUILD_SUBPROJECTS_${SUBPROJECTS}}
+CLEAN_SUBPROJECTS_=.clean-subprojects
+CLEAN_SUBPROJECTS_NO=
+CLEAN_SUBPROJECTS=${CLEAN_SUBPROJECTS_${SUBPROJECTS}}
+
+
+# Project Name
+PROJECTNAME=MPU6050_raw.X
+
+# Active Configuration
+DEFAULTCONF=default
+CONF=${DEFAULTCONF}
+
+# All Configurations
+ALLCONFS=default 
+
+
+# build
+.build-impl: .build-pre
+	${MAKE} -f nbproject/Makefile-${CONF}.mk SUBPROJECTS=${SUBPROJECTS} .build-conf
+
+
+# clean
+.clean-impl: .clean-pre
+	${MAKE} -f nbproject/Makefile-${CONF}.mk SUBPROJECTS=${SUBPROJECTS} .clean-conf
+
+# clobber
+.clobber-impl: .clobber-pre .depcheck-impl
+	    ${MAKE} SUBPROJECTS=${SUBPROJECTS} CONF=default clean
+
+
+
+# all
+.all-impl: .all-pre .depcheck-impl
+	    ${MAKE} SUBPROJECTS=${SUBPROJECTS} CONF=default build
+
+
+
+# dependency checking support
+.depcheck-impl:
+#	@echo "# This code depends on make tool being used" >.dep.inc
+#	@if [ -n "${MAKE_VERSION}" ]; then \
+#	    echo "DEPFILES=\$$(wildcard \$$(addsuffix .d, \$${OBJECTFILES}))" >>.dep.inc; \
+#	    echo "ifneq (\$${DEPFILES},)" >>.dep.inc; \
+#	    echo "include \$${DEPFILES}" >>.dep.inc; \
+#	    echo "endif" >>.dep.inc; \
+#	else \
+#	    echo ".KEEP_STATE:" >>.dep.inc; \
+#	    echo ".KEEP_STATE_FILE:.make.state.\$${CONF}" >>.dep.inc; \
+#	fi
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-local-default.mk b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-local-default.mk
new file mode 100644
index 00000000..dd484ed2
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-local-default.mk
@@ -0,0 +1,37 @@
+#
+# Generated Makefile - do not edit!
+#
+#
+# This file contains information about the location of compilers and other tools.
+# If you commmit this file into your revision control server, you will be able to 
+# to checkout the project and build it from the command line with make. However,
+# if more than one person works on the same project, then this file might show
+# conflicts since different users are bound to have compilers in different places.
+# In that case you might choose to not commit this file and let MPLAB X recreate this file
+# for each user. The disadvantage of not commiting this file is that you must run MPLAB X at
+# least once so the file gets created and the project can be built. Finally, you can also
+# avoid using this file at all if you are only building from the command line with make.
+# You can invoke make with the values of the macros:
+# $ makeMP_CC="/opt/microchip/mplabc30/v3.30c/bin/pic30-gcc" ...  
+#
+SHELL=cmd.exe
+PATH_TO_IDE_BIN=C:/Program Files (x86)/Microchip/MPLABX/mplab_ide/mplab_ide/modules/../../bin/
+# Adding MPLAB X bin directory to path.
+PATH:=C:/Program Files (x86)/Microchip/MPLABX/mplab_ide/mplab_ide/modules/../../bin/:$(PATH)
+# Path to java used to run MPLAB X when this makefile was created
+MP_JAVA_PATH="C:\Program Files (x86)\Microchip\MPLABX\sys\java\jre1.7.0_25-windows-x64\java-windows/bin/"
+OS_CURRENT="$(shell uname -s)"
+MP_CC="C:\Program Files (x86)\Microchip\xc8\v1.32\bin\xc8.exe"
+# MP_CPPC is not defined
+# MP_BC is not defined
+MP_AS="C:\Program Files (x86)\Microchip\xc8\v1.32\bin\xc8.exe"
+# MP_LD is not defined
+# MP_AR is not defined
+DEP_GEN=${MP_JAVA_PATH}java -jar "C:/Program Files (x86)/Microchip/MPLABX/mplab_ide/mplab_ide/modules/../../bin/extractobjectdependencies.jar" 
+MP_CC_DIR="C:\Program Files (x86)\Microchip\xc8\v1.32\bin"
+# MP_CPPC_DIR is not defined
+# MP_BC_DIR is not defined
+MP_AS_DIR="C:\Program Files (x86)\Microchip\xc8\v1.32\bin"
+# MP_LD_DIR is not defined
+# MP_AR_DIR is not defined
+# MP_BC_DIR is not defined
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-variables.mk b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-variables.mk
new file mode 100644
index 00000000..623663b1
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Makefile-variables.mk
@@ -0,0 +1,13 @@
+#
+# Generated - do not edit!
+#
+# NOCDDL
+#
+CND_BASEDIR=`pwd`
+# default configuration
+CND_ARTIFACT_DIR_default=dist/default/production
+CND_ARTIFACT_NAME_default=MPU6050_raw.X.production.hex
+CND_ARTIFACT_PATH_default=dist/default/production/MPU6050_raw.X.production.hex
+CND_PACKAGE_DIR_default=${CND_DISTDIR}/default/package
+CND_PACKAGE_NAME_default=mpu6050raw.x.tar
+CND_PACKAGE_PATH_default=${CND_DISTDIR}/default/package/mpu6050raw.x.tar
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Package-default.bash b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Package-default.bash
new file mode 100644
index 00000000..2b42e19c
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/Package-default.bash
@@ -0,0 +1,73 @@
+#!/bin/bash -x
+
+#
+# Generated - do not edit!
+#
+
+# Macros
+TOP=`pwd`
+CND_CONF=default
+CND_DISTDIR=dist
+TMPDIR=build/${CND_CONF}/${IMAGE_TYPE}/tmp-packaging
+TMPDIRNAME=tmp-packaging
+OUTPUT_PATH=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+OUTPUT_BASENAME=MPU6050_raw.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+PACKAGE_TOP_DIR=mpu6050raw.x/
+
+# Functions
+function checkReturnCode
+{
+    rc=$?
+    if [ $rc != 0 ]
+    then
+        exit $rc
+    fi
+}
+function makeDirectory
+# $1 directory path
+# $2 permission (optional)
+{
+    mkdir -p "$1"
+    checkReturnCode
+    if [ "$2" != "" ]
+    then
+      chmod $2 "$1"
+      checkReturnCode
+    fi
+}
+function copyFileToTmpDir
+# $1 from-file path
+# $2 to-file path
+# $3 permission
+{
+    cp "$1" "$2"
+    checkReturnCode
+    if [ "$3" != "" ]
+    then
+        chmod $3 "$2"
+        checkReturnCode
+    fi
+}
+
+# Setup
+cd "${TOP}"
+mkdir -p ${CND_DISTDIR}/${CND_CONF}/package
+rm -rf ${TMPDIR}
+mkdir -p ${TMPDIR}
+
+# Copy files and create directories and links
+cd "${TOP}"
+makeDirectory ${TMPDIR}/mpu6050raw.x/bin
+copyFileToTmpDir "${OUTPUT_PATH}" "${TMPDIR}/${PACKAGE_TOP_DIR}bin/${OUTPUT_BASENAME}" 0755
+
+
+# Generate tar file
+cd "${TOP}"
+rm -f ${CND_DISTDIR}/${CND_CONF}/package/mpu6050raw.x.tar
+cd ${TMPDIR}
+tar -vcf ../../../../${CND_DISTDIR}/${CND_CONF}/package/mpu6050raw.x.tar *
+checkReturnCode
+
+# Cleanup
+cd "${TOP}"
+rm -rf ${TMPDIR}
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/configurations.xml b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/configurations.xml
new file mode 100644
index 00000000..3b6fba66
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/configurations.xml
@@ -0,0 +1,173 @@
+
+
+  
+    
+      ../../../I2Cdev/I2Cdev.h
+      ../../MPU6050.h
+    
+    
+    
+    
+      ../../MPU6050.c
+      ../../../I2Cdev/I2Cdev.c
+      main.c
+    
+    
+      Makefile
+    
+  
+  
+    C:\Users\sebokmarton\Documents\i2cdevlib\PIC18\I2Cdev
+    ../..
+  
+  Makefile
+  
+    
+      
+        localhost
+        PIC18F46J50
+        
+        
+        PICkit3PlatformTool
+        XC8
+        1.32
+        3
+      
+      
+        
+          
+          
+        
+        
+        
+        
+          false
+          false
+          
+        
+      
+      
+        false
+        
+        false
+        
+        false
+        false
+        false
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+      
+    
+  
+
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/project.properties b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/project.properties
new file mode 100644
index 00000000..e69de29b
diff --git a/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/project.xml b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/project.xml
new file mode 100644
index 00000000..18806930
--- /dev/null
+++ b/PIC18/MPU6050/Examples/MPU6050_raw.X/nbproject/project.xml
@@ -0,0 +1,16 @@
+
+    com.microchip.mplab.nbide.embedded.makeproject
+    
+        
+            MPU6050_raw
+            84892e89-df8e-441e-9542-2524d6b2dc5a
+            0
+            c
+            
+            h
+            
+            UTF-8
+            
+        
+    
+
diff --git a/PIC18/MPU6050/MPU6050.c b/PIC18/MPU6050/MPU6050.c
new file mode 100644
index 00000000..2683529a
--- /dev/null
+++ b/PIC18/MPU6050/MPU6050.c
@@ -0,0 +1,3141 @@
+// 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 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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"
+
+MPU6050_t mpu6050;
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+void MPU6050(uint8_t address) {
+    mpu6050.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() {
+    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    MPU6050_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 MPU6050_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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_SMPLRT_DIV, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050_getExternalFrameSync() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// 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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050_getDHPFMode() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_THR, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+/** Set free-fall 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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getTempFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getXGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getYGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getZGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getAccelFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave2FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave1FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave0FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+bool MPU6050_getSlave3FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050_getMasterClockSpeed() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_ADDR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_REG, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_DI, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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) {
+    MPU6050_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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, mpu6050.buffer);
+    *ax = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *ay = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *az = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
+    *gx = (((int16_t)mpu6050.buffer[8]) << 8) | mpu6050.buffer[9];
+    *gy = (((int16_t)mpu6050.buffer[10]) << 8) | mpu6050.buffer[11];
+    *gz = (((int16_t)mpu6050.buffer[12]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_TEMP_OUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, mpu6050.buffer);
+    return (((uint32_t)mpu6050.buffer[0]) << 24) | (((uint32_t)mpu6050.buffer[1]) << 16) | (((uint16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+}
+
+// MOT_DETECT_STATUS register
+
+/** 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer is disabled. The FIFO mpu6050.buffer
+ * cannot be written to or read from while disabled. The FIFO mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO mpu6050.buffer size.
+ * This value indicates the number of bytes stored in the FIFO mpu6050.buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO mpu6050.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 mpu6050.buffer size
+ */
+uint16_t MPU6050_getFIFOCount() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_COUNTH, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO mpu6050.buffer.
+ * This register is used to read and write data from the FIFO mpu6050.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 mpu6050.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 mpu6050.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 mpu6050.buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO mpu6050.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 mpu6050.buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO mpu6050.buffer
+ */
+uint8_t MPU6050_getFIFOByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO mpu6050.buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_setFIFOByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.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(mpu6050.devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setOTPBankValid(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050_getXGyroOffsetTC() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050_getYFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050_getZFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050_getXAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050_getYAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050_getZAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050_getXGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050_getYGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050_getZGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050_getIntPLLReadyEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050_getIntDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050_getDMPInt5Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt4Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt3Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt2Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt1Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt0Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050_getIntPLLReadyStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getIntDMPStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_getDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_resetDMP() {
+    I2Cdev_writeBit(mpu6050.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(mpu6050.devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_setMemoryStartAddress(uint8_t address) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050_readMemoryByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_writeMemoryByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_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(mpu6050.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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+        }
+    }
+}
+/*bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer;
+    uint8_t *progBuffer;
+    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(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+            I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                //Serial.print("Block write verification error, bank ");
+                //Serial.print(bank, DEC);
+                //Serial.print(", address ");
+                //Serial.print(address, DEC);
+                //Serial.print("!\nExpected:");
+                //for (j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (progBuffer[j] < 16) Serial.print("0");
+                //    Serial.print(progBuffer[j], HEX);
+                //}
+                //Serial.print("\nReceived:");
+                //for (uint8_t j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                //    Serial.print(verifyBuffer[i + j], HEX);
+                //}
+                Serial.print("\n");
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_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 MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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 = MPU6050_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(mpu6050.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 MPU6050_writeDMPConfigurationSet(data, dataSize, true);
+}*/
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_getDMPConfig1() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig1(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050_getDMPConfig2() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
\ No newline at end of file
diff --git a/PIC18/MPU6050/MPU6050.h b/PIC18/MPU6050/MPU6050.h
new file mode 100644
index 00000000..4f35e336
--- /dev/null
+++ b/PIC18/MPU6050/MPU6050.h
@@ -0,0 +1,786 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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 "../I2Cdev/I2Cdev.h"
+
+#if ((defined MPU6050_INCLUDE_DMP_MOTIONAPPS20) || (defined MPU6050_INCLUDE_DMP_MOTIONAPPS41))
+    #error DMP is not supported yet
+#endif
+
+#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_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_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
+
+typedef struct MPU6050_t {
+    uint8_t devAddr;
+    uint8_t buffer[14];
+} MPU6050_t;
+
+void MPU6050(uint8_t address);
+
+void MPU6050_initialize();
+bool MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8_t MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8_t level);
+
+// SMPLRT_DIV register
+uint8_t MPU6050_getRate();
+void MPU6050_setRate(uint8_t rate);
+
+// CONFIG register
+uint8_t MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8_t sync);
+uint8_t MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8_t bandwidth);
+
+// GYRO_CONFIG register
+uint8_t MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8_t range);
+
+// ACCEL_CONFIG register
+bool MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+bool MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+bool MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8_t MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8_t range);
+uint8_t MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8_t mode);
+
+// FF_THR register
+uint8_t MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8_t threshold);
+
+// FF_DUR register
+uint8_t MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8_t duration);
+
+// MOT_THR register
+uint8_t MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8_t threshold);
+
+// MOT_DUR register
+uint8_t MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8_t duration);
+
+// ZRMOT_THR register
+uint8_t MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8_t threshold);
+
+// ZRMOT_DUR register
+uint8_t MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8_t duration);
+
+// FIFO_EN register
+bool MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+bool MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+bool MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+bool MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+bool MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+bool MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+bool MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+bool MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+bool MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+bool MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+bool MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+bool MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8_t MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8_t speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8_t MPU6050_getSlaveAddress(uint8_t num);
+void MPU6050_setSlaveAddress(uint8_t num, uint8_t address);
+uint8_t MPU6050_getSlaveRegister(uint8_t num);
+void MPU6050_setSlaveRegister(uint8_t num, uint8_t reg);
+bool MPU6050_getSlaveEnabled(uint8_t num);
+void MPU6050_setSlaveEnabled(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWordByteSwap(uint8_t num);
+void MPU6050_setSlaveWordByteSwap(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWriteMode(uint8_t num);
+void MPU6050_setSlaveWriteMode(uint8_t num, bool mode);
+bool MPU6050_getSlaveWordGroupOffset(uint8_t num);
+void MPU6050_setSlaveWordGroupOffset(uint8_t num, bool enabled);
+uint8_t MPU6050_getSlaveDataLength(uint8_t num);
+void MPU6050_setSlaveDataLength(uint8_t num, uint8_t length);
+
+// I2C_SLV* registers (Slave 4)
+uint8_t MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8_t address);
+uint8_t MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8_t reg);
+void MPU6050_setSlave4OutputByte(uint8_t data);
+bool MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+bool MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+bool MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8_t MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8_t delay);
+uint8_t MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+bool MPU6050_getPassthroughStatus();
+bool MPU6050_getSlave4IsDone();
+bool MPU6050_getLostArbitration();
+bool MPU6050_getSlave4Nack();
+bool MPU6050_getSlave3Nack();
+bool MPU6050_getSlave2Nack();
+bool MPU6050_getSlave1Nack();
+bool MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+bool MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+bool MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+bool MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+bool MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+bool MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+bool MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+bool MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+bool MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8_t MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8_t enabled);
+bool MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+bool MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+bool MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+bool MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+bool MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+bool MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8_t MPU6050_getIntStatus();
+bool MPU6050_getIntFreefallStatus();
+bool MPU6050_getIntMotionStatus();
+bool MPU6050_getIntZeroMotionStatus();
+bool MPU6050_getIntFIFOBufferOverflowStatus();
+bool MPU6050_getIntI2CMasterStatus();
+bool MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getAccelerationX();
+int16_t MPU6050_getAccelerationY();
+int16_t MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16_t MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getRotationX();
+int16_t MPU6050_getRotationY();
+int16_t MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8_t MPU6050_getExternalSensorByte(int position);
+uint16_t MPU6050_getExternalSensorWord(int position);
+uint32_t getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+bool MPU6050_getXNegMotionDetected();
+bool MPU6050_getXPosMotionDetected();
+bool MPU6050_getYNegMotionDetected();
+bool MPU6050_getYPosMotionDetected();
+bool MPU6050_getZNegMotionDetected();
+bool MPU6050_getZPosMotionDetected();
+bool MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8_t num, uint8_t data);
+
+// I2C_MST_DELAY_CTRL register
+bool MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+bool MPU6050_getSlaveDelayEnabled(uint8_t num);
+void MPU6050_setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8_t MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8_t delay);
+uint8_t MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8_t decrement);
+uint8_t MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8_t decrement);
+
+// USER_CTRL register
+bool MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+bool MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+bool MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+bool MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+bool MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8_t MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8_t source);
+
+// PWR_MGMT_2 register
+uint8_t MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8_t frequency);
+bool MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+bool MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+bool MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+bool MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+bool MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+bool MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16_t MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8_t MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8_t data);
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length);
+
+// WHO_AM_I register
+uint8_t MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8_t id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8_t MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8_t getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8_t offset);
+
+// YG_OFFS_TC register
+int8_t getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8_t offset);
+
+// ZG_OFFS_TC register
+int8_t getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8_t offset);
+
+// X_FINE_GAIN register
+int8_t getXFineGain();
+void MPU6050_setXFineGain(int8_t gain);
+
+// Y_FINE_GAIN register
+int8_t getYFineGain();
+void MPU6050_setYFineGain(int8_t gain);
+
+// Z_FINE_GAIN register
+int8_t getZFineGain();
+void MPU6050_setZFineGain(int8_t gain);
+
+// XA_OFFS_* registers
+int16_t MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16_t offset);
+
+// YA_OFFS_* register
+int16_t MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16_t offset);
+
+// ZA_OFFS_* register
+int16_t MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16_t offset);
+
+// XG_OFFS_USR* registers
+int16_t MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16_t offset);
+
+// YG_OFFS_USR* register
+int16_t MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16_t offset);
+
+// ZG_OFFS_USR* register
+int16_t MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16_t offset);
+
+// INT_ENABLE register (DMP functions)
+bool MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+bool MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+bool MPU6050_getDMPInt5Status();
+bool MPU6050_getDMPInt4Status();
+bool MPU6050_getDMPInt3Status();
+bool MPU6050_getDMPInt2Status();
+bool MPU6050_getDMPInt1Status();
+bool MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+bool MPU6050_getIntPLLReadyStatus();
+bool MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+bool MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank);
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8_t address);
+
+// MEM_R_W register
+uint8_t MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8_t data);
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address);
+//bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem);
+//bool MPU6050_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify);
+
+//bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem);
+//bool MPU6050_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
+
+// DMP_CFG_1 register
+uint8_t MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8_t config);
+
+// DMP_CFG_2 register
+uint8_t MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8_t config);
+
+#endif /* _MPU6050_H_ */
diff --git a/PIC18/README.md b/PIC18/README.md
new file mode 100644
index 00000000..858ae2bf
--- /dev/null
+++ b/PIC18/README.md
@@ -0,0 +1,14 @@
+# I2C Device Library for PIC18
+
+## I2Cdev
+We use XC8's peripheral libraries (_plib_), read timeout is not implemented.
+
+## Devices
+Currently only MPU6050 is supported without DMP functions. There's an example MPLABX project showing how to read raw data from the MPU.
+
+Adding more functions and devices should be straighforward after reading the source code.
+
+## Licence
+I2Cdev device library code is placed under the MIT license.
+
+_Copyright (c) 2011 Jeff Rowberg. Copyright (c) 2014 Marton Sebok._
diff --git a/README b/README
deleted file mode 100644
index 5eb328ba..00000000
--- a/README
+++ /dev/null
@@ -1,15 +0,0 @@
-============================================================================
-Note: for details about this project, please visit: http://www.i2cdevlib.com
-============================================================================
-
-The I2C Device Library (i2cdevlib) is a collection of uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices. Each device is built to make use of the generic "I2Cdev" class, which abstracts the I2C bit- and byte-level communication away from each specific device class, making it easy to keep the device class clean while providing a simple way to modify just one class to port the I2C communication code onto different platforms (Arduino, PIC, MSP430, simple bit-banging, etc.). Device classes are designed to provide complete coverage of all functionality described by each device's documentation, plus any generic convenience functions that are helpful.
-
-The code is written primarily to support the Arduino/Wiring implementation, but it may be useful in other circumstances. There are multiple I2C/TWI implementations selectable in the I2Cdev.h header file if the default Arduino "Wire.h" is not available or preferable for any reason.
-
-There are examples in many of the classes that demonstrate basic usage patterns. The I2Cdev class is built to be used statically, reducing the memory requirement if you have multiple I2C devices in your project. Only one instance of the I2Cdev class is required.
-
-Documentation for each class is created using Doxygen-style comments placed in each class definition file, based on the information available in each device's datasheet. This documentation is available in HTML format on the i2cdevlib.com website, which also holds helpful information for most of the classes present here on the repository.
-
-To use the library, just place the I2Cdev .cpp/.h source files and any device library .cpp/.h source files in the same folder as your sketch (or a suitable place relative to your avr-gcc project), and include just the device library headers that you need. Arduino users will also need to include  in your main sketch source file. Create a single device object (e.g. "ADXL345 accel();"), place any appropriate init function calls in your setup() routine (e.g. "accel.initialize();"), and off you go! See the example sketches inside many of the device class directories for reference.
-
-Want a library for a device that isn't up on the repository? Request it, or fork the code and contribute! Better yet, send me a device on a breakout board to test the code during development. No guarantees on how fast I can get it done, but I'd love to make this the biggest consistent and well-documented I2C device library around.
\ No newline at end of file
diff --git a/README.md b/README.md
new file mode 100644
index 00000000..f1ea8561
--- /dev/null
+++ b/README.md
@@ -0,0 +1,40 @@
+# I2C Device Library
+
+The I2C Device Library (i2cdevlib) is a collection of mostly uniform and well-documented classes to provide simple and intuitive interfaces to I2C devices. Each device is built to make use of the generic "I2Cdev" class, which abstracts the I2C bit- and byte-level communication away from each specific device class, making it easy to keep the device class clean while providing a simple way to modify just one class to port the I2C communication code onto different platforms (Arduino, PIC, MSP430, Jennic, simple bit-banging, etc.). Device classes are designed to provide complete coverage of all functionality described by each device's documentation, plus any generic convenience functions that are helpful.
+
+There are examples in many of the classes that demonstrate basic usage patterns. The I2Cdev class is built to be used statically, reducing the memory requirement if you have multiple I2C devices in your project. Only one instance of the I2Cdev class is required. Recent additions as of late 2021 have also made it possible to pass in non-default `Wire` objects (in the Arduino environment) to allow using multiple I2C transceivers at the same time, specifically because of the number of people who wanted to use up to four MPU-6050 IMUs without I2C mux ICs involved.
+
+Documentation for each class is created using Doxygen-style comments placed in each class definition file, based on the information available in each device's datasheet. This documentation is available in HTML format on the i2cdevlib.com website, which also holds helpful information for most of the classes present here on the repository.
+
+## Installation
+
+Due to my...ahem...unfortunate ignorance way back when I first created this project, the entire codebase (all platforms, cores, and device libraries) are all inside of this one giant repository. That means there's no easy IDE integration the way most libraries work in the Arduino world and elsewhere. Instead, do the following:
+
+1. Clone or [download a .zip archive](https://github.com/jrowberg/i2cdevlib/archive/refs/heads/master.zip) of the repo
+2. Move or copy the relevant core and device drivers into your project tree or library subfolder
(For Arduino, this means the `/Arduino/I2Cdev` and `/Arduino/MPU6050` folders, for example)
+3. Rescan libraries or restart your IDE if necessary
+
+For both usage and development, I've found that it's best to clone using the git client of your choice, and then create symlinks as needed from the master repository sources into your development location(s). This is usually more intuitive for people who use Linux, but it can be done in Windows as well using the `mklink /D` command. See [this page](https://www.howtogeek.com/howto/16226/complete-guide-to-symbolic-links-symlinks-on-windows-or-linux/) for a set of Windows-specific instructions with screenshots.
+
+## Usage
+
+Exact usage varies from device to device, but most (especially the more popular ones) include example projects demonstrating the basics. Refer to those examples for the best material currently available.
+
+## Contributing
+
+Want a library for a device that isn't up on the repository? You can either request it in the discussion area for this repo on Github, or fork the code and write it yourself.
+
+Realistically, Option B is more reliable. Try to mimic the structure and code conventions of the existing codebase as much as possible. If you go this route, please use the following approach:
+
+1. Fork the repository to your own user
+2. Create a new branch specific to your new code
+3. Write, test, and commit your new code
+4. Submit a pull request from your branch back to the original source
+
+I and a few others will review the pull request and comment as needed, and then hopefully merge it.
+
+

+
+***Note: additional details about this project can be found at https://www.i2cdevlib.com***
+
+***Another note: this project has a fledgling successor that aims to [address all of its shortcomings](https://community.perilib.io/t/perilib-i2cdevlib-reborn/15), which can be found at https://github.com/perilib***
diff --git a/RP2040/I2Cdev/I2Cdev.cpp b/RP2040/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..0452d79d
--- /dev/null
+++ b/RP2040/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,332 @@
+// Raspberry Pi Pico port for:
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 2013-06-05 by Jeff Rowberg 
+//
+// Changelog:
+//		2021-09-29 - Initial port release by Gino Ipóliti.
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 "I2Cdev.h"
+
+/** Default constructor.
+ */
+I2Cdev::I2Cdev() {
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint32_t timeout) {
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint32_t timeout) {
+    uint16_t b;
+    uint8_t count = readWord(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint32_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint32_t timeout) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint32_t timeout) {
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint32_t timeout) {
+    return readWords(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint32_t timeout) {
+    uint8_t count = 0;
+
+    i2c_write_blocking(i2c_default, devAddr, ®Addr, 1, true);
+    count = i2c_read_timeout_us(i2c_default, devAddr, data, length, false, timeout * 1000);
+
+    return count;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint32_t timeout) {
+    uint8_t count = 0;
+    uint8_t data_buf[length*2];
+
+    i2c_write_blocking(i2c_default, devAddr, ®Addr, 1, true);
+    count = i2c_read_timeout_us(i2c_default, devAddr, data_buf, length*2, false, timeout * 1000);
+    for(int i=0; i> 8;
+        data_buf[j+1] = data[i];
+        j += 2;
+    }
+    status = i2c_write_blocking(i2c_default, devAddr, data_buf, new_len, false);
+
+    return status;
+}
+
+/** Default timeout value for read operations.
+ * Set this to 0 to disable timeout detection.
+ */
+uint32_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
diff --git a/RP2040/I2Cdev/I2Cdev.h b/RP2040/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..2a314ee0
--- /dev/null
+++ b/RP2040/I2Cdev/I2Cdev.h
@@ -0,0 +1,72 @@
+// Raspberry Pi Pico port for:
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 2013-06-05 by Jeff Rowberg 
+//
+// Changelog:
+//		2021-09-29 - Initial port release by Gino Ipóliti.
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 _I2CDEV_PICO_WRAPPER_H_
+#define _I2CDEV_PICO_WRAPPER_H_
+
+#include 
+#include 
+#include 
+#include 
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     ((uint32_t)1000000) // RP2040 I2C functions with timeout use microseconds so we have to multiply by 10^3
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint32_t timeout=I2Cdev::readTimeout);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint32_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint32_t readTimeout;
+
+};
+
+#endif /* _I2CDEV_PICO_WRAPPER_H_ */
diff --git a/RP2040/MPU6050/MPU6050.cpp b/RP2040/MPU6050/MPU6050.cpp
new file mode 100644
index 00000000..f15d2208
--- /dev/null
+++ b/RP2040/MPU6050/MPU6050.cpp
@@ -0,0 +1,3381 @@
+// I2Cdev library collection
+// Raspberry Pi Pico port for: 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 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2021-09-29 - Initial port release by Gino Ipóliti.
+// 2019-07-08 - Added Auto Calibration routine
+//     ... - ongoing debug release
+
+// 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.
+
+/* ============================================
+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"
+
+
+#ifndef BUFFER_LENGTH
+// band-aid fix for platforms without Wire-defined BUFFER_LENGTH (removed from some official implementations)
+#define BUFFER_LENGTH 32
+#endif
+
+static int addr = 0x68;
+
+int16_t map(int16_t x, int16_t in_min, int16_t in_max, int16_t out_min, int16_t out_max) {
+  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
+}
+
+/** Specific address constructor.
+ * @param address I2C address, uses default I2C address if none is specified
+ * @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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 

+ *
+ * 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;
+    }
+}
+
+/** Get latest byte from FIFO buffer no matter how much time has passed.
+ * ===                  GetCurrentFIFOPacket                    ===
+ * ================================================================
+ * Returns 1) when nothing special was done
+ *         2) when recovering from overflow
+ *         0) when no valid data is available
+ * ================================================================ */
+ int8_t MPU6050::GetCurrentFIFOPacket(uint8_t *data, uint8_t length) { // overflow proof
+     int16_t fifoC;
+     // This section of code is for when we allowed more than 1 packet to be acquired
+     //uint32_t BreakTimer = micros();
+     uint32_t BreakTimer = time_us_32();
+     do {
+         if ((fifoC = getFIFOCount())  > length) {
+
+             if (fifoC > 200) { // if you waited to get the FIFO buffer to > 200 bytes it will take longer to get the last packet in the FIFO Buffer than it will take to  reset the buffer and wait for the next to arrive
+                 resetFIFO(); // Fixes any overflow corruption
+                 fifoC = 0;
+                 while (!(fifoC = getFIFOCount()) && ((time_us_32() - BreakTimer) <= (11000))); // Get Next New Packet
+                 } else { //We have more than 1 packet but less than 200 bytes of data in the FIFO Buffer
+                 uint8_t Trash[BUFFER_LENGTH];
+                 while ((fifoC = getFIFOCount()) > length) {  // Test each time just in case the MPU is writing to the FIFO Buffer
+                     fifoC = fifoC - length; // Save the last packet
+                     uint16_t  RemoveBytes;
+                     while (fifoC) { // fifo count will reach zero so this is safe
+                         RemoveBytes = MIN((int)fifoC, BUFFER_LENGTH); // Buffer Length is different than the packet length this will efficiently clear the buffer
+                         getFIFOBytes(Trash, (uint8_t)RemoveBytes);
+                         fifoC -= RemoveBytes;
+                     }
+                 }
+             }
+         }
+         if (!fifoC) return 0; // Called too early no data or we timed out after FIFO Reset
+         // We have 1 packet
+         if ((time_us_32() - BreakTimer) > (11000)) return 0;
+     } while (fifoC != length);
+     getFIFOBytes(data, length); //Get 1 packet
+     return 1;
+}
+
+
+/** Write byte to FIFO buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050::setFIFOByte(uint8_t data) {
+    I2Cdev::writeByte(devAddr, MPU6050_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() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setXAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050::getYAccelOffset() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setYAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_YA_OFFS_H:0x7A); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050::getZAccelOffset() {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::readBytes(devAddr, SaveAddress, 2, buffer);
+	return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+void MPU6050::setZAccelOffset(int16_t offset) {
+	uint8_t SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D); // MPU6050,MPU9150 Vs MPU6500,MPU9250
+	I2Cdev::writeWord(devAddr, SaveAddress, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050::getXGyroOffset() {
+    I2Cdev::readBytes(devAddr, MPU6050_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) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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, false);
+}
+
+// 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 Routines            **********************
+//***************************************************************************************
+/**
+  @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);
+}
+
+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 ;
+	printf(">");
+	for (int i = 0; i < 3; i++) {
+		I2Cdev::readWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
+		Reading = Data;
+		if(SaveAddress != 0x13){
+			BitZero[i] = Data & 1;										 // Capture Bit Zero to properly handle Accelerometer calibration
+			ITerm[i] = ((float)Reading) * 8;
+			} else {
+			ITerm[i] = Reading * 4;
+		}
+	}
+	for (int L = 0; L < Loops; L++) {
+		eSample = 0;
+		for (int c = 0; c < 100; c++) {// 100 PI Calculations
+			eSum = 0;
+			for (int i = 0; i < 3; i++) {
+				I2Cdev::readWords(devAddr, ReadAddress + (i * 2), 1, (uint16_t *)&Data); // reads 1 or more 16 bit integers (Word)
+				Reading = Data;
+				if ((ReadAddress == 0x3B)&&(i == 2)) Reading -= 16384;	//remove Gravity
+				Error = -Reading;
+				eSum += abs(Reading);
+				PTerm = kP * Error;
+				ITerm[i] += (Error * 0.001) * kI;				// Integral term 1000 Calculations a second = 0.001
+				if(SaveAddress != 0x13){
+					Data = round((PTerm + ITerm[i] ) / 8);		//Compute PID Output
+					Data = ((Data)&0xFFFE) |BitZero[i];			// Insert Bit0 Saved at beginning
+				} else Data = round((PTerm + ITerm[i] ) / 4);	//Compute PID Output
+				I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+			}
+			if((c == 99) && eSum > 1000){						// Error is still to great to continue 
+				c = 0;
+				printf("*");
+			}
+			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
+			sleep_ms(1);
+		}
+		printf(".");
+		kP *= .75;
+		kI *= .75;
+		for (int i = 0; i < 3; i++){
+			if(SaveAddress != 0x13) {
+				Data = round((ITerm[i] ) / 8);		//Compute PID Output
+				Data = ((Data)&0xFFFE) |BitZero[i];	// Insert Bit0 Saved at beginning
+			} else Data = round((ITerm[i]) / 4);
+			I2Cdev::writeWords(devAddr, SaveAddress + (i * shift), 1, (uint16_t *)&Data);
+		}
+	}
+	resetFIFO();
+	resetDMP();
+}
+
+void MPU6050::PrintActiveOffsets() {
+	uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77;
+	int16_t Data[3];
+	if(AOffsetRegister == 0x06)	I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data);
+	else {
+		I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data);
+		I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1);
+		I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2);
+	}
+    printf("\nOffset Values on device:\n");
+	//	A_OFFSET_H_READ_A_OFFS(Data);
+    printf("Acc. X = %d, Y = %d, Z = %d\n", Data[0], Data[1], Data[2]);
+	I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data);
+	//	XG_OFFSET_H_READ_OFFS_USR(Data);    
+    printf("Gyro. X = %d, Y = %d, Z = %d\n", Data[0], Data[1], Data[2]);
+}
diff --git a/RP2040/MPU6050/MPU6050.h b/RP2040/MPU6050/MPU6050.h
new file mode 100644
index 00000000..1ea3c65f
--- /dev/null
+++ b/RP2040/MPU6050/MPU6050.h
@@ -0,0 +1,1032 @@
+// I2Cdev library collection
+// Raspberry Pi Pico port for: MPU6050 I2C device class
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 10/3/2011 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2021-09-29 - Initial port release by Gino Ipóliti.
+//     ... - ongoing debug release
+
+// 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.
+
+/* ============================================
+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 "I2Cdev.h"
+
+
+#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(uint8_t address=MPU6050_DEFAULT_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();
+		int8_t GetCurrentFIFOPacket(uint8_t *data, uint8_t length);
+        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);
+
+		
+        // Calibration Routines
+		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 math
+		void PrintActiveOffsets(); // See the results of the Calibration
+        
+
+
+
+        // special methods for MotionApps 2.0 implementation
+        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
+
+            uint8_t dmpInitialize();
+            bool dmpPacketAvailable();
+
+            uint8_t dmpSetFIFORate(uint8_t fifoRate);
+            uint8_t dmpGetFIFORate();
+            uint8_t dmpGetSampleStepSizeMS();
+            uint8_t dmpGetSampleFrequency();
+            int32_t dmpDecodeTemperature(int8_t tempReg);
+            
+            // Register callbacks after a packet of FIFO data is processed
+            //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+            //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
+            uint8_t dmpRunFIFORateProcesses();
+            
+            // Setup FIFO for various output
+            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
+            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
+            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+            // Get Fixed Point data from FIFO
+            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
+            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
+            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
+            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
+            
+            uint8_t dmpGetEuler(float *data, Quaternion *q);
+            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+            // Get Floating Point data from FIFO
+            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
+
+            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
+            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
+
+            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
+
+            uint8_t dmpInitFIFOParam();
+            uint8_t dmpCloseFIFO();
+            uint8_t dmpSetGyroDataSource(uint8_t source);
+            uint8_t dmpDecodeQuantizedAccel();
+            uint32_t dmpGetGyroSumOfSquare();
+            uint32_t dmpGetAccelSumOfSquare();
+            void dmpOverrideQuaternion(long *q);
+            uint16_t dmpGetFIFOPacketSize();
+            uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof
+        #endif
+
+        // special methods for MotionApps 4.1 implementation
+        #ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS41
+
+            uint8_t dmpInitialize();
+            bool dmpPacketAvailable();
+
+            uint8_t dmpSetFIFORate(uint8_t fifoRate);
+            uint8_t dmpGetFIFORate();
+            uint8_t dmpGetSampleStepSizeMS();
+            uint8_t dmpGetSampleFrequency();
+            int32_t dmpDecodeTemperature(int8_t tempReg);
+            
+            // Register callbacks after a packet of FIFO data is processed
+            //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+            //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func);
+            uint8_t dmpRunFIFORateProcesses();
+            
+            // Setup FIFO for various output
+            uint8_t dmpSendQuaternion(uint_fast16_t accuracy);
+            uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendPacketNumber(uint_fast16_t accuracy);
+            uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+            uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+            // Get Fixed Point data from FIFO
+            uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpSetLinearAccelFilterCoefficient(float coef);
+            uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+            uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+            uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q);
+            uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+            uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
+            uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0);
+            
+            uint8_t dmpGetEuler(float *data, Quaternion *q);
+            uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+            // Get Floating Point data from FIFO
+            uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0);
+            uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
+
+            uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData);
+            uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
+
+            uint8_t dmpSetFIFOProcessedCallback(void (*func) (void));
+
+            uint8_t dmpInitFIFOParam();
+            uint8_t dmpCloseFIFO();
+            uint8_t dmpSetGyroDataSource(uint8_t source);
+            uint8_t dmpDecodeQuantizedAccel();
+            uint32_t dmpGetGyroSumOfSquare();
+            uint32_t dmpGetAccelSumOfSquare();
+            void dmpOverrideQuaternion(long *q);
+            uint16_t dmpGetFIFOPacketSize();
+        #endif
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[14];
+    #if defined(MPU6050_INCLUDE_DMP_MOTIONAPPS20) or defined(MPU6050_INCLUDE_DMP_MOTIONAPPS41)
+        uint8_t *dmpPacketBuffer;
+        uint16_t dmpPacketSize;
+    #endif
+};
+
+#endif /* _MPU6050_H_ */
diff --git a/RP2040/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h b/RP2040/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h
new file mode 100644
index 00000000..0df64e7a
--- /dev/null
+++ b/RP2040/MPU6050/MPU6050_6Axis_MotionApps_V6_12.h
@@ -0,0 +1,563 @@
+// I2Cdev library collection
+// Raspberry Pi Pico port for: MPU6050 I2C device class, 6-axis MotionApps 6.12 implementation
+// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
+// 5/20/2013 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2021-09-29 - Initial port release by Gino Ipóliti.
+// 2019/7/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes.
+//           - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present
+//           - Run-time calibration routine is enabled which calibrates after no motion state is detected
+//           - once no motion state is detected Calibration completes within 0.5 seconds
+//           - The Drawback is that the firmware image is larger.
+//     ... - ongoing debug release
+
+/* ============================================
+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_6AXIS_MOTIONAPPS20_H_
+#define _MPU6050_6AXIS_MOTIONAPPS20_H_
+
+#include "I2Cdev.h"
+#include "helper_3dmath.h"
+#include "pico/double.h"
+
+// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 // same definitions Should work with V6.12
+
+#include "MPU6050.h"
+
+const double PI = 3.1415926535897932384626433832795;
+
+/* Source is from the InvenSense MotionApps v2 demo code. Original source is
+ * unavailable, unless you happen to be amazing as decompiling binary by
+ * hand (in which case, please contact me, and I'm totally serious).
+ *
+ * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
+ * DMP reverse-engineering he did to help make this bit of wizardry
+ * possible.
+ */
+
+#define MPU6050_DMP_CODE_SIZE       3062    // dmpMemory[]
+
+/* ================================================================ *
+ | Default MotionApps v6.12 28-byte FIFO packet structure:           |
+ |                                                                  |
+ | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ] |
+ |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  |
+ |                                                                  |
+ | [GYRO X][GYRO Y][GYRO Z][ACC X ][ACC Y ][ACC Z ]					|
+ |  16  17  18  19  20  21  22  23  24  25  26  27					|
+ * ================================================================ */
+
+// this block of memory gets written to the MPU on start-up, and it seems
+// to be volatile memory, so it has to be done each time (it only takes ~1
+// second though)
+
+// *** this is a capture of the DMP Firmware V6.1.2 after all the messy changes were made so we can just load it
+const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] = {
+/* bank # 0 */
+0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x03, 0x00, 0x00,
+0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
+0x03, 0x0C, 0x30, 0xC3, 0x0A, 0x74, 0x56, 0x2D, 0x0D, 0x62, 0xDB, 0xC7, 0x16, 0xF4, 0xBA, 0x02,
+0x38, 0x83, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83, 0x25, 0x8E, 0xF8, 0x83, 0x30, 0x00, 0xF8, 0x83,
+0xFF, 0xFF, 0xFF, 0xFF, 0x0C, 0xBD, 0xD8, 0x11, 0x24, 0x00, 0x04, 0x00, 0x1A, 0x82, 0x79, 0xA1,
+0x00, 0x36, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00, 0x00, 0x38, 0x83, 0x6F, 0xA2,
+0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
+0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
+0x1F, 0xA4, 0xE8, 0xE4, 0xFF, 0xF5, 0xDC, 0xB9, 0x00, 0x5B, 0x79, 0xCF, 0x1F, 0x3F, 0x78, 0x76,
+0x00, 0x86, 0x7C, 0x5A, 0x00, 0x86, 0x23, 0x47, 0xFA, 0xB9, 0x86, 0x31, 0x00, 0x74, 0x87, 0x8A,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x43, 0x05, 0xFF, 0xFF, 0xE9, 0xA8, 0x00, 0x00, 0x21, 0x82,
+0xFA, 0xB8, 0x4D, 0x46, 0xFF, 0xFA, 0xDF, 0x3D, 0xFF, 0xFF, 0xB2, 0xB3, 0x00, 0x00, 0x00, 0x00,
+0x3F, 0xFF, 0xBA, 0x98, 0x00, 0x5D, 0xAC, 0x08, 0x00, 0x0A, 0x63, 0x78, 0x00, 0x01, 0x46, 0x21,
+0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x42, 0xB5, 0x00, 0x06, 0x00, 0x64, 0x00, 0x64, 0x00, 0x06,
+0x14, 0x06, 0x02, 0x9F, 0x0F, 0x47, 0x91, 0x32, 0xD9, 0x0E, 0x9F, 0xC9, 0x1D, 0xCF, 0x4C, 0x34,
+0x3B, 0xB6, 0x7A, 0xE8, 0x00, 0x64, 0x00, 0x06, 0x00, 0xC8, 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFE,
+/* bank # 1 */
+0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x07, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00,
+0x10, 0x00, 0x00, 0x00, 0x04, 0xD6, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00, 0x04, 0xCC, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x06, 0x00, 0x02, 0x00, 0x05, 0x00, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00, 0x64, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x05, 0x00, 0x64, 0x00, 0x20, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x03, 0x00,
+0x00, 0x00, 0x00, 0x32, 0xF8, 0x98, 0x00, 0x00, 0xFF, 0x65, 0x00, 0x00, 0x83, 0x0F, 0x00, 0x00,
+0x00, 0x06, 0x00, 0x00, 0xFF, 0xF1, 0x00, 0x00, 0xFA, 0x46, 0x00, 0x00, 0xA2, 0xB8, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x10, 0x00,
+0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0D, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x02, 0x00, 0x00,
+0x00, 0x01, 0xFB, 0x83, 0x00, 0x7C, 0x00, 0x00, 0xFB, 0x15, 0xFC, 0x00, 0x1F, 0xB4, 0xFF, 0x83,
+0x00, 0x00, 0x00, 0x01, 0x00, 0x65, 0x00, 0x07, 0x00, 0x64, 0x03, 0xE8, 0x00, 0x64, 0x00, 0x28,
+0x00, 0x00, 0xFF, 0xFD, 0x00, 0x00, 0x00, 0x00, 0x16, 0xA0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x10, 0x00, 0x00, 0x2F, 0x00, 0x00, 0x00, 0x00, 0x01, 0xF4, 0x00, 0x00, 0x10, 0x00,
+/* bank # 2 */
+0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0xBA, 0xC6, 0x00, 0x47, 0x78, 0xA2,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
+0x00, 0x00, 0x23, 0xBB, 0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49,
+0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x64, 0x00, 0x07, 0x00, 0x08, 0x00, 0x06, 0x00, 0x06, 0xFF, 0xFE, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x2E, 0xA2, 0x5B, 0x00, 0x00, 0x05, 0x68, 0x00, 0x0B, 0xCF, 0x49, 0x00, 0x00, 0x00, 0x00,
+0x00, 0xF8, 0xF6, 0x2A, 0x3F, 0x68, 0xF5, 0x7A, 0x00, 0x04, 0xFF, 0xFD, 0x00, 0x02, 0x00, 0x00,
+0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0E, 0x00, 0x0E,
+0xFF, 0xFF, 0xFF, 0xCF, 0x00, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x32, 0xFF, 0xFF, 0xFF, 0x9C,
+0x00, 0x00, 0x43, 0x05, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x64,
+0xFF, 0xE5, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+/* bank # 3 */
+0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0x24, 0x26, 0xD3,
+0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x10, 0x00, 0x96, 0x00, 0x3C,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x9E, 0x65, 0x5D,
+0x0C, 0x0A, 0x4E, 0x68, 0xCD, 0xCF, 0x77, 0x09, 0x50, 0x16, 0x67, 0x59, 0xC6, 0x19, 0xCE, 0x82,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x17, 0xD7, 0x84, 0x00, 0x03, 0x00, 0x00, 0x00,
+0x00, 0x11, 0xDC, 0x47, 0x03, 0x00, 0x00, 0x00, 0xC7, 0x93, 0x8F, 0x9D, 0x1E, 0x1B, 0x1C, 0x19,
+0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0E, 0xDF, 0xA4, 0x38, 0x1F, 0x9E, 0x65, 0x5D,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x71, 0x1C, 0x02, 0x03, 0x18, 0x85, 0x00, 0x00, 0x40, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x3F, 0xFF, 0xFF, 0xFD, 0xFF, 0xFF, 0xF4, 0xC9, 0xFF, 0xFF, 0xBC, 0xF0, 0x00, 0x01, 0x0C, 0x0F,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+0xF5, 0xB7, 0xBA, 0xB3, 0x67, 0x7D, 0xDF, 0x7E, 0x72, 0x90, 0x2E, 0x55, 0x4C, 0xF6, 0xE6, 0x88,
+0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+/* bank # 4 */
+0xD8, 0xDC, 0xB4, 0xB8, 0xB0, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xB3, 0xB7, 0xBB, 0x8E, 0x9E,
+0xAE, 0xF1, 0x32, 0xF5, 0x1B, 0xF1, 0xB4, 0xB8, 0xB0, 0x80, 0x97, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF,
+0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0xF1, 0xA9,
+0x89, 0x26, 0x46, 0x66, 0xB2, 0x89, 0x99, 0xA9, 0x2D, 0x55, 0x7D, 0xB0, 0xB0, 0x8A, 0xA8, 0x96,
+0x36, 0x56, 0x76, 0xF1, 0xBA, 0xA3, 0xB4, 0xB2, 0x80, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB2, 0x83,
+0x98, 0xBA, 0xA3, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xB2, 0xB9, 0xB4, 0x98, 0x83, 0xF1,
+0xA3, 0x29, 0x55, 0x7D, 0xBA, 0xB5, 0xB1, 0xA3, 0x83, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xB2,
+0xB6, 0xAA, 0x83, 0x93, 0x28, 0x54, 0x7C, 0xF1, 0xB9, 0xA3, 0x82, 0x93, 0x61, 0xBA, 0xA2, 0xDA,
+0xDE, 0xDF, 0xDB, 0x81, 0x9A, 0xB9, 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xF1, 0xDA, 0xBA, 0xA2, 0xDF,
+0xD9, 0xBA, 0xA2, 0xFA, 0xB9, 0xA3, 0x82, 0x92, 0xDB, 0x31, 0xBA, 0xA2, 0xD9, 0xBA, 0xA2, 0xF8,
+0xDF, 0x85, 0xA4, 0xD0, 0xC1, 0xBB, 0xAD, 0x83, 0xC2, 0xC5, 0xC7, 0xB8, 0xA2, 0xDF, 0xDF, 0xDF,
+0xBA, 0xA0, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xAA, 0xB3, 0x8D, 0xB4, 0x98, 0x0D, 0x35,
+0x5D, 0xB2, 0xB6, 0xBA, 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A,
+0xB8, 0xAA, 0x87, 0x2C, 0x54, 0x7C, 0xBA, 0xA4, 0xB0, 0x8A, 0xB6, 0x91, 0x32, 0x56, 0x76, 0xB2,
+0x84, 0x94, 0xA4, 0xC8, 0x08, 0xCD, 0xD8, 0xB8, 0xB4, 0xB0, 0xF1, 0x99, 0x82, 0xA8, 0x2D, 0x55,
+0x7D, 0x98, 0xA8, 0x0E, 0x16, 0x1E, 0xA2, 0x2C, 0x54, 0x7C, 0x92, 0xA4, 0xF0, 0x2C, 0x50, 0x78,
+/* bank # 5 */
+0xF1, 0x84, 0xA8, 0x98, 0xC4, 0xCD, 0xFC, 0xD8, 0x0D, 0xDB, 0xA8, 0xFC, 0x2D, 0xF3, 0xD9, 0xBA,
+0xA6, 0xF8, 0xDA, 0xBA, 0xA6, 0xDE, 0xD8, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xF3, 0xC8,
+0x41, 0xDA, 0xA6, 0xC8, 0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0x82, 0xA8, 0x92, 0xF5, 0x2C, 0x54, 0x88,
+0x98, 0xF1, 0x35, 0xD9, 0xF4, 0x18, 0xD8, 0xF1, 0xA2, 0xD0, 0xF8, 0xF9, 0xA8, 0x84, 0xD9, 0xC7,
+0xDF, 0xF8, 0xF8, 0x83, 0xC5, 0xDA, 0xDF, 0x69, 0xDF, 0x83, 0xC1, 0xD8, 0xF4, 0x01, 0x14, 0xF1,
+0xA8, 0x82, 0x4E, 0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x28, 0x97, 0x88, 0xF1,
+0x09, 0xF4, 0x1C, 0x1C, 0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x29,
+0xF4, 0x0D, 0xD8, 0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC2, 0x03, 0xD8, 0xDE, 0xDF, 0x1A,
+0xD8, 0xF1, 0xA2, 0xFA, 0xF9, 0xA8, 0x84, 0x98, 0xD9, 0xC7, 0xDF, 0xF8, 0xF8, 0xF8, 0x83, 0xC7,
+0xDA, 0xDF, 0x69, 0xDF, 0xF8, 0x83, 0xC3, 0xD8, 0xF4, 0x01, 0x14, 0xF1, 0x98, 0xA8, 0x82, 0x2E,
+0xA8, 0x84, 0xF3, 0x11, 0xD1, 0x82, 0xF5, 0xD9, 0x92, 0x50, 0x97, 0x88, 0xF1, 0x09, 0xF4, 0x1C,
+0xD8, 0x84, 0xA8, 0xF3, 0xC0, 0xF8, 0xF9, 0xD1, 0xD9, 0x97, 0x82, 0xF1, 0x49, 0xF4, 0x0D, 0xD8,
+0xF3, 0xF9, 0xF9, 0xD1, 0xD9, 0x82, 0xF4, 0xC4, 0x03, 0xD8, 0xDE, 0xDF, 0xD8, 0xF1, 0xAD, 0x88,
+0x98, 0xCC, 0xA8, 0x09, 0xF9, 0xD9, 0x82, 0x92, 0xA8, 0xF5, 0x7C, 0xF1, 0x88, 0x3A, 0xCF, 0x94,
+0x4A, 0x6E, 0x98, 0xDB, 0x69, 0x31, 0xDA, 0xAD, 0xF2, 0xDE, 0xF9, 0xD8, 0x87, 0x95, 0xA8, 0xF2,
+0x21, 0xD1, 0xDA, 0xA5, 0xF9, 0xF4, 0x17, 0xD9, 0xF1, 0xAE, 0x8E, 0xD0, 0xC0, 0xC3, 0xAE, 0x82,
+/* bank # 6 */
+0xC6, 0x84, 0xC3, 0xA8, 0x85, 0x95, 0xC8, 0xA5, 0x88, 0xF2, 0xC0, 0xF1, 0xF4, 0x01, 0x0E, 0xF1,
+0x8E, 0x9E, 0xA8, 0xC6, 0x3E, 0x56, 0xF5, 0x54, 0xF1, 0x88, 0x72, 0xF4, 0x01, 0x15, 0xF1, 0x98,
+0x45, 0x85, 0x6E, 0xF5, 0x8E, 0x9E, 0x04, 0x88, 0xF1, 0x42, 0x98, 0x5A, 0x8E, 0x9E, 0x06, 0x88,
+0x69, 0xF4, 0x01, 0x1C, 0xF1, 0x98, 0x1E, 0x11, 0x08, 0xD0, 0xF5, 0x04, 0xF1, 0x1E, 0x97, 0x02,
+0x02, 0x98, 0x36, 0x25, 0xDB, 0xF9, 0xD9, 0x85, 0xA5, 0xF3, 0xC1, 0xDA, 0x85, 0xA5, 0xF3, 0xDF,
+0xD8, 0x85, 0x95, 0xA8, 0xF3, 0x09, 0xDA, 0xA5, 0xFA, 0xD8, 0x82, 0x92, 0xA8, 0xF5, 0x78, 0xF1,
+0x88, 0x1A, 0x84, 0x9F, 0x26, 0x88, 0x98, 0x21, 0xDA, 0xF4, 0x1D, 0xF3, 0xD8, 0x87, 0x9F, 0x39,
+0xD1, 0xAF, 0xD9, 0xDF, 0xDF, 0xFB, 0xF9, 0xF4, 0x0C, 0xF3, 0xD8, 0xFA, 0xD0, 0xF8, 0xDA, 0xF9,
+0xF9, 0xD0, 0xDF, 0xD9, 0xF9, 0xD8, 0xF4, 0x0B, 0xD8, 0xF3, 0x87, 0x9F, 0x39, 0xD1, 0xAF, 0xD9,
+0xDF, 0xDF, 0xF4, 0x1D, 0xF3, 0xD8, 0xFA, 0xFC, 0xA8, 0x69, 0xF9, 0xF9, 0xAF, 0xD0, 0xDA, 0xDE,
+0xFA, 0xD9, 0xF8, 0x8F, 0x9F, 0xA8, 0xF1, 0xCC, 0xF3, 0x98, 0xDB, 0x45, 0xD9, 0xAF, 0xDF, 0xD0,
+0xF8, 0xD8, 0xF1, 0x8F, 0x9F, 0xA8, 0xCA, 0xF3, 0x88, 0x09, 0xDA, 0xAF, 0x8F, 0xCB, 0xF8, 0xD8,
+0xF2, 0xAD, 0x97, 0x8D, 0x0C, 0xD9, 0xA5, 0xDF, 0xF9, 0xBA, 0xA6, 0xF3, 0xFA, 0xF4, 0x12, 0xF2,
+0xD8, 0x95, 0x0D, 0xD1, 0xD9, 0xBA, 0xA6, 0xF3, 0xFA, 0xDA, 0xA5, 0xF2, 0xC1, 0xBA, 0xA6, 0xF3,
+0xDF, 0xD8, 0xF1, 0xBA, 0xB2, 0xB6, 0x86, 0x96, 0xA6, 0xD0, 0xCA, 0xF3, 0x49, 0xDA, 0xA6, 0xCB,
+0xF8, 0xD8, 0xB0, 0xB4, 0xB8, 0xD8, 0xAD, 0x84, 0xF2, 0xC0, 0xDF, 0xF1, 0x8F, 0xCB, 0xC3, 0xA8,
+/* bank # 7 */
+0xB2, 0xB6, 0x86, 0x96, 0xC8, 0xC1, 0xCB, 0xC3, 0xF3, 0xB0, 0xB4, 0x88, 0x98, 0xA8, 0x21, 0xDB,
+0x71, 0x8D, 0x9D, 0x71, 0x85, 0x95, 0x21, 0xD9, 0xAD, 0xF2, 0xFA, 0xD8, 0x85, 0x97, 0xA8, 0x28,
+0xD9, 0xF4, 0x08, 0xD8, 0xF2, 0x8D, 0x29, 0xDA, 0xF4, 0x05, 0xD9, 0xF2, 0x85, 0xA4, 0xC2, 0xF2,
+0xD8, 0xA8, 0x8D, 0x94, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xF2, 0xD8, 0x87, 0x21, 0xD8, 0xF4, 0x0A,
+0xD8, 0xF2, 0x84, 0x98, 0xA8, 0xC8, 0x01, 0xD1, 0xD9, 0xF4, 0x11, 0xD8, 0xF3, 0xA4, 0xC8, 0xBB,
+0xAF, 0xD0, 0xF2, 0xDE, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xD8, 0xF1, 0xB8, 0xF6,
+0xB5, 0xB9, 0xB0, 0x8A, 0x95, 0xA3, 0xDE, 0x3C, 0xA3, 0xD9, 0xF8, 0xD8, 0x5C, 0xA3, 0xD9, 0xF8,
+0xD8, 0x7C, 0xA3, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA5, 0xD9, 0xDF, 0xDA, 0xFA, 0xD8, 0xB1,
+0x85, 0x30, 0xF7, 0xD9, 0xDE, 0xD8, 0xF8, 0x30, 0xAD, 0xDA, 0xDE, 0xD8, 0xF2, 0xB4, 0x8C, 0x99,
+0xA3, 0x2D, 0x55, 0x7D, 0xA0, 0x83, 0xDF, 0xDF, 0xDF, 0xB5, 0x91, 0xA0, 0xF6, 0x29, 0xD9, 0xFB,
+0xD8, 0xA0, 0xFC, 0x29, 0xD9, 0xFA, 0xD8, 0xA0, 0xD0, 0x51, 0xD9, 0xF8, 0xD8, 0xFC, 0x51, 0xD9,
+0xF9, 0xD8, 0x79, 0xD9, 0xFB, 0xD8, 0xA0, 0xD0, 0xFC, 0x79, 0xD9, 0xFA, 0xD8, 0xA1, 0xF9, 0xF9,
+0xF9, 0xF9, 0xF9, 0xA0, 0xDA, 0xDF, 0xDF, 0xDF, 0xD8, 0xA1, 0xF8, 0xF8, 0xF8, 0xF8, 0xF8, 0xAC,
+0xDE, 0xF8, 0xAD, 0xDE, 0x83, 0x93, 0xAC, 0x2C, 0x54, 0x7C, 0xF1, 0xA8, 0xDF, 0xDF, 0xDF, 0xF6,
+0x9D, 0x2C, 0xDA, 0xA0, 0xDF, 0xD9, 0xFA, 0xDB, 0x2D, 0xF8, 0xD8, 0xA8, 0x50, 0xDA, 0xA0, 0xD0,
+0xDE, 0xD9, 0xD0, 0xF8, 0xF8, 0xF8, 0xDB, 0x55, 0xF8, 0xD8, 0xA8, 0x78, 0xDA, 0xA0, 0xD0, 0xDF,
+/* bank # 8 */
+0xD9, 0xD0, 0xFA, 0xF8, 0xF8, 0xF8, 0xF8, 0xDB, 0x7D, 0xF8, 0xD8, 0x9C, 0xA8, 0x8C, 0xF5, 0x30,
+0xDB, 0x38, 0xD9, 0xD0, 0xDE, 0xDF, 0xA0, 0xD0, 0xDE, 0xDF, 0xD8, 0xA8, 0x48, 0xDB, 0x58, 0xD9,
+0xDF, 0xD0, 0xDE, 0xA0, 0xDF, 0xD0, 0xDE, 0xD8, 0xA8, 0x68, 0xDB, 0x70, 0xD9, 0xDF, 0xDF, 0xA0,
+0xDF, 0xDF, 0xD8, 0xF1, 0xA8, 0x88, 0x90, 0x2C, 0x54, 0x7C, 0x98, 0xA8, 0xD0, 0x5C, 0x38, 0xD1,
+0xDA, 0xF2, 0xAE, 0x8C, 0xDF, 0xF9, 0xD8, 0xB0, 0x87, 0xA8, 0xC1, 0xC1, 0xB1, 0x88, 0xA8, 0xC6,
+0xF9, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xA8,
+0xF9, 0xDA, 0x36, 0xD8, 0xA8, 0xF9, 0xDA, 0x36, 0xD8, 0xF7, 0x8D, 0x9D, 0xAD, 0xF8, 0x18, 0xDA,
+0xF2, 0xAE, 0xDF, 0xD8, 0xF7, 0xAD, 0xFA, 0x30, 0xD9, 0xA4, 0xDE, 0xF9, 0xD8, 0xF2, 0xAE, 0xDE,
+0xFA, 0xF9, 0x83, 0xA7, 0xD9, 0xC3, 0xC5, 0xC7, 0xF1, 0x88, 0x9B, 0xA7, 0x7A, 0xAD, 0xF7, 0xDE,
+0xDF, 0xA4, 0xF8, 0x84, 0x94, 0x08, 0xA7, 0x97, 0xF3, 0x00, 0xAE, 0xF2, 0x98, 0x19, 0xA4, 0x88,
+0xC6, 0xA3, 0x94, 0x88, 0xF6, 0x32, 0xDF, 0xF2, 0x83, 0x93, 0xDB, 0x09, 0xD9, 0xF2, 0xAA, 0xDF,
+0xD8, 0xD8, 0xAE, 0xF8, 0xF9, 0xD1, 0xDA, 0xF3, 0xA4, 0xDE, 0xA7, 0xF1, 0x88, 0x9B, 0x7A, 0xD8,
+0xF3, 0x84, 0x94, 0xAE, 0x19, 0xF9, 0xDA, 0xAA, 0xF1, 0xDF, 0xD8, 0xA8, 0x81, 0xC0, 0xC3, 0xC5,
+0xC7, 0xA3, 0x92, 0x83, 0xF6, 0x28, 0xAD, 0xDE, 0xD9, 0xF8, 0xD8, 0xA3, 0x50, 0xAD, 0xD9, 0xF8,
+0xD8, 0xA3, 0x78, 0xAD, 0xD9, 0xF8, 0xD8, 0xF8, 0xF9, 0xD1, 0xA1, 0xDA, 0xDE, 0xC3, 0xC5, 0xC7,
+0xD8, 0xA1, 0x81, 0x94, 0xF8, 0x18, 0xF2, 0xB0, 0x89, 0xAC, 0xC3, 0xC5, 0xC7, 0xF1, 0xD8, 0xB8,
+/* bank # 9 */
+0xB4, 0xB0, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0,
+0x0C, 0x20, 0x14, 0x40, 0xB0, 0xB4, 0xB8, 0xF0, 0xA8, 0x8A, 0x9A, 0x28, 0x50, 0x78, 0xB7, 0x9B,
+0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xF1, 0xBB, 0xAB,
+0x88, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0xB3, 0x8B, 0xB8, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0xB0,
+0x88, 0xB4, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xBB, 0xAB, 0xB3, 0x8B, 0x02, 0x26, 0x46, 0x66, 0xB0,
+0xB8, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 0x8A, 0x24, 0x70, 0x59,
+0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 0x8A, 0x64, 0x48, 0x31,
+0x8B, 0x30, 0x49, 0x60, 0x88, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 0x28,
+0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 0xF0,
+0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 0xA9,
+0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 0x8C,
+0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 0x7E,
+0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB5, 0xB9, 0xA3, 0xDF, 0xDF, 0xDF, 0xAE, 0xD0,
+0xDF, 0xAA, 0xD0, 0xDE, 0xF2, 0xAB, 0xF8, 0xF9, 0xD9, 0xB0, 0x87, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF,
+0xBB, 0xAF, 0xDF, 0xDF, 0xB9, 0xD8, 0xB1, 0xF1, 0xA3, 0x97, 0x8E, 0x60, 0xDF, 0xB0, 0x84, 0xF2,
+0xC8, 0xF8, 0xF9, 0xD9, 0xDE, 0xD8, 0x93, 0x85, 0xF1, 0x4A, 0xB1, 0x83, 0xA3, 0x08, 0xB5, 0x83,
+/* bank # 10 */
+0x9A, 0x08, 0x10, 0xB7, 0x9F, 0x10, 0xD8, 0xF1, 0xB0, 0xBA, 0xAE, 0xB0, 0x8A, 0xC2, 0xB2, 0xB6,
+0x8E, 0x9E, 0xF1, 0xFB, 0xD9, 0xF4, 0x1D, 0xD8, 0xF9, 0xD9, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD,
+0x61, 0xD9, 0xAE, 0xFB, 0xD8, 0xF4, 0x0C, 0xF1, 0xD8, 0xF8, 0xF8, 0xAD, 0x19, 0xD9, 0xAE, 0xFB,
+0xDF, 0xD8, 0xF4, 0x16, 0xF1, 0xD8, 0xF8, 0xAD, 0x8D, 0x61, 0xD9, 0xF4, 0xF4, 0xAC, 0xF5, 0x9C,
+0x9C, 0x8D, 0xDF, 0x2B, 0xBA, 0xB6, 0xAE, 0xFA, 0xF8, 0xF4, 0x0B, 0xD8, 0xF1, 0xAE, 0xD0, 0xF8,
+0xAD, 0x51, 0xDA, 0xAE, 0xFA, 0xF8, 0xF1, 0xD8, 0xB9, 0xB1, 0xB6, 0xA3, 0x83, 0x9C, 0x08, 0xB9,
+0xB1, 0x83, 0x9A, 0xB5, 0xAA, 0xC0, 0xFD, 0x30, 0x83, 0xB7, 0x9F, 0x10, 0xB5, 0x8B, 0x93, 0xF2,
+0x02, 0x02, 0xD1, 0xAB, 0xDA, 0xDE, 0xD8, 0xF1, 0xB0, 0x80, 0xBA, 0xAB, 0xC0, 0xC3, 0xB2, 0x84,
+0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0,
+0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28,
+0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3,
+0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9,
+0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3,
+0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28,
+0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90,
+0x2C, 0x87, 0x0C, 0xA7, 0x81, 0x97, 0x62, 0x93, 0xF0, 0x71, 0x71, 0x60, 0x85, 0x94, 0x01, 0x29,
+/* bank # 11 */
+0x51, 0x79, 0x90, 0xA5, 0xF1, 0x28, 0x4C, 0x6C, 0x87, 0x0C, 0x95, 0x18, 0x85, 0x78, 0xA3, 0x83,
+0x90, 0x28, 0x4C, 0x6C, 0x88, 0x6C, 0xD8, 0xF3, 0xA2, 0x82, 0x00, 0xF2, 0x10, 0xA8, 0x92, 0x19,
+0x80, 0xA2, 0xF2, 0xD9, 0x26, 0xD8, 0xF1, 0x88, 0xA8, 0x4D, 0xD9, 0x48, 0xD8, 0x96, 0xA8, 0x39,
+0x80, 0xD9, 0x3C, 0xD8, 0x95, 0x80, 0xA8, 0x39, 0xA6, 0x86, 0x98, 0xD9, 0x2C, 0xDA, 0x87, 0xA7,
+0x2C, 0xD8, 0xA8, 0x89, 0x95, 0x19, 0xA9, 0x80, 0xD9, 0x38, 0xD8, 0xA8, 0x89, 0x39, 0xA9, 0x80,
+0xDA, 0x3C, 0xD8, 0xA8, 0x2E, 0xA8, 0x39, 0x90, 0xD9, 0x0C, 0xD8, 0xA8, 0x95, 0x31, 0x98, 0xD9,
+0x0C, 0xD8, 0xA8, 0x09, 0xD9, 0xFF, 0xD8, 0x01, 0xDA, 0xFF, 0xD8, 0x95, 0x39, 0xA9, 0xDA, 0x26,
+0xFF, 0xD8, 0x90, 0xA8, 0x0D, 0x89, 0x99, 0xA8, 0x10, 0x80, 0x98, 0x21, 0xDA, 0x2E, 0xD8, 0x89,
+0x99, 0xA8, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x86, 0x96, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8,
+0x87, 0x31, 0x80, 0xDA, 0x2E, 0xD8, 0xA8, 0x82, 0x92, 0xF3, 0x41, 0x80, 0xF1, 0xD9, 0x2E, 0xD8,
+0xA8, 0x82, 0xF3, 0x19, 0x80, 0xF1, 0xD9, 0x2E, 0xD8, 0x82, 0xAC, 0xF3, 0xC0, 0xA2, 0x80, 0x22,
+0xF1, 0xA6, 0x2E, 0xA7, 0x2E, 0xA9, 0x22, 0x98, 0xA8, 0x29, 0xDA, 0xAC, 0xDE, 0xFF, 0xD8, 0xA2,
+0xF2, 0x2A, 0xF1, 0xA9, 0x2E, 0x82, 0x92, 0xA8, 0xF2, 0x31, 0x80, 0xA6, 0x96, 0xF1, 0xD9, 0x00,
+0xAC, 0x8C, 0x9C, 0x0C, 0x30, 0xAC, 0xDE, 0xD0, 0xDE, 0xFF, 0xD8, 0x8C, 0x9C, 0xAC, 0xD0, 0x10,
+0xAC, 0xDE, 0x80, 0x92, 0xA2, 0xF2, 0x4C, 0x82, 0xA8, 0xF1, 0xCA, 0xF2, 0x35, 0xF1, 0x96, 0x88,
+0xA6, 0xD9, 0x00, 0xD8, 0xF1, 0xFF,
+};
+
+// this divisor is pre configured into the above image and can't be modified at this time.
+#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR 
+#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default 
+#endif
+
+// this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done.
+// for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions"
+uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely
+	uint8_t val;
+	uint16_t ival;
+  // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41
+	I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay
+	sleep_ms(100);
+	I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay
+	sleep_ms(100);         
+	I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro
+	I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt
+	I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead
+	I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 =  Accel Full Scale Select: 2g
+	I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
+	I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
+	I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
+	I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ  //Im betting this will be the beat
+	if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail
+	I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address
+	I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec
+	I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo
+	I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on
+	I2Cdev::writeBit(devAddr,0x6A, 2, 1);      // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte)
+
+  setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library
+/*
+    dmpPacketSize += 16;//DMP_FEATURE_6X_LP_QUAT
+    dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_ACCEL
+    dmpPacketSize += 6;//DMP_FEATURE_SEND_RAW_GYRO
+*/
+	dmpPacketSize = 28;
+	return 0;
+}
+
+bool MPU6050::dmpPacketAvailable() {
+    return getFIFOCount() >= dmpGetFIFOPacketSize();
+}
+
+// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
+// uint8_t MPU6050::dmpGetFIFORate();
+// uint8_t MPU6050::dmpGetSampleStepSizeMS();
+// uint8_t MPU6050::dmpGetSampleFrequency();
+// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
+
+//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
+//uint8_t MPU6050::dmpRunFIFORateProcesses();
+
+// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[16] << 8) | packet[17]);
+    data[1] = (((uint32_t)packet[18] << 8) | packet[19]);
+    data[2] = (((uint32_t)packet[20] << 8) | packet[21]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[16] << 8) | packet[17];
+    data[1] = (packet[18] << 8) | packet[19];
+    data[2] = (packet[20] << 8) | packet[21];
+    return 0;
+}
+uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[16] << 8) | packet[17];
+    v -> y = (packet[18] << 8) | packet[19];
+    v -> z = (packet[20] << 8) | packet[21];
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
+    data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
+    data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
+    data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = ((packet[0] << 8) | packet[1]);
+    data[1] = ((packet[4] << 8) | packet[5]);
+    data[2] = ((packet[8] << 8) | packet[9]);
+    data[3] = ((packet[12] << 8) | packet[13]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    if (status == 0) {
+        q -> w = (float)qI[0] / 16384.0f;
+        q -> x = (float)qI[1] / 16384.0f;
+        q -> y = (float)qI[2] / 16384.0f;
+        q -> z = (float)qI[3] / 16384.0f;
+        return 0;
+    }
+    return status; // int16 return value, indicates error if this line is reached
+}
+// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (((uint32_t)packet[22] << 8) | packet[23]);
+    data[1] = (((uint32_t)packet[24] << 8) | packet[25]);
+    data[2] = (((uint32_t)packet[26] << 8) | packet[27]);
+    return 0;
+}
+uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    data[0] = (packet[22] << 8) | packet[23];
+    data[1] = (packet[24] << 8) | packet[25];
+    data[2] = (packet[26] << 8) | packet[27];
+    return 0;
+}
+uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) {
+    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
+    if (packet == 0) packet = dmpPacketBuffer;
+    v -> x = (packet[22] << 8) | packet[23];
+    v -> y = (packet[24] << 8) | packet[25];
+    v -> z = (packet[26] << 8) | packet[27];
+    return 0;
+}
+// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
+// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
+    // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
+    v -> x = vRaw -> x - gravity -> x*8192;
+    v -> y = vRaw -> y - gravity -> y*8192;
+    v -> z = vRaw -> z - gravity -> z*8192;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
+    // rotate measured 3D acceleration vector into original state
+    // frame of reference based on orientation quaternion
+    memcpy(v, vReal, sizeof(VectorInt16));
+    v -> rotate(q);
+    return 0;
+}
+// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
+uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) {
+    /* +1g corresponds to +8192, sensitivity is 2g. */
+    int16_t qI[4];
+    uint8_t status = dmpGetQuaternion(qI, packet);
+    data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384;
+    data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384;
+    data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1]
+	       - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384);
+    return status;
+}
+
+uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
+    v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
+    v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
+    v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
+    return 0;
+}
+// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
+    data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
+    data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
+    return 0;
+}
+
+#ifdef USE_OLD_DMPGETYAWPITCHROLL
+uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
+    return 0;
+}
+#else 
+uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
+    // yaw: (about Z axis)
+    data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
+    // pitch: (nose up/down, about Y axis)
+    data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
+    // roll: (tilt left/right, about X axis)
+    data[2] = atan2(gravity -> y , gravity -> z);
+    if (gravity -> z < 0) {
+        if(data[1] > 0) {
+            data[1] = PI - data[1]; 
+        } else { 
+            data[1] = -PI - data[1];
+        }
+    }
+    return 0;
+}
+#endif
+
+// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
+// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
+
+uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
+    /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
+        if (dmpData[k] < 0x10) Serial.print("0");
+        Serial.print(dmpData[k], HEX);
+        Serial.print(" ");
+    }
+    Serial.print("\n");*/
+    //Serial.println((uint16_t)dmpPacketBuffer);
+    return 0;
+}
+uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
+    uint8_t status;
+    uint8_t buf[dmpPacketSize];
+    for (uint8_t i = 0; i < numPackets; i++) {
+        // read packet from FIFO
+        getFIFOBytes(buf, dmpPacketSize);
+
+        // process packet
+        if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
+        
+        // increment external process count variable, if supplied
+        if (processed != 0) (*processed)++;
+    }
+    return 0;
+}
+
+// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
+
+// uint8_t MPU6050::dmpInitFIFOParam();
+// uint8_t MPU6050::dmpCloseFIFO();
+// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
+// uint8_t MPU6050::dmpDecodeQuantizedAccel();
+// uint32_t MPU6050::dmpGetGyroSumOfSquare();
+// uint32_t MPU6050::dmpGetAccelSumOfSquare();
+// void MPU6050::dmpOverrideQuaternion(long *q);
+uint16_t MPU6050::dmpGetFIFOPacketSize() {
+    return dmpPacketSize;
+}
+
+
+
+uint8_t MPU6050::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof
+    return(GetCurrentFIFOPacket(data, dmpPacketSize));
+}
+
+#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
diff --git a/RP2040/MPU6050/examples/README.md b/RP2040/MPU6050/examples/README.md
new file mode 100644
index 00000000..4bc90006
--- /dev/null
+++ b/RP2040/MPU6050/examples/README.md
@@ -0,0 +1,16 @@
+These are examples for the Raspberry Pi Pico development board and have serial output enabled through USB.
+Also, if the libraries files (I2Cdev.h, I2Cdev.cpp, MPU6050.h, MPU6050.cpp, MPU6050\_6Axis\_MotionApps\_V6\_12.h, helper\_3dmath.h) are not in a common include directory, you will have to copy them to the example folder (whichever you want to build), unless you modify the CMakeLists.txt and specify the path.
+
+#### Instructions for building examples
+1. ```cd``` to the folder of the example you want to build.
+2. If you have PICO W (PICO board with Infineon CYW43439 wireless chip), you need to uncommented 3 lines in CMakeLists.txt file.
+3. ```mkdir build && cd build```
+4. ```cmake ..```
+5. ```make```
+6. Copy the uf2 file to your Pico board, using ```cp``` or the file explorer you have.
+7. ```sudo minicom -D /dev/ttyACM0``` to watch the serial output. Use ```sudo```, otherwise minicom will fail to open the device and show no warnings. On Windows you can use PuTTY, choosing the COM port that was assigned (check the Device Manager) and a baudrate of 115200.
+
+#### Sensor calibration
+You will get better results if you measure the gyro and accelerometer offsets for your sensor *(e.g. with the accompanying calibration example)*. Set the initial offsets using `mpu.setXAccelOffset()` and friends in mpu6050_DMP_port.cpp after the call to `mpu.dmpInitialize()`.
+
+Alternatively you could call `mpu.CalibrateAccel(6)` and `mpu.CalibrateGyro(6)` in the same location (6 loops should be enough).
diff --git a/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/CMakeLists.txt b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/CMakeLists.txt
new file mode 100644
index 00000000..41f24dec
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/CMakeLists.txt
@@ -0,0 +1,46 @@
+# Generated Cmake Pico project file
+
+cmake_minimum_required(VERSION 3.13)
+# Pull in SDK (must be before project)
+# set(PICO_BOARD pico_w)  # Needed only for PICO W
+# set(CYW43_LWIP 1)       # Needed only for PICO W
+include(pico_sdk_import.cmake)
+
+project(mpu6050_DMP_port C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+# Initialise the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+# Add executable. Default name is the project name, version 0.1
+
+add_executable(mpu6050_DMP_port 
+         mpu6050_DMP_port.cpp 
+         ../../MPU6050.cpp
+         ../../../I2Cdev/I2Cdev.cpp
+        )
+
+target_include_directories(mpu6050_DMP_port PRIVATE
+         ${CMAKE_CURRENT_LIST_DIR}/../../
+         ${CMAKE_CURRENT_LIST_DIR}/../../../I2Cdev
+        )
+
+# Add any user requested libraries
+target_link_libraries(mpu6050_DMP_port
+        pico_stdlib 
+        #        pico_cyw43_arch_none     # we need Wifi to access the GPIO. Needed only for PICO W
+        hardware_i2c
+        pico_double
+        )
+
+pico_set_program_name(mpu6050_DMP_port "mpu6050_DMP_port")
+pico_set_program_version(mpu6050_DMP_port "0.1")
+
+pico_enable_stdio_uart(mpu6050_DMP_port 0)
+pico_enable_stdio_usb(mpu6050_DMP_port 1)
+
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(mpu6050_DMP_port)
+
diff --git a/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/mpu6050_DMP_port.cpp b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/mpu6050_DMP_port.cpp
new file mode 100644
index 00000000..bc9c1725
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/mpu6050_DMP_port.cpp
@@ -0,0 +1,179 @@
+#include 
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#ifndef PICO_DEFAULT_LED_PIN // PICO w with WiFi
+#include "pico/cyw43_arch.h"
+#endif
+#include "MPU6050_6Axis_MotionApps_V6_12.h"
+
+MPU6050 mpu;
+
+//#define OUTPUT_READABLE_YAWPITCHROLL
+//#define OUTPUT_READABLE_REALACCEL
+//#define OUTPUT_READABLE_WORLDACCEL
+#define OUTPUT_READABLE_CUSTOM
+
+
+bool dmpReady = false;  // set true if DMP init was successful
+uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
+uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount;     // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+Quaternion q;           // [w, x, y, z]         quaternion container
+VectorInt16 aa;         // [x, y, z]            accel sensor measurements
+VectorInt16 gy;         // [x, y, z]            gyro sensor measurements
+VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
+VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
+VectorFloat gravity;    // [x, y, z]            gravity vector
+float euler[3];         // [psi, theta, phi]    Euler angle container
+float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
+float yaw, pitch, roll;
+
+volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+    mpuInterrupt = true;
+}
+
+void initLED() {
+#ifndef PICO_DEFAULT_LED_PIN // PICO w with WiFi
+    printf ("we have board with wifi (pico w) \n");
+    if (cyw43_arch_init()) {
+        printf("WiFi init failed");
+        exit(3);
+    }
+#else
+    printf ("we have board without wifi\n");
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+#endif // PICO_DEFAULT_LED_PIN
+
+} // initLED()
+
+void  waitForUsbConnect() {
+#ifdef _PICO_STDIO_USB_H // We are using PICO_STDIO_USB. Have to wait for connection.
+    
+#ifndef PICO_DEFAULT_LED_PIN
+    while (!stdio_usb_connected()) { // blink the pico's led until usb connection is established
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
+        sleep_ms(250);
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
+        sleep_ms(250);
+    }
+#else
+    while (!stdio_usb_connected()) { // blink the pico's led until usb connection is established
+        gpio_put(PICO_DEFAULT_LED_PIN, 0);
+        sleep_ms(250);
+        gpio_put(PICO_DEFAULT_LED_PIN, 1);
+        sleep_ms(250);
+    }
+#endif // PICO_DEFAULT_LED_PIN
+#endif // _PICO_STDIO_USB_H
+} //  waitForUsbConnect
+
+int main() {
+    stdio_init_all();
+    // This example will use I2C0 on the default SDA and SCL (pins 6, 7 on a Pico)
+    i2c_init(i2c_default, 400 * 1000);
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+    // Make the I2C pins available to picotool
+    
+    // setup blink led
+    // setup blink led
+    initLED();
+    waitForUsbConnect();
+
+    // ================================================================
+    // ===                      INITIAL SETUP                       ===
+    // ================================================================
+
+    mpu.initialize();
+    devStatus = mpu.dmpInitialize();
+
+    /* --- if you have calibration data then set the sensor offsets here --- */
+    // mpu.setXAccelOffset();
+    // mpu.setYAccelOffset();
+    // mpu.setZAccelOffset();
+    // mpu.setXGyroOffset();
+    // mpu.setYGyroOffset();
+    // mpu.setZGyroOffset();
+
+    /* --- alternatively you can try this (6 loops should be enough) --- */
+    // mpu.CalibrateAccel(6);
+    // mpu.CalibrateGyro(6);	
+	
+    if (devStatus == 0) 
+    {
+        mpu.setDMPEnabled(true);                // turn on the DMP, now that it's ready
+        mpuIntStatus = mpu.getIntStatus();
+        dmpReady = true;                        // set our DMP Ready flag so the main loop() function knows it's okay to use it
+        packetSize = mpu.dmpGetFIFOPacketSize();      // get expected DMP packet size for later comparison
+    } 
+    else 
+    {                                          // ERROR!        1 = initial memory load failed         2 = DMP configuration updates failed        (if it's going to break, usually the code will be 1)
+        printf("DMP Initialization failed (code %d)", devStatus);
+        sleep_ms(2000);
+    }
+    yaw = 0.0;
+    pitch = 0.0;
+    roll = 0.0;
+
+    // ================================================================
+    // ===                    MAIN PROGRAM LOOP                     ===
+    // ================================================================
+
+    while(1){
+
+        if (!dmpReady);                                                    // if programming failed, don't try to do anything
+        mpuInterrupt = true;
+        fifoCount = mpu.getFIFOCount();                                           // get current FIFO count
+        if ((mpuIntStatus & 0x10) || fifoCount == 1024)                           // check for overflow (this should never happen unless our code is too inefficient)
+        {
+            mpu.resetFIFO();                                                      // reset so we can continue cleanly
+            printf("FIFO overflow!");
+        } 
+        else if (mpuIntStatus & 0x01)                                             // otherwise, check for DMP data ready interrupt (this should happen frequently)
+        {    
+            while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();        // wait for correct available data length, should be a VERY short wait
+            mpu.getFIFOBytes(fifoBuffer, packetSize);                             // read a packet from FIFO
+            fifoCount -= packetSize;                                              // track FIFO count here in case there is > 1 packet available
+            #ifdef OUTPUT_READABLE_YAWPITCHROLL                                               // display Euler angles in degrees
+                mpu.dmpGetQuaternion(&q, fifoBuffer);
+                mpu.dmpGetGravity(&gravity, &q);
+                mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+                yaw = ypr[0] * 180 / PI;
+                pitch = ypr[1] * 180 / PI;
+                roll = ypr[2] * 180 / PI;
+                printf("ypr: %f,\t %f,\t %f\n", yaw, pitch, roll);
+            #endif
+            #ifdef OUTPUT_READABLE_REALACCEL
+                // display real acceleration, adjusted to remove gravity
+                mpu.dmpGetQuaternion(&q, fifoBuffer);
+                mpu.dmpGetAccel(&aa, fifoBuffer);
+                mpu.dmpGetGravity(&gravity, &q);
+                mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+                printf("areal: %d,\t %d,\t %d\n", aaReal.x, aaReal.y, aaReal.z);
+            #endif
+            #ifdef OUTPUT_READABLE_WORLDACCEL
+                // display initial world-frame acceleration, adjusted to remove gravity
+                // and rotated based on known orientation from quaternion
+                mpu.dmpGetQuaternion(&q, fifoBuffer);
+                mpu.dmpGetAccel(&aa, fifoBuffer);
+                mpu.dmpGetGravity(&gravity, &q);
+                mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+                mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+                printf("aworld: %d,\t %d,\t %d\n", aaWorld.x, aaWorld.y, aaWorld.z);
+            #endif
+            #ifdef OUTPUT_READABLE_CUSTOM
+                mpu.dmpGetQuaternion(&q, fifoBuffer);
+                printf("W: %f\t X: %f\t Y: %f\t Z: %f\n", q.w, q.x, q.y, q.z);
+            #endif
+        }
+    }
+
+    return 0;
+}
diff --git a/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/pico_sdk_import.cmake b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/pico_sdk_import.cmake
new file mode 100644
index 00000000..65f8a6f7
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_DMP_V6.12/pico_sdk_import.cmake
@@ -0,0 +1,73 @@
+# This is a copy of /external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        # GIT_SUBMODULES_RECURSE was added in 3.17
+        if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG master
+                    GIT_SUBMODULES_RECURSE FALSE
+            )
+        else ()
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG master
+            )
+        endif ()
+
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            FetchContent_Populate(pico_sdk)
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/RP2040/MPU6050/examples/mpu6050_calibration/CMakeLists.txt b/RP2040/MPU6050/examples/mpu6050_calibration/CMakeLists.txt
new file mode 100644
index 00000000..da6892d3
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_calibration/CMakeLists.txt
@@ -0,0 +1,44 @@
+# Generated Cmake Pico project file
+
+cmake_minimum_required(VERSION 3.13)
+# Pull in SDK (must be before project)
+# set(PICO_BOARD pico_w)  # Needed only for PICO W
+# set(CYW43_LWIP 1)       # Needed only for PICO W
+include(pico_sdk_import.cmake)
+
+project(mpu6050_calibration C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+# Initialise the Raspberry Pi Pico SDK
+pico_sdk_init()
+
+# Add executable. Default name is the project name, version 0.1
+
+add_executable( mpu6050_calibration
+         mpu6050_calibration.cpp 
+         ../../MPU6050.cpp
+         ../../../I2Cdev/I2Cdev.cpp
+        )
+
+target_include_directories(mpu6050_calibration PRIVATE
+         ${CMAKE_CURRENT_LIST_DIR}/../../
+         ${CMAKE_CURRENT_LIST_DIR}/../../../I2Cdev
+        )
+
+# Add any user requested libraries
+target_link_libraries( mpu6050_calibration 
+        pico_stdlib 
+        #        pico_cyw43_arch_none     # we need Wifi to access the GPIO. Needed only for PICO W
+        hardware_i2c
+        )
+
+pico_set_program_name(mpu6050_calibration "mpu6050_calibration")
+pico_set_program_version(mpu6050_calibration "0.1")
+
+pico_enable_stdio_uart(mpu6050_calibration 0)
+pico_enable_stdio_usb(mpu6050_calibration 1)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs( mpu6050_calibration)
+
diff --git a/RP2040/MPU6050/examples/mpu6050_calibration/mpu6050_calibration.cpp b/RP2040/MPU6050/examples/mpu6050_calibration/mpu6050_calibration.cpp
new file mode 100644
index 00000000..4ac496d6
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_calibration/mpu6050_calibration.cpp
@@ -0,0 +1,274 @@
+#include 
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#ifndef PICO_DEFAULT_LED_PIN // PICO w with WiFi
+#include "pico/cyw43_arch.h"
+#endif
+#include "MPU6050.h"
+
+MPU6050 accelgyro;
+
+const int iAx = 0;
+const int iAy = 1;
+const int iAz = 2;
+const int iGx = 3;
+const int iGy = 4;
+const int iGz = 5;
+
+const int usDelay = 3150;   // empirical, to hold sampling to 200 Hz
+const int NFast =  1000;    // the bigger, the better (but slower)
+const int NSlow = 10000;    // ..
+      int LowValue[6];
+      int HighValue[6];
+      int Smoothed[6];
+      int LowOffset[6];
+      int HighOffset[6];
+      int Target[6];
+      int N;
+
+void GetSmoothed(){ int16_t RawValue[6];
+    int i;
+    long Sums[6];
+    for (i = iAx; i <= iGz; i++)
+      { Sums[i] = 0; }
+
+    for (i = 1; i <= N; i++)
+      { // get sums
+        accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], 
+                             &RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
+        if ((i % 500) == 0)
+          printf(".");
+        sleep_us(usDelay);
+        for (int j = iAx; j <= iGz; j++)
+          Sums[j] = Sums[j] + RawValue[j];
+      } // get sums
+    for (i = iAx; i <= iGz; i++)
+      { Smoothed[i] = (Sums[i] + N/2) / N ; }
+} // GetSmoothed
+
+void Initialize(){
+    printf("Initializing I2C devices...\n");
+    accelgyro.initialize();
+
+    // verify connection
+    printf("Testing device connections...\n");
+    if(accelgyro.testConnection()) printf("MPU6050 connection successful\n");
+    else {printf("MPU6050 connection failed\n");}
+    printf("\nPID tuning Each Dot = 100 readings");
+  /*A tidbit on how PID (PI actually) tuning works. 
+    When we change the offset in the MPU6050 we can get instant results. This allows us to use Proportional and 
+    integral of the PID to discover the ideal offsets. Integral is the key to discovering these offsets, Integral 
+    uses the error from set-point (set-point is zero), it takes a fraction of this error (error * ki) and adds it 
+    to the integral value. Each reading narrows the error down to the desired offset. The greater the error from 
+    set-point, the more we adjust the integral value. The proportional does its part by hiding the noise from the 
+    integral math. The Derivative is not used because of the noise and because the sensor is stationary. With the 
+    noise removed the integral value lands on a solid offset after just 600 readings. At the end of each set of 100 
+    readings, the integral value is used for the actual offsets and the last proportional reading is ignored due to 
+    the fact it reacts to any noise.
+  */
+        accelgyro.CalibrateAccel(6);
+        accelgyro.CalibrateGyro(6);
+        printf("\nat 600 Readings");
+        accelgyro.PrintActiveOffsets();
+        printf("\n");
+        accelgyro.CalibrateAccel(1);
+        accelgyro.CalibrateGyro(1);
+        printf("700 Total Readings");
+        accelgyro.PrintActiveOffsets();
+        printf("\n");
+        accelgyro.CalibrateAccel(1);
+        accelgyro.CalibrateGyro(1);
+        printf("800 Total Readings");
+        accelgyro.PrintActiveOffsets();
+        printf("\n");
+        accelgyro.CalibrateAccel(1);
+        accelgyro.CalibrateGyro(1);
+        printf("900 Total Readings");
+        accelgyro.PrintActiveOffsets();
+        printf("\n");    
+        accelgyro.CalibrateAccel(1);
+        accelgyro.CalibrateGyro(1);
+        printf("1000 Total Readings");
+        accelgyro.PrintActiveOffsets();
+     printf("\n\nAny of the above offsets will work nice\nLets proof the PID tuning using another method:"); 
+} // Initialize
+
+void SetOffsets(int TheOffsets[6]){
+    accelgyro.setXAccelOffset(TheOffsets[iAx]);
+    accelgyro.setYAccelOffset(TheOffsets[iAy]);
+    accelgyro.setZAccelOffset(TheOffsets[iAz]);
+    accelgyro.setXGyroOffset(TheOffsets[iGx]);
+    accelgyro.setYGyroOffset(TheOffsets[iGy]);
+    accelgyro.setZGyroOffset(TheOffsets[iGz]);
+} // SetOffsets
+
+// The following may show up as a mess in the terminal unless it is on fullscreen
+void ShowProgress(){
+    printf("\nXAccel\t\t\tYAccel\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro\n");
+    for (int i = iAx; i <= iGz; i++){
+        printf("[%d,%d] --> [%d,%d]\t", LowOffset[i], HighOffset[i], LowValue[i], HighValue[i]);
+    }
+    printf("\n");
+} // ShowProgress
+
+void SetAveraging(int NewN){
+    N = NewN;
+    printf("\nAveraging %d readings each time\n", N);
+} // SetAveraging
+
+void PullBracketsIn(){
+    bool AllBracketsNarrow;
+    bool StillWorking;
+    int NewOffset[6];
+  
+    printf("\nClosing in:\n");
+    AllBracketsNarrow = false;
+    StillWorking = true;
+    while (StillWorking) 
+      { StillWorking = false;
+        if (AllBracketsNarrow && (N == NFast))
+          { SetAveraging(NSlow); }
+        else
+          { AllBracketsNarrow = true; }// tentative
+        for (int i = iAx; i <= iGz; i++)
+          { if (HighOffset[i] <= (LowOffset[i]+1))
+              { NewOffset[i] = LowOffset[i]; }
+            else
+              { // binary search
+                StillWorking = true;
+                NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
+                if (HighOffset[i] > (LowOffset[i] + 10))
+                  { AllBracketsNarrow = false; }
+              } // binary search
+          }
+        SetOffsets(NewOffset);
+        GetSmoothed();
+        for (int i = iAx; i <= iGz; i++)
+          { // closing in
+            if (Smoothed[i] > Target[i])
+              { // use lower half
+                HighOffset[i] = NewOffset[i];
+                HighValue[i] = Smoothed[i];
+              } // use lower half
+            else
+              { // use upper half
+                LowOffset[i] = NewOffset[i];
+                LowValue[i] = Smoothed[i];
+              } // use upper half
+          } // closing in
+        ShowProgress();
+      } // still working
+   
+} // PullBracketsIn
+
+void PullBracketsOut(){
+    bool Done = false;
+    int NextLowOffset[6];
+    int NextHighOffset[6];
+
+    printf("\nExpanding:\n");
+ 
+    while (!Done)
+      { Done = true;
+        SetOffsets(LowOffset);
+        GetSmoothed();
+        for (int i = iAx; i <= iGz; i++)
+          { // got low values
+            LowValue[i] = Smoothed[i];
+            if (LowValue[i] >= Target[i])
+              { Done = false;
+                NextLowOffset[i] = LowOffset[i] - 1000;
+              }
+            else
+              { NextLowOffset[i] = LowOffset[i]; }
+          } // got low values
+      
+        SetOffsets(HighOffset);
+        GetSmoothed();
+        for (int i = iAx; i <= iGz; i++)
+          { // got high values
+            HighValue[i] = Smoothed[i];
+            if (HighValue[i] <= Target[i])
+              { Done = false;
+                NextHighOffset[i] = HighOffset[i] + 1000;
+              }
+            else
+              { NextHighOffset[i] = HighOffset[i]; }
+          } // got high values
+        ShowProgress();
+        for (int i = iAx; i <= iGz; i++)
+          { LowOffset[i] = NextLowOffset[i];   // had to wait until ShowProgress done
+            HighOffset[i] = NextHighOffset[i]; // ..
+          }
+     } // keep going
+} // PullBracketsOut
+
+void initLED() {
+#ifndef PICO_DEFAULT_LED_PIN // PICO w with WiFi
+    printf ("we have board with wifi (pico w) \n");
+    if (cyw43_arch_init()) {
+        printf("WiFi init failed");
+        exit(3);
+    }
+#else
+    printf ("we have board without wifi\n");
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+#endif // PICO_DEFAULT_LED_PIN
+
+} // initLED()
+
+void  waitForUsbConnect() {
+#ifdef _PICO_STDIO_USB_H // We are using PICO_STDIO_USB. Have to wait for connection.
+    
+#ifndef PICO_DEFAULT_LED_PIN
+    while (!stdio_usb_connected()) { // blink the pico's led until usb connection is established
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 0);
+        sleep_ms(250);
+        cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
+        sleep_ms(250);
+    }
+#else
+    while (!stdio_usb_connected()) { // blink the pico's led until usb connection is established
+        gpio_put(PICO_DEFAULT_LED_PIN, 0);
+        sleep_ms(250);
+        gpio_put(PICO_DEFAULT_LED_PIN, 1);
+        sleep_ms(250);
+    }
+#endif // PICO_DEFAULT_LED_PIN
+#endif // _PICO_STDIO_USB_H
+} //  waitForUsbConnect
+
+int main()
+{
+    stdio_init_all();
+
+    // I2C Initialisation. Using it at 400Khz.
+    i2c_init(i2c_default, 400*1000);
+    
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+    // setup blink led
+    initLED();
+    waitForUsbConnect();
+
+    Initialize();
+    for (int i = iAx; i <= iGz; i++)
+      { // set targets and initial guesses
+        Target[i] = 0; // must fix for ZAccel 
+        HighOffset[i] = 0;
+        LowOffset[i] = 0;
+      } // set targets and initial guesses
+    Target[iAz] = 16384;
+    SetAveraging(NFast);
+    
+    PullBracketsOut();
+    PullBracketsIn();
+    
+    printf("\n\n-------------- done --------------");
+
+    return 0;
+}
diff --git a/RP2040/MPU6050/examples/mpu6050_calibration/pico_sdk_import.cmake b/RP2040/MPU6050/examples/mpu6050_calibration/pico_sdk_import.cmake
new file mode 100644
index 00000000..65f8a6f7
--- /dev/null
+++ b/RP2040/MPU6050/examples/mpu6050_calibration/pico_sdk_import.cmake
@@ -0,0 +1,73 @@
+# This is a copy of /external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+    set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+    message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+    set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+    message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+    set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+    message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+
+if (NOT PICO_SDK_PATH)
+    if (PICO_SDK_FETCH_FROM_GIT)
+        include(FetchContent)
+        set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+        if (PICO_SDK_FETCH_FROM_GIT_PATH)
+            get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+        endif ()
+        # GIT_SUBMODULES_RECURSE was added in 3.17
+        if (${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.17.0")
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG master
+                    GIT_SUBMODULES_RECURSE FALSE
+            )
+        else ()
+            FetchContent_Declare(
+                    pico_sdk
+                    GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+                    GIT_TAG master
+            )
+        endif ()
+
+        if (NOT pico_sdk)
+            message("Downloading Raspberry Pi Pico SDK")
+            FetchContent_Populate(pico_sdk)
+            set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+        endif ()
+        set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+    else ()
+        message(FATAL_ERROR
+                "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+                )
+    endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+    message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/RP2040/MPU6050/helper_3dmath.h b/RP2040/MPU6050/helper_3dmath.h
new file mode 100644
index 00000000..cf56aec1
--- /dev/null
+++ b/RP2040/MPU6050/helper_3dmath.h
@@ -0,0 +1,218 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
+// 6/5/2012 by Jeff Rowberg 
+// 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 
+
+class Quaternion {
+    public:
+        float w;
+        float x;
+        float y;
+        float z;
+        
+        Quaternion() {
+            w = 1.0f;
+            x = 0.0f;
+            y = 0.0f;
+            z = 0.0f;
+        }
+        
+        Quaternion(float nw, float nx, float ny, float nz) {
+            w = nw;
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        Quaternion getProduct(Quaternion q) {
+            // Quaternion multiplication is defined by:
+            //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
+            //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
+            //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
+            //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
+            return Quaternion(
+                w*q.w - x*q.x - y*q.y - z*q.z,  // new w
+                w*q.x + x*q.w + y*q.z - z*q.y,  // new x
+                w*q.y - x*q.z + y*q.w + z*q.x,  // new y
+                w*q.z + x*q.y - y*q.x + z*q.w); // new z
+        }
+
+        Quaternion getConjugate() {
+            return Quaternion(w, -x, -y, -z);
+        }
+        
+        float getMagnitude() {
+            return sqrt(w*w + x*x + y*y + z*z);
+        }
+        
+        void normalize() {
+            float m = getMagnitude();
+            w /= m;
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        Quaternion getNormalized() {
+            Quaternion r(w, x, y, z);
+            r.normalize();
+            return r;
+        }
+};
+
+class VectorInt16 {
+    public:
+        int16_t x;
+        int16_t y;
+        int16_t z;
+
+        VectorInt16() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorInt16 getNormalized() {
+            VectorInt16 r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            // http://www.cprogramming.com/tutorial/3d/quaternions.html
+            // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
+            // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
+            // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
+        
+            // P_out = q * P_in * conj(q)
+            // - P_out is the output vector
+            // - q is the orientation quaternion
+            // - P_in is the input vector (a*aReal)
+            // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorInt16 getRotated(Quaternion *q) {
+            VectorInt16 r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+class VectorFloat {
+    public:
+        float x;
+        float y;
+        float z;
+
+        VectorFloat() {
+            x = 0;
+            y = 0;
+            z = 0;
+        }
+        
+        VectorFloat(float nx, float ny, float nz) {
+            x = nx;
+            y = ny;
+            z = nz;
+        }
+
+        float getMagnitude() {
+            return sqrt(x*x + y*y + z*z);
+        }
+
+        void normalize() {
+            float m = getMagnitude();
+            x /= m;
+            y /= m;
+            z /= m;
+        }
+        
+        VectorFloat getNormalized() {
+            VectorFloat r(x, y, z);
+            r.normalize();
+            return r;
+        }
+        
+        void rotate(Quaternion *q) {
+            Quaternion p(0, x, y, z);
+
+            // quaternion multiplication: q * p, stored back in p
+            p = q -> getProduct(p);
+
+            // quaternion multiplication: p * conj(q), stored back in p
+            p = p.getProduct(q -> getConjugate());
+
+            // p quaternion is now [0, x', y', z']
+            x = p.x;
+            y = p.y;
+            z = p.z;
+        }
+
+        VectorFloat getRotated(Quaternion *q) {
+            VectorFloat r(x, y, z);
+            r.rotate(q);
+            return r;
+        }
+};
+
+#endif /* _HELPER_3DMATH_H_ */
\ No newline at end of file
diff --git a/RaspberryPi_bcm2835/ADXL345/examples/ADXL345_example_1.cpp b/RaspberryPi_bcm2835/ADXL345/examples/ADXL345_example_1.cpp
new file mode 100644
index 00000000..29eb0f3f
--- /dev/null
+++ b/RaspberryPi_bcm2835/ADXL345/examples/ADXL345_example_1.cpp
@@ -0,0 +1,64 @@
+/*
+I2Cdev library collection - ADXL345 RPi example
+Based on the example in Arduino/ADXL345/
+
+==============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+
+To compile on a Raspberry Pi (1 or 2)
+  1. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html
+  2. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c
+  3. connect your i2c devices
+  4. then from bash
+      $ PATH_I2CDEVLIB=~/i2cdevlib/
+      $ gcc -o ADXL345_example_1 ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/ADXL345/examples/ADXL345_example_1.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \
+         -I ${PATH_I2CDEVLIB}/Arduino/ADXL345/ ${PATH_I2CDEVLIB}/Arduino/ADXL345/ADXL345.cpp -l bcm2835 -l m
+      $ sudo ./ADXL345_example_1
+
+*/
+
+#include 
+#include 
+#include "I2Cdev.h"
+#include "ADXL345.h"
+
+int main(int argc, char **argv) {
+  printf("ADXL345 3-axis acceleromter example program\n");
+  I2Cdev::initialize();
+  ADXL345 accel ;
+  if ( accel.testConnection() ) 
+    printf("ADXL345 connection test successful\n") ;
+  else {
+    fprintf( stderr, "ADXL345 connection test failed! exiting ...\n");
+    return 1;
+  }
+  accel.initialize();
+  int16_t ax, ay, az;
+  while (true) {
+    accel.getAcceleration(&ax, &ay, &az);
+    printf("  x_raw:  0x%04X       y_raw:  0x%04X      z_raw:  0x%04X\r", ax, ay, az);
+    fflush(stdout);
+    bcm2835_delay(200);
+  }
+  return 1; 
+}
diff --git a/RaspberryPi_bcm2835/BMP085/examples/BMP085_basic.cpp b/RaspberryPi_bcm2835/BMP085/examples/BMP085_basic.cpp
new file mode 100644
index 00000000..f6e6d579
--- /dev/null
+++ b/RaspberryPi_bcm2835/BMP085/examples/BMP085_basic.cpp
@@ -0,0 +1,72 @@
+/*
+I2Cdev library collection - BMP085 RPi example
+Based on the example in Arduino/BMP085/
+
+==============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+
+To compile on a Raspberry Pi (1 or 2)
+  1. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html
+  2. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c
+  3. connect your i2c devices
+  4. then from bash
+      $ PATH_I2CDEVLIB=~/i2cdevlib/
+      $ gcc -o BMP085_basic_example ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/BMP085/examples/BMP085_basic.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \
+         -I ${PATH_I2CDEVLIB}/Arduino/BMP085/ ${PATH_I2CDEVLIB}/Arduino/BMP085/BMP085.cpp -l bcm2835 -l m
+      $ sudo ./BMP085_basic_example
+
+*/
+
+#include 
+#include 
+#include "I2Cdev.h"
+#include "BMP085.h"
+
+int main(int argc, char **argv) {
+  printf("BMPO85 examples program\n");
+  I2Cdev::initialize();
+  BMP085 barometer ; // BMP085 class default I2C address is 0x77, specific I2C addresses may be passed as a parameter here
+  if ( barometer.testConnection() ) 
+    printf("BMP085 connection test successful\n") ;
+  else {
+    fprintf( stderr, "BMP085 connection test failed! exiting ...\n");
+    return 1;
+  }
+  barometer.loadCalibration();
+  float temperature;
+  float pressure;
+  float altitude;
+  while (true) {
+    barometer.setControl(BMP085_MODE_TEMPERATURE);
+    bcm2835_delay(5); // wait 5 ms for conversion 
+    temperature = barometer.getTemperatureC();
+    barometer.setControl(BMP085_MODE_PRESSURE_3) ; //taking reading in highest accuracy measurement mode
+    bcm2835_delay( barometer.getMeasureDelayMicroseconds() / 1000 );
+    pressure = barometer.getPressure();
+    altitude = barometer.getAltitude(pressure);
+    printf("  Temperature: %3.1f deg C      Pressure %3.3f kPa      altitude %3.1f m\r", temperature, pressure/1000.0, altitude);
+    fflush(stdout);
+    bcm2835_delay(200);
+  }
+  return 1; 
+}
diff --git a/RaspberryPi_bcm2835/HMC5883L/examples/HMC5883L_example_1.cpp b/RaspberryPi_bcm2835/HMC5883L/examples/HMC5883L_example_1.cpp
new file mode 100644
index 00000000..b79b455c
--- /dev/null
+++ b/RaspberryPi_bcm2835/HMC5883L/examples/HMC5883L_example_1.cpp
@@ -0,0 +1,71 @@
+/*
+I2Cdev library collection - HMC5883L RPi example
+Based on the example in Arduino/HMC5883L/
+
+==============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+
+To compile on a Raspberry Pi (1 or 2)
+  1. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html
+  2. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c
+  3. connect your i2c devices
+  4. then from bash
+      $ PATH_I2CDEVLIB=~/i2cdevlib/
+      $ gcc -o HMC5883L_example_1 ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/HMC5883L/examples/HMC5883L_example_1.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \
+         -I ${PATH_I2CDEVLIB}/Arduino/HMC5883L/ ${PATH_I2CDEVLIB}/Arduino/HMC5883L/HMC5883L.cpp -l bcm2835 -l m
+      $ sudo ./HMC5883L_example_1
+
+*/
+
+#include 
+#include 
+#include "I2Cdev.h"
+#include "HMC5883L.h"
+#include 
+
+#define PI 3.14159265
+
+int main(int argc, char **argv) {
+  printf("HMC5883L 3-axis acceleromter example program\n");
+  I2Cdev::initialize();
+  HMC5883L mag ;
+  if ( mag.testConnection() ) 
+    printf("HMC5883L connection test successful\n") ;
+  else {
+    fprintf( stderr, "HMC5883L connection test failed! something maybe wrong, continueing anyway though ...\n");
+    //return 1;
+  }
+  mag.initialize();
+  mag.setSampleAveraging(HMC5883L_AVERAGING_8);
+  mag.setGain(HMC5883L_GAIN_1090);
+  int16_t mx, my, mz;
+  float heading ;
+  while (true) {
+    mag.getHeading(&mx, &my, &mz);
+    heading = atan2(my, mx)  * 180 / PI;
+    printf("  mx:  %d       my:  %d      mz:  %d     heading:  %3.1f deg\r", mx, my, mz, heading);
+    fflush(stdout);
+    bcm2835_delay(200);
+  }
+  return 1; 
+}
diff --git a/RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp b/RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp
new file mode 100644
index 00000000..599aa765
--- /dev/null
+++ b/RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp
@@ -0,0 +1,261 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// RaspberryPi bcm2835 library port: bcm2835 library available at http://www.airspayce.com/mikem/bcm2835/index.html
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+
+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 "I2Cdev.h"
+#include 
+
+I2Cdev::I2Cdev() { }
+
+void I2Cdev::initialize() {
+  bcm2835_init();
+  bcm2835_i2c_set_baudrate( i2c_baudrate  );
+}
+
+/** Enable or disable I2C, 
+ * @param isEnabled true = enable, false = disable
+ */
+void I2Cdev::enable(bool isEnabled) {
+  if ( set_I2C_pins ){
+    if (isEnabled)
+      bcm2835_i2c_end();
+    else
+      bcm2835_i2c_begin() ;
+  }
+}
+
+char sendBuf[256];
+char recvBuf[256];
+
+
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  *data = recvBuf[1] & (1 << bitNum);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
+  // 01101001 read byte
+  // 76543210 bit numbers
+  //    xxx   args: bitStart=4, length=3
+  //    010   masked
+  //   -> 010 shifted
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  uint8_t b = (uint8_t) recvBuf[0];
+  if (response == BCM2835_I2C_REASON_OK) {
+    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+    b &= mask;
+    b >>= (bitStart - length + 1);
+    *data = b;
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1);
+  data[0] = (uint8_t) recvBuf[0];
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length);
+  int i ;
+  for (i = 0; i < length ; i++) {
+    data[i] = (uint8_t) recvBuf[i];
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  //first reading registery value
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
+  if ( response == BCM2835_I2C_REASON_OK ) {
+    uint8_t b = recvBuf[0] ;
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    sendBuf[1] = b ;
+    response = bcm2835_i2c_write(sendBuf, 2);
+  }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+  //      010 value to write
+  // 76543210 bit numbers
+  //    xxx   args: bitStart=4, length=3
+  // 00011100 mask byte
+  // 10101111 original value (sample)
+  // 10100011 original & ~mask
+  // 10101011 masked | value
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  //first reading registery value
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 1 );
+  if ( response == BCM2835_I2C_REASON_OK ) {
+    uint8_t b = recvBuf[0];
+    uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+    data <<= (bitStart - length + 1); // shift data into correct position
+    data &= mask; // zero all non-important bits in data
+    b &= ~(mask); // zero all important bits in existing byte
+    b |= data; // combine data with existing byte
+    sendBuf[1] = b ;
+    response = bcm2835_i2c_write(sendBuf, 2);
+    }
+  return response == BCM2835_I2C_REASON_OK;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  sendBuf[1] = data;
+  uint8_t response = bcm2835_i2c_write(sendBuf, 2);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, 2 );
+  data[0] = (recvBuf[0] << 8) | recvBuf[1] ;
+  return  response == BCM2835_I2C_REASON_OK ;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t response = bcm2835_i2c_write_read_rs(sendBuf, 1, recvBuf, length*2 );
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    data[i] = (recvBuf[i*2] << 8) | recvBuf[i*2+1] ;
+  }
+  return  response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  sendBuf[1] = (uint8_t) (data >> 8); //MSByte
+  sendBuf[2] = (uint8_t) (data >> 0); //LSByte
+  uint8_t response = bcm2835_i2c_write(sendBuf, 3);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    sendBuf[i+1] = data[i] ;
+  }
+  uint8_t response = bcm2835_i2c_write(sendBuf, 1+length);
+  return response == BCM2835_I2C_REASON_OK ;
+}
+
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data){
+  bcm2835_i2c_setSlaveAddress(devAddr);
+  sendBuf[0] = regAddr;
+  uint8_t i;
+  for (i = 0; i < length; i++) {
+    sendBuf[1+2*i] = (uint8_t) (data[i] >> 8); //MSByte
+    sendBuf[2+2*i] = (uint8_t) (data[i] >> 0); //LSByte
+  }
+  uint8_t response = bcm2835_i2c_write(sendBuf, 1+2*length);
+  return response == BCM2835_I2C_REASON_OK ;
+}
diff --git a/RaspberryPi_bcm2835/I2Cdev/I2Cdev.h b/RaspberryPi_bcm2835/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..a2cd6768
--- /dev/null
+++ b/RaspberryPi_bcm2835/I2Cdev/I2Cdev.h
@@ -0,0 +1,76 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// RaspberryPi bcm2835 library port: bcm2835 library available at http://www.airspayce.com/mikem/bcm2835/index.html
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#ifndef __cplusplus
+#error A C++ compiler is required!
+#endif 
+
+#include 
+#include  // required for BMP180
+#include  // required for MPU6060
+#include  // required for MPU6060
+
+
+#define set_I2C_pins  false  
+/* used to boolean for setting RPi I2C pins P1-03 (SDA) and P1-05 (SCL) to alternate function ALT0, which enables those pins for I2C interface. 
+   setI2Cpin should be false, if the I2C are already configured in alt mode ... */
+
+#define i2c_baudrate 400000
+//uint32_t i2c_baudrate = 400000 ; //400 kHz, 
+
+class I2Cdev {
+ public:
+        I2Cdev();
+
+        static void initialize();
+        static void enable(bool isEnabled);
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+        static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+};
+
+#endif /* _I2CDEV_H_ */
diff --git a/RaspberryPi_bcm2835/MPU6050/MPU6050.cpp b/RaspberryPi_bcm2835/MPU6050/MPU6050.cpp
new file mode 100644
index 00000000..fe433e9f
--- /dev/null
+++ b/RaspberryPi_bcm2835/MPU6050/MPU6050.cpp
@@ -0,0 +1,3213 @@
+// 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 
+// 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"
+
+/** 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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 

+ *
+ * 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;
+    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) {
+            setMemoryBank(bank);
+            setMemoryStartAddress(address);
+            I2Cdev::readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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, true);
+}
+bool MPU6050::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer = 0;
+	uint8_t success, special;
+    uint16_t i, j;
+    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);
+}
\ No newline at end of file
diff --git a/RaspberryPi_bcm2835/MPU6050/MPU6050.h b/RaspberryPi_bcm2835/MPU6050/MPU6050.h
new file mode 100644
index 00000000..c3f878a3
--- /dev/null
+++ b/RaspberryPi_bcm2835/MPU6050/MPU6050.h
@@ -0,0 +1,1032 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 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 "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
+
+#ifdef __AVR__
+#include 
+#else
+#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
+#endif
+
+
+#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
+
+    private:
+        uint8_t devAddr;
+        uint8_t buffer[14];
+};
+
+#endif /* _MPU6050_H_ */
\ No newline at end of file
diff --git a/RaspberryPi_bcm2835/MPU6050/examples/IMU_zero.cpp b/RaspberryPi_bcm2835/MPU6050/examples/IMU_zero.cpp
new file mode 100644
index 00000000..1d72afc1
--- /dev/null
+++ b/RaspberryPi_bcm2835/MPU6050/examples/IMU_zero.cpp
@@ -0,0 +1,349 @@
+/*
+I2Cdev library collection - MPU6050 RPi calibration program
+Based on the example in Arduino/MPU6050/
+
+==============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+
+To compile on a Raspberry Pi (1 or 2 or 3)
+  1. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html
+  2. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c
+  3. connect your i2c devices
+  4. then from bash
+      $ PATH_I2CDEVLIB=~/i2cdevlib/
+      $ gcc -o IMU_zero ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/examples/IMU_zero.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/ ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/MPU6050.cpp -l bcm2835 -l m
+      $ sudo ./IMU_zero
+
+If an MPU6050 
+      * is an ideal member of its tribe, 
+      * is properly warmed up, 
+      * is at rest in a neutral position, 
+      * is in a location where the pull of gravity is exactly 1g, and 
+      * has been loaded with the best possible offsets, 
+then it will report 0 for all accelerations and displacements, except for 
+Z acceleration, for which it will report 16384 (that is, 2^14).  Your device 
+probably won't do quite this well, but good offsets will all get the baseline 
+outputs close to these target values.
+
+  Put the MPU6050 on a flat and horizontal surface, and leave it operating for 
+5-10 minutes so its temperature gets stabilized.
+
+  Run this program.  A "----- done -----" line will indicate that it has done its best.
+With the current accuracy-related constants (NFast = 1000, NSlow = 10000), it will take 
+a few minutes to get there.
+
+  Along the way, it will generate a dozen or so lines of output, showing that for each 
+of the 6 desired offsets, it is 
+      * first, trying to find two estimates, one too low and one too high, and
+      * then, closing in until the bracket can't be made smaller.
+
+  The line just above the "done" line will look something like
+    [567,567] --> [-1,2]  [-2223,-2223] --> [0,1] [1131,1132] --> [16374,16404] [155,156] --> [-1,1]  [-25,-24] --> [0,3] [5,6] --> [0,4]
+As will have been shown in interspersed header lines, the six groups making up this
+line describe the optimum offsets for the X acceleration, Y acceleration, Z acceleration,
+X gyro, Y gyro, and Z gyro, respectively.  In the sample shown just above, the trial showed
+that +567 was the best offset for the X acceleration, -2223 was best for Y acceleration, 
+and so on.
+
+  The need for the delay between readings (usDelay) was brought to attention by Nikolaus Doppelhammer.
+===============================================
+*/
+
+
+#include 
+#include 
+#include 
+#include "I2Cdev.h"
+#include "MPU6050.h"
+#include 
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 accelgyro;
+//MPU6050 accelgyro(0x69); // <-- use for AD0 high
+
+
+const char LBRACKET = '[';
+const char RBRACKET = ']';
+const char COMMA    = ',';
+const char BLANK    = ' ';
+const char PERIOD   = '.';
+
+const int iAx = 0;
+const int iAy = 1;
+const int iAz = 2;
+const int iGx = 3;
+const int iGy = 4;
+const int iGz = 5;
+
+const int usDelay = 3150;   // empirical, to hold sampling to 200 Hz
+const int NFast =  1000;    // the bigger, the better (but slower)
+const int NSlow = 10000;    // ..
+const int LinesBetweenHeaders = 5;
+
+
+int LowValue[6];
+int HighValue[6];
+int Smoothed[6];
+int LowOffset[6];
+int HighOffset[6];
+int Target[6];
+int LinesOut;
+int N;
+
+
+void ForceHeader()
+{ 
+	LinesOut = 99; 
+}
+    
+void GetSmoothed()
+{ 
+	int16_t RawValue[6];
+	int i;
+	long Sums[6];
+	for (i = iAx; i <= iGz; i++)
+	{ 
+		Sums[i] = 0; 
+	}
+	//    unsigned long Start = micros();
+
+	for (i = 1; i <= N; i++)
+	{ 
+		// get sums
+		accelgyro.getMotion6(&RawValue[iAx], &RawValue[iAy], &RawValue[iAz], 
+							&RawValue[iGx], &RawValue[iGy], &RawValue[iGz]);
+		if ((i % 500) == 0)
+		{
+			printf("%c", PERIOD);
+		}
+		delayMicroseconds(usDelay);
+		for (int j = iAx; j <= iGz; j++)
+		{
+			Sums[j] = Sums[j] + RawValue[j];
+		}
+	} // get sums
+	//    unsigned long usForN = micros() - Start;
+	//    printf(" reading at %d Hz\n", 1000000/((usForN+N/2)/N));
+	for (i = iAx; i <= iGz; i++)
+	{ 
+		Smoothed[i] = (Sums[i] + N/2) / N ; 
+	}
+} // GetSmoothed
+
+void Initialize()
+{
+	// initialize device
+	printf("Initializing I2C devices...\n");
+	I2Cdev::initialize();
+
+	// verify connection
+	printf("Testing device connections...\n");
+	if(!accelgyro.testConnection()) 
+	{
+		printf("MPU6050 connection failed\n");
+		return;
+	}
+
+	printf("MPU6050 connection successful\n"); 
+	accelgyro.initialize();
+	printf("Initialization done!\n");
+} // Initialize
+
+void SetAveraging(int NewN)
+{ 
+	N = NewN;
+	printf("averaging %d  readings each time\n", N);
+} // SetAveraging
+
+void SetOffsets(int TheOffsets[6])
+{ 
+	accelgyro.setXAccelOffset(TheOffsets [iAx]);
+	accelgyro.setYAccelOffset(TheOffsets [iAy]);
+	accelgyro.setZAccelOffset(TheOffsets [iAz]);
+	accelgyro.setXGyroOffset (TheOffsets [iGx]);
+	accelgyro.setYGyroOffset (TheOffsets [iGy]);
+	accelgyro.setZGyroOffset (TheOffsets [iGz]);
+} // SetOffsets
+
+void ShowProgress()
+{ 
+	if (LinesOut >= LinesBetweenHeaders)
+	{ 
+		// show header
+		printf("\tXAccel\t\t\tYAccel\t\t\t\tZAccel\t\t\tXGyro\t\t\tYGyro\t\t\tZGyro\n");
+		LinesOut = 0;
+	} // show header
+	printf("%c", BLANK);
+	for (int i = iAx; i <= iGz; i++)
+	{ 
+		printf("%c%d%c%d] --> [%d%c%d", LBRACKET, LowOffset[i], COMMA, HighOffset[i],
+						LowValue[i], COMMA, HighValue[i]);
+		if (i == iGz)
+		{ 
+			printf("%c\n", RBRACKET); 
+		}
+		else
+		{ 
+			printf("]\t"); 
+		}
+	}
+	LinesOut++;
+} // ShowProgress
+
+void PullBracketsIn()
+{ 
+	bool AllBracketsNarrow;
+	bool StillWorking;
+	int NewOffset[6];
+
+	printf("\nclosing in:\n");
+	AllBracketsNarrow = false;
+	ForceHeader();
+	StillWorking = true;
+	while (StillWorking) 
+	{ 
+		StillWorking = false;
+		if (AllBracketsNarrow && (N == NFast))
+		{ 
+			SetAveraging(NSlow); 
+		}
+		else
+		{ 
+			AllBracketsNarrow = true; 
+		}// tentative
+		for (int i = iAx; i <= iGz; i++)
+		{ 
+			if (HighOffset[i] <= (LowOffset[i]+1))
+			{ 
+			NewOffset[i] = LowOffset[i]; 
+			}
+			else
+			{ 
+			// binary search
+				StillWorking = true;
+				NewOffset[i] = (LowOffset[i] + HighOffset[i]) / 2;
+				if (HighOffset[i] > (LowOffset[i] + 10))
+				{ 
+					AllBracketsNarrow = false; 
+				}
+			} // binary search
+		}
+		SetOffsets(NewOffset);
+		GetSmoothed();
+		for (int i = iAx; i <= iGz; i++)
+		{ 
+			// closing in
+			if (Smoothed[i] > Target[i])
+			{ 
+				// use lower half
+				HighOffset[i] = NewOffset[i];
+				HighValue[i] = Smoothed[i];
+			} // use lower half
+			else
+			{ 
+				// use upper half
+				LowOffset[i] = NewOffset[i];
+				LowValue[i] = Smoothed[i];
+			} // use upper half
+		} // closing in
+		ShowProgress();
+	} // still working
+} // PullBracketsIn
+
+void PullBracketsOut()
+{ 
+	bool Done = false;
+	int NextLowOffset[6];
+	int NextHighOffset[6];
+
+	printf("expanding:\n");
+	ForceHeader();
+
+	while (!Done)
+	{
+		Done = true;
+		SetOffsets(LowOffset);
+		GetSmoothed();
+		for (int i = iAx; i <= iGz; i++)
+		{
+			// got low values
+			LowValue[i] = Smoothed[i];
+			if (LowValue[i] >= Target[i])
+			{
+				Done = false;
+				NextLowOffset[i] = LowOffset[i] - 1000;
+			}
+			else
+			{
+				NextLowOffset[i] = LowOffset[i]; 
+			}
+		} // got low values
+
+		SetOffsets(HighOffset);
+		GetSmoothed();
+		for (int i = iAx; i <= iGz; i++)
+		{
+			// got high values
+			HighValue[i] = Smoothed[i];
+			if (HighValue[i] <= Target[i])
+			{
+				Done = false;
+				NextHighOffset[i] = HighOffset[i] + 1000;
+			}
+			else
+			{ 
+				NextHighOffset[i] = HighOffset[i]; 
+			}
+		} // got high values
+		ShowProgress();
+		for (int i = iAx; i <= iGz; i++)
+		{ 
+			LowOffset[i] = NextLowOffset[i];   // had to wait until ShowProgress done
+			HighOffset[i] = NextHighOffset[i]; // ..
+		}
+	} // keep going
+} // PullBracketsOut
+
+
+
+int main(int argc, char **argv)
+{ 
+	Initialize();
+	for (int i = iAx; i <= iGz; i++)
+	{ // set targets and initial guesses
+		Target[i] = 0; // must fix for ZAccel 
+		HighOffset[i] = 0;
+		LowOffset[i] = 0;
+	} // set targets and initial guesses
+	Target[iAz] = 16384;
+	SetAveraging(NFast);
+	PullBracketsOut();
+	PullBracketsIn();
+
+	printf("-------------- done --------------\n\n");
+	return 0;
+}
diff --git a/RaspberryPi_bcm2835/MPU6050/examples/MPU6050_example_1.cpp b/RaspberryPi_bcm2835/MPU6050/examples/MPU6050_example_1.cpp
new file mode 100644
index 00000000..5e4b6ac8
--- /dev/null
+++ b/RaspberryPi_bcm2835/MPU6050/examples/MPU6050_example_1.cpp
@@ -0,0 +1,91 @@
+/*
+I2Cdev library collection - MPU6050 RPi example
+Based on the example in Arduino/MPU6050/
+
+==============================================
+I2Cdev device library code is placed under the MIT license
+
+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.
+===============================================
+
+To compile on a Raspberry Pi (1 or 2)
+  1. install the bcm2835 library, see http://www.airspayce.com/mikem/bcm2835/index.html
+  2. enable i2c on your RPi , see https://learn.adafruit.com/adafruits-raspberry-pi-lesson-4-gpio-setup/configuring-i2c
+  3. connect your i2c devices
+  4. then from bash
+      $ PATH_I2CDEVLIB=~/i2cdevlib/
+      $ gcc -o MPU6050_example_1 ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/examples/MPU6050_example_1.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/I2Cdev/I2Cdev.cpp \
+         -I ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/ ${PATH_I2CDEVLIB}RaspberryPi_bcm2835/MPU6050/MPU6050.cpp -l bcm2835 -l m
+      $ sudo ./MPU6050_example_1
+
+*/
+
+#include 
+#include 
+#include "I2Cdev.h"
+#include "MPU6050.h"
+#include 
+
+int main(int argc, char **argv) {
+  printf("MPU6050 3-axis acceleromter example program\n");
+  I2Cdev::initialize();
+  MPU6050 accelgyro ;
+  int16_t ax, ay, az;
+  int16_t gx, gy, gz;
+  if ( accelgyro.testConnection() ) 
+    printf("MPU6050 connection test successful\n") ;
+  else {
+    fprintf( stderr, "MPU6050 connection test failed! something maybe wrong, continuing anyway though ...\n");
+    //return 1;
+  }
+  accelgyro.initialize();
+  // use the code below to change accel/gyro offset values
+  /*
+  printf("Updating internal sensor offsets...\n");
+  // -76	-2359	1688	0	0	0
+  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
+	 accelgyro.getXAccelOffset(),
+	 accelgyro.getYAccelOffset(),
+	 accelgyro.getZAccelOffset(),
+	 accelgyro.getXGyroOffset(),
+	 accelgyro.getYGyroOffset(),
+	 accelgyro.getZGyroOffset());
+  accelgyro.setXGyroOffset(220);
+  accelgyro.setYGyroOffset(76);
+  accelgyro.setZGyroOffset(-85);
+  printf("%i \t %i \t %i \t %i \t %i \t %i\n", 
+	 accelgyro.getXAccelOffset(),
+	 accelgyro.getYAccelOffset(),
+	 accelgyro.getZAccelOffset(),
+	 accelgyro.getXGyroOffset(),
+	 accelgyro.getYGyroOffset(),
+	 accelgyro.getZGyroOffset());
+  */
+  
+  printf("\n");
+  printf("  ax \t ay \t az \t gx \t gy \t gz:\n");
+  while (true) {
+    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+    printf("  %d \t %d \t %d \t %d \t %d \t %d\r", ax, ay, az, gx, gy, gz);
+    fflush(stdout);
+    bcm2835_delay(100);
+  }
+  return 1; 
+}
diff --git a/STM32/HMC5883/HMC5883L.c b/STM32/HMC5883/HMC5883L.c
new file mode 100644
index 00000000..e4adba09
--- /dev/null
+++ b/STM32/HMC5883/HMC5883L.c
@@ -0,0 +1,358 @@
+// I2Cdev library collection - HMC5883L I2C device class header file
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+// 03/28/2017 by Kamnev Yuriy 
+//
+// Changelog:
+//     2017-03-28 - ported to STM32 using Keil MDK Pack
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 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 "HMC5883L.h"
+
+static uint8_t devAddr;
+static uint8_t buffer[6];
+static uint8_t mode;
+
+
+/** Power on and prepare for general usage.
+ * This will prepare the magnetometer with default settings, ready for single-
+ * use mode (very low power requirements). Default settings include 8-sample
+ * averaging, 15 Hz data output rate, normal measurement bias, a,d 1090 gain (in
+ * terms of LSB/Gauss). Be sure to adjust any settings you need specifically
+ * after initialization, especially the gain settings if you happen to be seeing
+ * a lot of -4096 values (see the datasheet for mor information).
+ */
+void HMC5883L_initialize() {
+    devAddr = HMC5883L_DEFAULT_ADDRESS;
+    // write CONFIG_A register
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_CONFIG_A,
+        (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+        (HMC5883L_RATE_15     << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+        (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
+
+    // write CONFIG_B register
+    HMC5883L_setGain(HMC5883L_GAIN_1090);
+
+    // write MODE register
+    HMC5883L_setMode(HMC5883L_MODE_SINGLE);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool HMC5883L_testConnection() {
+    if (I2Cdev_readBytes(devAddr, HMC5883L_RA_ID_A, 3, buffer) == 0) {
+        return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
+    }
+    return false;
+}
+
+// CONFIG_A register
+
+/** Get number of samples averaged per measurement.
+ * @return Current samples averaged per measurement (0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_AVERAGING_8
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+uint8_t HMC5883L_getSampleAveraging() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set number of samples averaged per measurement.
+ * @param averaging New samples averaged per measurement setting(0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+void HMC5883L_setSampleAveraging(uint8_t averaging) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+}
+/** Get data output rate value.
+ * The Table below shows all selectable output rates in continuous measurement
+ * mode. All three channels shall be measured within a given output rate. Other
+ * output rates with maximum rate of 160 Hz can be achieved by monitoring DRDY
+ * interrupt pin in single measurement mode.
+ *
+ * Value | Typical Data Output Rate (Hz)
+ * ------+------------------------------
+ * 0     | 0.75
+ * 1     | 1.5
+ * 2     | 3
+ * 3     | 7.5
+ * 4     | 15 (Default)
+ * 5     | 30
+ * 6     | 75
+ * 7     | Not used
+ *
+ * @return Current rate of data output to registers
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+uint8_t HMC5883L_getDataRate() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set data output rate value.
+ * @param rate Rate of data output to registers
+ * @see getDataRate()
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+void HMC5883L_setDataRate(uint8_t rate) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+}
+/** Get measurement bias value.
+ * @return Current bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+uint8_t HMC5883L_getMeasurementBias() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set measurement bias value.
+ * @param bias New bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+void HMC5883L_setMeasurementBias(uint8_t bias) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
+}
+
+// CONFIG_B register
+
+/** Get magnetic field gain value.
+ * The table below shows nominal gain settings. Use the "Gain" column to convert
+ * counts to Gauss. Choose a lower gain value (higher GN#) when total field
+ * strength causes overflow in one of the data output registers (saturation).
+ * The data output range for all settings is 0xF800-0x07FF (-2048 - 2047).
+ *
+ * Value | Field Range | Gain (LSB/Gauss)
+ * ------+-------------+-----------------
+ * 0     | +/- 0.88 Ga | 1370
+ * 1     | +/- 1.3 Ga  | 1090 (Default)
+ * 2     | +/- 1.9 Ga  | 820
+ * 3     | +/- 2.5 Ga  | 660
+ * 4     | +/- 4.0 Ga  | 440
+ * 5     | +/- 4.7 Ga  | 390
+ * 6     | +/- 5.6 Ga  | 330
+ * 7     | +/- 8.1 Ga  | 230
+ *
+ * @return Current magnetic field gain value
+ * @see HMC5883L_GAIN_1090
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+uint8_t HMC5883L_getGain() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set magnetic field gain value.
+ * @param gain New magnetic field gain value
+ * @see getGain()
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+void HMC5883L_setGain(uint8_t gain) {
+    // use this method to guarantee that bits 4-0 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+}
+
+// MODE register
+
+/** Get measurement mode.
+ * In continuous-measurement mode, the device continuously performs measurements
+ * and places the result in the data register. RDY goes high when new data is
+ * placed in all three registers. After a power-on or a write to the mode or
+ * configuration register, the first measurement set is available from all three
+ * data output registers after a period of 2/fDO and subsequent measurements are
+ * available at a frequency of fDO, where fDO is the frequency of data output.
+ *
+ * When single-measurement mode (default) is selected, device performs a single
+ * measurement, sets RDY high and returned to idle mode. Mode register returns
+ * to idle mode bit values. The measurement remains in the data output register
+ * and RDY remains high until the data output register is read or another
+ * measurement is performed.
+ *
+ * @return Current measurement mode
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+uint8_t HMC5883L_getMode() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer);
+    return buffer[0];
+}
+/** Set measurement mode.
+ * @param newMode New measurement mode
+ * @see getMode()
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+void HMC5883L_setMode(uint8_t newMode) {
+    // use this method to guarantee that bits 7-2 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, newMode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    mode = newMode; // track to tell if we have to clear bit 7 after a read
+}
+
+// DATA* registers
+
+/** Get 3-axis heading measurements.
+ * In the event the ADC reading overflows or underflows for the given channel,
+ * or if there is a math overflow during the bias measurement, this data
+ * register will contain the value -4096. This register value will clear when
+ * after the next valid measurement is made. Note that this method automatically
+ * clears the appropriate bit in the MODE register if Single mode is active.
+ * @param x 16-bit signed integer container for X-axis heading
+ * @param y 16-bit signed integer container for Y-axis heading
+ * @param z 16-bit signed integer container for Z-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+void HMC5883L_getHeading(int16_t *x, int16_t *y, int16_t *z) {
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+    *y = (((int16_t)buffer[4]) << 8) | buffer[5];
+    *z = (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+/** Get X-axis heading measurement.
+ * @return 16-bit signed integer with X-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+int16_t HMC5883L_getHeadingX() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Get Y-axis heading measurement.
+ * @return 16-bit signed integer with Y-axis heading
+ * @see HMC5883L_RA_DATAY_H
+ */
+int16_t HMC5883L_getHeadingY() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[4]) << 8) | buffer[5];
+}
+/** Get Z-axis heading measurement.
+ * @return 16-bit signed integer with Z-axis heading
+ * @see HMC5883L_RA_DATAZ_H
+ */
+int16_t HMC5883L_getHeadingZ() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+
+// STATUS register
+
+/** Get data output register lock status.
+ * This bit is set when this some but not all for of the six data output
+ * registers have been read. When this bit is set, the six data output registers
+ * are locked and any new data will not be placed in these register until one of
+ * three conditions are met: one, all six bytes have been read or the mode
+ * changed, two, the mode is changed, or three, the measurement configuration is
+ * changed.
+ * @return Data output register lock status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_LOCK_BIT
+ */
+bool HMC5883L_getLockStatus() {
+    I2Cdev_readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer);
+    return buffer[0];
+}
+/** Get data ready status.
+ * This bit is set when data is written to all six data registers, and cleared
+ * when the device initiates a write to the data output registers and after one
+ * or more of the data output registers are written to. When RDY bit is clear it
+ * shall remain cleared for 250 us. DRDY pin can be used as an alternative to
+ * the status register for monitoring the device for measurement data.
+ * @return Data ready status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_READY_BIT
+ */
+bool HMC5883L_getReadyStatus() {
+    I2Cdev_readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer);
+    return buffer[0];
+}
+
+// ID_* registers
+
+/** Get identification byte A
+ * @return ID_A byte (should be 01001000, ASCII value 'H')
+ */
+uint8_t HMC5883L_getIDA() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_A, buffer);
+    return buffer[0];
+}
+/** Get identification byte B
+ * @return ID_A byte (should be 00110100, ASCII value '4')
+ */
+uint8_t HMC5883L_getIDB() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_B, buffer);
+    return buffer[0];
+}
+/** Get identification byte C
+ * @return ID_A byte (should be 00110011, ASCII value '3')
+ */
+uint8_t HMC5883L_getIDC() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_C, buffer);
+    return buffer[0];
+}
diff --git a/STM32/HMC5883/HMC5883L.h b/STM32/HMC5883/HMC5883L.h
new file mode 100644
index 00000000..ae7f955a
--- /dev/null
+++ b/STM32/HMC5883/HMC5883L.h
@@ -0,0 +1,142 @@
+// I2Cdev library collection - HMC5883L I2C device class header file
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+// 03/28/2017 by Kamnev Yuriy 
+//
+// Changelog:
+//     2017-03-28 - ported to STM32 using Keil MDK Pack
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 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 _HMC5883L_H_
+#define _HMC5883L_H_
+
+#include "I2Cdev.h"
+#include 
+
+#define HMC5883L_ADDRESS            0x1E // this device only has one address
+#define HMC5883L_DEFAULT_ADDRESS    0x1E
+
+#define HMC5883L_RA_CONFIG_A        0x00
+#define HMC5883L_RA_CONFIG_B        0x01
+#define HMC5883L_RA_MODE            0x02
+#define HMC5883L_RA_DATAX_H         0x03
+#define HMC5883L_RA_DATAX_L         0x04
+#define HMC5883L_RA_DATAZ_H         0x05
+#define HMC5883L_RA_DATAZ_L         0x06
+#define HMC5883L_RA_DATAY_H         0x07
+#define HMC5883L_RA_DATAY_L         0x08
+#define HMC5883L_RA_STATUS          0x09
+#define HMC5883L_RA_ID_A            0x0A
+#define HMC5883L_RA_ID_B            0x0B
+#define HMC5883L_RA_ID_C            0x0C
+
+#define HMC5883L_CRA_AVERAGE_BIT    6
+#define HMC5883L_CRA_AVERAGE_LENGTH 2
+#define HMC5883L_CRA_RATE_BIT       4
+#define HMC5883L_CRA_RATE_LENGTH    3
+#define HMC5883L_CRA_BIAS_BIT       1
+#define HMC5883L_CRA_BIAS_LENGTH    2
+
+#define HMC5883L_AVERAGING_1        0x00
+#define HMC5883L_AVERAGING_2        0x01
+#define HMC5883L_AVERAGING_4        0x02
+#define HMC5883L_AVERAGING_8        0x03
+
+#define HMC5883L_RATE_0P75          0x00
+#define HMC5883L_RATE_1P5           0x01
+#define HMC5883L_RATE_3             0x02
+#define HMC5883L_RATE_7P5           0x03
+#define HMC5883L_RATE_15            0x04
+#define HMC5883L_RATE_30            0x05
+#define HMC5883L_RATE_75            0x06
+
+#define HMC5883L_BIAS_NORMAL        0x00
+#define HMC5883L_BIAS_POSITIVE      0x01
+#define HMC5883L_BIAS_NEGATIVE      0x02
+
+#define HMC5883L_CRB_GAIN_BIT       7
+#define HMC5883L_CRB_GAIN_LENGTH    3
+
+#define HMC5883L_GAIN_1370          0x00
+#define HMC5883L_GAIN_1090          0x01
+#define HMC5883L_GAIN_820           0x02
+#define HMC5883L_GAIN_660           0x03
+#define HMC5883L_GAIN_440           0x04
+#define HMC5883L_GAIN_390           0x05
+#define HMC5883L_GAIN_330           0x06
+#define HMC5883L_GAIN_220           0x07
+
+#define HMC5883L_MODEREG_BIT        1
+#define HMC5883L_MODEREG_LENGTH     2
+
+#define HMC5883L_MODE_CONTINUOUS    0x00
+#define HMC5883L_MODE_SINGLE        0x01
+#define HMC5883L_MODE_IDLE          0x02
+
+#define HMC5883L_STATUS_LOCK_BIT    1
+#define HMC5883L_STATUS_READY_BIT   0
+
+void HMC5883L_initialize();
+bool HMC5883L_testConnection();
+
+// CONFIG_A register
+uint8_t HMC5883L_getSampleAveraging();
+void HMC5883L_setSampleAveraging(uint8_t averaging);
+uint8_t HMC5883L_getDataRate();
+void HMC5883L_setDataRate(uint8_t rate);
+uint8_t HMC5883L_getMeasurementBias();
+void HMC5883L_setMeasurementBias(uint8_t bias);
+
+// CONFIG_B register
+uint8_t HMC5883L_getGain();
+void HMC5883L_setGain(uint8_t gain);
+
+// MODE register
+uint8_t HMC5883L_getMode();
+void HMC5883L_setMode(uint8_t mode);
+
+// DATA* registers
+void HMC5883L_getHeading(int16_t *x, int16_t *y, int16_t *z);
+int16_t HMC5883L_getHeadingX();
+int16_t HMC5883L_getHeadingY();
+int16_t HMC5883L_getHeadingZ();
+
+// STATUS register
+bool HMC5883L_getLockStatus();
+bool HMC5883L_getReadyStatus();
+
+// ID_* registers
+uint8_t HMC5883L_getIDA();
+uint8_t HMC5883L_getIDB();
+uint8_t HMC5883L_getIDC();
+
+
+#endif /* _HMC5883L_H_ */
diff --git a/STM32/I2Cdev.c b/STM32/I2Cdev.c
new file mode 100644
index 00000000..3770dba3
--- /dev/null
+++ b/STM32/I2Cdev.c
@@ -0,0 +1,391 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 03/28/2017 by Kamnev Yuriy 
+//
+// Changelog:
+//     2017-03-28 - ported to STM32 using Keil MDK Pack
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2017 Kamnev Yuriy
+
+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 "I2Cdev.h"
+#include 
+
+#include 
+
+extern ARM_DRIVER_I2C I2CDev_Driver;
+
+
+#define _i2c_transmit(dev_addr, data, len, pending) \
+	I2CDev_Driver.MasterTransmit(dev_addr, data, len, pending);\
+	while(I2CDev_Driver.GetStatus().busy)
+
+
+#define _i2c_receive(dev_addr, data, len, pending) \
+	I2CDev_Driver.MasterReceive(dev_addr, data, len, pending); \
+	while(I2CDev_Driver.GetStatus().busy)
+
+
+#define i2c_transmit_ack(dev_addr, data, len) 	_i2c_transmit(dev_addr, data, len, true)
+#define i2c_transmit_nack(dev_addr, data, len) 	_i2c_transmit(dev_addr, data, len, false)
+
+#define i2c_receive_ack(dev_addr, data, len)	_i2c_receive(dev_addr, data, len, true)
+#define i2c_receive_nack(dev_addr, data, len)	_i2c_receive(dev_addr, data, len, false)
+
+
+/** Read several byte from an 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register regAddr to read from
+ * @param len 		How many bytes to read
+ * @param data 		Buffer to save data into
+ * @return Status of read operation (0 = success, <0 = error)
+ */
+int8_t I2Cdev_readBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t len, uint8_t *data) {
+	int8_t err = 0;
+	uint8_t reg_data[1] = {reg_addr};
+
+	err = i2c_transmit_ack(dev_addr, reg_data, 1);
+
+	if(err < 0) {
+		return err;
+	}
+
+	err = i2c_receive_nack(dev_addr, data, len);
+
+	return err;
+}
+
+
+/** Read a single byte from a 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register reg_addr to read from
+ * @param data 		Buffer to save data into
+ * @return Status of read operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_readByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t *data) {
+	return I2Cdev_readBytes(dev_addr, reg_addr, 1, data);
+}
+
+
+/** Read a several 16-bit words from a 16-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register reg_addr to read from
+ * @param len		Number of words to read
+ * @param data 		Buffer to save data into
+ * @return Status of read operation (true = success)
+ */
+int8_t 	I2Cdev_readWords(uint8_t dev_addr, uint8_t reg_addr, uint8_t len, uint16_t *data) {
+	int8_t err;
+	uint16_t bytes_num = len*2;
+
+	uint8_t reg_info[1] = {reg_addr};
+	err = i2c_transmit_ack(dev_addr, reg_info, 1);
+
+	if(err < 0) {
+		return err;
+	}
+
+	uint8_t words_in_bytes[bytes_num];
+	err = i2c_receive_nack(dev_addr, words_in_bytes, bytes_num);
+
+	if(err < 0) {
+		return err;
+	}
+
+	uint8_t words_cnt = 0;
+	for(uint16_t i=0; i> bitn) & 0x01;
+
+	return err;
+}
+
+
+/** Read several bits from a 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register regAddr to read from
+ * @param start_bit First bit position to read (0-7)
+ * @param len		Number of bits to read (<= 8)
+ * @param data 		Container for right-aligned value
+ * @return Status of read operation (0 = success, <0 = error)
+ */
+int8_t I2Cdev_readBits(uint8_t dev_addr, uint8_t reg_addr, uint8_t start_bit, 
+		uint8_t len, uint8_t *data) 
+{
+	int8_t err;
+
+	uint8_t b;
+	if ((err = I2Cdev_readByte(dev_addr, reg_addr, &b)) == 0) {
+		uint8_t mask = ((1 << len) - 1) << (start_bit - len + 1);
+		b &= mask;
+		b >>= (start_bit - len + 1);
+		*data = b;
+	}
+
+	return err;
+}
+
+
+/** Read a single bit from a 16-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register regAddr to read from
+ * @param bit_n 	Bit position to read (0-15)
+ * @param data 		Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t 	I2Cdev_readBitW(uint8_t dev_addr, uint8_t reg_addr, uint8_t bit_n, uint16_t *data) {
+	int8_t err;
+
+	err = I2Cdev_readWord(dev_addr, reg_addr, data);
+	*data = (*data >> bit_n) & 0x01;
+
+	return err;
+}
+
+
+/** Read several bits from a 16-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register regAddr to read from
+ * @param start_bit First bit position to read (0-15)
+ * @param len		Number of bits to read (<= 16)
+ * @param data 		Container for right-aligned value
+ * @return Status of read operation (0 = success, <0 = error)
+ */
+int8_t I2Cdev_readBitsW(uint8_t dev_addr, uint8_t reg_addr, uint8_t start_bit,
+		uint8_t len, uint16_t *data)
+{
+    int8_t err;
+    uint16_t w;
+
+    if ((err = I2Cdev_readWord(dev_addr, reg_addr, &w)) == 0) {
+        uint16_t mask = ((1 << len) - 1) << (start_bit - len + 1);
+        w &= mask;
+        w >>= (start_bit - len + 1);
+        *data = w;
+    }
+
+    return err;
+}
+
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	First register address to write to
+ * @param len 		Number of bytes to write
+ * @param data 		Buffer to copy new data from
+ * @return Status of operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_writeBytes(uint8_t dev_addr, uint8_t reg_addr, uint8_t len, uint8_t *data) {
+	int8_t err;
+	uint8_t ts_data[len+1];
+
+	ts_data[0] = reg_addr;
+	memcpy(ts_data+1, data, len);
+
+	err = i2c_transmit_nack(dev_addr, ts_data, len+1);
+	return err;
+}
+
+
+/** Write single byte to an 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register address to write to
+ * @param data 		New byte value to write
+ * @return Status of operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_writeByte(uint8_t dev_addr, uint8_t reg_addr, uint8_t data) {
+	int8_t err;
+
+	uint8_t ts_data[2] = {reg_addr, data};
+	err = i2c_transmit_nack(dev_addr, ts_data, 2);
+
+	return err;
+}
+
+
+/** Write single 16-bit word to an 16-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register address to write to
+ * @param data 		New byte value to write
+ * @return Status of operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_writeWord(uint8_t dev_addr, uint8_t reg_addr, uint16_t data) {
+	int8_t err;
+	uint8_t ts_data[3] = {reg_addr, (data >> 8) & 0xFF, data & 0xFF};
+
+	err = i2c_transmit_nack(dev_addr, ts_data, 3);
+	return err;
+}
+
+
+/** Write multiple words to a 16-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	First register address to write to
+ * @param len 		Number of words to write
+ * @param data 		Buffer to copy new data from
+ * @return Status of operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_writeWords(uint8_t dev_addr, uint8_t reg_addr, uint8_t len, uint16_t *data) {
+	uint16_t bytes_num = len*2+1;
+	uint8_t bytes[bytes_num];
+
+	bytes[0] = reg_addr;
+
+	uint16_t bytes_pos = 1;
+	for(uint8_t i=0; i> 8) & 0xFF;
+		bytes[bytes_pos+1] = data[i] & 0xFF;
+
+		bytes_pos += 2;
+	}
+
+	return i2c_transmit_nack(dev_addr, bytes, bytes_num);
+}
+
+
+/** write a single bit in an 8-bit device register.
+ * @param dev_addr 	I2C slave device address
+ * @param reg_addr 	Register regAddr to write to
+ * @param bit_n 	Bit position to write (0-7)
+ * @param data 		New bit value to write
+ * @return Status of operation (0 = success, <0 = error)
+ */
+int8_t 	I2Cdev_writeBit(uint8_t dev_addr, uint8_t reg_addr, uint8_t bit_n, uint8_t data) {
+	uint8_t b;
+	int8_t err;
+
+	err = I2Cdev_readByte(dev_addr, reg_addr, &b);
+	if(err < 0) {
+		return err;
+	}
+
+	b = (data != 0) ? (b | (1<
+// 03/28/2017 by Kamnev Yuriy 
+//
+// Changelog:
+//     2017-03-28 - ported to STM32 using Keil MDK Pack
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2017 Kamnev Yuriy
+
+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 SRC_I2CDEVLIB_I2CDEV_H_
+#define SRC_I2CDEVLIB_I2CDEV_H_
+
+#include 
+
+#define I2CDev_Driver	Driver_I2C2
+
+
+int8_t 	I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+int8_t 	I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+int8_t 	I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+int8_t 	I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+int8_t 	I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+int8_t 	I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+int8_t 	I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+int8_t 	I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+int8_t 	I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+int8_t 	I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+int8_t 	I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+int8_t 	I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+int8_t 	I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+int8_t 	I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+int8_t 	I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+int8_t 	I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+
+#endif /* SRC_I2CDEVLIB_I2CDEV_H_ */
diff --git a/STM32/MPU6050/MPU6050.c b/STM32/MPU6050/MPU6050.c
new file mode 100644
index 00000000..524e3ff8
--- /dev/null
+++ b/STM32/MPU6050/MPU6050.c
@@ -0,0 +1,3143 @@
+// 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 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+//     2017-03-28 - tested basic function on STM32
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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"
+
+
+MPU6050_t mpu6050;
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+void MPU6050(uint8_t address) {
+    mpu6050.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() {
+    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    MPU6050_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 MPU6050_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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_SMPLRT_DIV, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050_getExternalFrameSync() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// 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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050_getDHPFMode() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_THR, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+/** Set free-fall 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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getTempFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getXGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getYGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getZGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getAccelFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave2FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave1FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave0FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+bool MPU6050_getSlave3FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050_getMasterClockSpeed() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_ADDR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_REG, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_DI, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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) {
+    MPU6050_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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, mpu6050.buffer);
+    *ax = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *ay = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *az = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
+    *gx = (((int16_t)mpu6050.buffer[8]) << 8) | mpu6050.buffer[9];
+    *gy = (((int16_t)mpu6050.buffer[10]) << 8) | mpu6050.buffer[11];
+    *gz = (((int16_t)mpu6050.buffer[12]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_TEMP_OUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, mpu6050.buffer);
+    return (((uint32_t)mpu6050.buffer[0]) << 24) | (((uint32_t)mpu6050.buffer[1]) << 16) | (((uint16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+}
+
+// MOT_DETECT_STATUS register
+
+/** 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer is disabled. The FIFO mpu6050.buffer
+ * cannot be written to or read from while disabled. The FIFO mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO mpu6050.buffer size.
+ * This value indicates the number of bytes stored in the FIFO mpu6050.buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO mpu6050.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 mpu6050.buffer size
+ */
+uint16_t MPU6050_getFIFOCount() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_COUNTH, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO mpu6050.buffer.
+ * This register is used to read and write data from the FIFO mpu6050.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 mpu6050.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 mpu6050.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 mpu6050.buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO mpu6050.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 mpu6050.buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO mpu6050.buffer
+ */
+uint8_t MPU6050_getFIFOByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO mpu6050.buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_setFIFOByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.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(mpu6050.devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setOTPBankValid(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050_getXGyroOffsetTC() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050_getYFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050_getZFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050_getXAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050_getYAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050_getZAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050_getXGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050_getYGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050_getZGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050_getIntPLLReadyEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050_getIntDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050_getDMPInt5Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt4Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt3Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt2Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt1Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt0Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050_getIntPLLReadyStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getIntDMPStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_getDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_resetDMP() {
+    I2Cdev_writeBit(mpu6050.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(mpu6050.devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_setMemoryStartAddress(uint8_t address) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050_readMemoryByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_writeMemoryByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_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(mpu6050.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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+        }
+    }
+}
+/*bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer;
+    uint8_t *progBuffer;
+    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(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+            I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                //Serial.print("Block write verification error, bank ");
+                //Serial.print(bank, DEC);
+                //Serial.print(", address ");
+                //Serial.print(address, DEC);
+                //Serial.print("!\nExpected:");
+                //for (j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (progBuffer[j] < 16) Serial.print("0");
+                //    Serial.print(progBuffer[j], HEX);
+                //}
+                //Serial.print("\nReceived:");
+                //for (uint8_t j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                //    Serial.print(verifyBuffer[i + j], HEX);
+                //}
+                Serial.print("\n");
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_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 MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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 = MPU6050_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(mpu6050.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 MPU6050_writeDMPConfigurationSet(data, dataSize, true);
+}*/
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_getDMPConfig1() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig1(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050_getDMPConfig2() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
diff --git a/STM32/MPU6050/MPU6050.h b/STM32/MPU6050/MPU6050.h
new file mode 100644
index 00000000..6e4cf693
--- /dev/null
+++ b/STM32/MPU6050/MPU6050.h
@@ -0,0 +1,788 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+//     2017-03-11 - tested basic functions on STM32
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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 "I2Cdev.h"
+#include 
+
+#if ((defined MPU6050_INCLUDE_DMP_MOTIONAPPS20) || (defined MPU6050_INCLUDE_DMP_MOTIONAPPS41))
+    #error DMP is not supported yet
+#endif
+
+#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_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_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
+
+typedef struct MPU6050_t {
+    uint8_t devAddr;
+    uint8_t buffer[14];
+} MPU6050_t;
+
+void MPU6050(uint8_t address);
+
+void MPU6050_initialize();
+bool MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8_t MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8_t level);
+
+// SMPLRT_DIV register
+uint8_t MPU6050_getRate();
+void MPU6050_setRate(uint8_t rate);
+
+// CONFIG register
+uint8_t MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8_t sync);
+uint8_t MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8_t bandwidth);
+
+// GYRO_CONFIG register
+uint8_t MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8_t range);
+
+// ACCEL_CONFIG register
+bool MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+bool MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+bool MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8_t MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8_t range);
+uint8_t MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8_t mode);
+
+// FF_THR register
+uint8_t MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8_t threshold);
+
+// FF_DUR register
+uint8_t MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8_t duration);
+
+// MOT_THR register
+uint8_t MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8_t threshold);
+
+// MOT_DUR register
+uint8_t MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8_t duration);
+
+// ZRMOT_THR register
+uint8_t MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8_t threshold);
+
+// ZRMOT_DUR register
+uint8_t MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8_t duration);
+
+// FIFO_EN register
+bool MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+bool MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+bool MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+bool MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+bool MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+bool MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+bool MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+bool MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+bool MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+bool MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+bool MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+bool MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8_t MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8_t speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8_t MPU6050_getSlaveAddress(uint8_t num);
+void MPU6050_setSlaveAddress(uint8_t num, uint8_t address);
+uint8_t MPU6050_getSlaveRegister(uint8_t num);
+void MPU6050_setSlaveRegister(uint8_t num, uint8_t reg);
+bool MPU6050_getSlaveEnabled(uint8_t num);
+void MPU6050_setSlaveEnabled(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWordByteSwap(uint8_t num);
+void MPU6050_setSlaveWordByteSwap(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWriteMode(uint8_t num);
+void MPU6050_setSlaveWriteMode(uint8_t num, bool mode);
+bool MPU6050_getSlaveWordGroupOffset(uint8_t num);
+void MPU6050_setSlaveWordGroupOffset(uint8_t num, bool enabled);
+uint8_t MPU6050_getSlaveDataLength(uint8_t num);
+void MPU6050_setSlaveDataLength(uint8_t num, uint8_t length);
+
+// I2C_SLV* registers (Slave 4)
+uint8_t MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8_t address);
+uint8_t MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8_t reg);
+void MPU6050_setSlave4OutputByte(uint8_t data);
+bool MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+bool MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+bool MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8_t MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8_t delay);
+uint8_t MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+bool MPU6050_getPassthroughStatus();
+bool MPU6050_getSlave4IsDone();
+bool MPU6050_getLostArbitration();
+bool MPU6050_getSlave4Nack();
+bool MPU6050_getSlave3Nack();
+bool MPU6050_getSlave2Nack();
+bool MPU6050_getSlave1Nack();
+bool MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+bool MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+bool MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+bool MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+bool MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+bool MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+bool MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+bool MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+bool MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8_t MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8_t enabled);
+bool MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+bool MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+bool MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+bool MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+bool MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+bool MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8_t MPU6050_getIntStatus();
+bool MPU6050_getIntFreefallStatus();
+bool MPU6050_getIntMotionStatus();
+bool MPU6050_getIntZeroMotionStatus();
+bool MPU6050_getIntFIFOBufferOverflowStatus();
+bool MPU6050_getIntI2CMasterStatus();
+bool MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getAccelerationX();
+int16_t MPU6050_getAccelerationY();
+int16_t MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16_t MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getRotationX();
+int16_t MPU6050_getRotationY();
+int16_t MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8_t MPU6050_getExternalSensorByte(int position);
+uint16_t MPU6050_getExternalSensorWord(int position);
+uint32_t getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+bool MPU6050_getXNegMotionDetected();
+bool MPU6050_getXPosMotionDetected();
+bool MPU6050_getYNegMotionDetected();
+bool MPU6050_getYPosMotionDetected();
+bool MPU6050_getZNegMotionDetected();
+bool MPU6050_getZPosMotionDetected();
+bool MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8_t num, uint8_t data);
+
+// I2C_MST_DELAY_CTRL register
+bool MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+bool MPU6050_getSlaveDelayEnabled(uint8_t num);
+void MPU6050_setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8_t MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8_t delay);
+uint8_t MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8_t decrement);
+uint8_t MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8_t decrement);
+
+// USER_CTRL register
+bool MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+bool MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+bool MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+bool MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+bool MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8_t MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8_t source);
+
+// PWR_MGMT_2 register
+uint8_t MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8_t frequency);
+bool MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+bool MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+bool MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+bool MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+bool MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+bool MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16_t MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8_t MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8_t data);
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length);
+
+// WHO_AM_I register
+uint8_t MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8_t id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8_t MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8_t getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8_t offset);
+
+// YG_OFFS_TC register
+int8_t getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8_t offset);
+
+// ZG_OFFS_TC register
+int8_t getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8_t offset);
+
+// X_FINE_GAIN register
+int8_t getXFineGain();
+void MPU6050_setXFineGain(int8_t gain);
+
+// Y_FINE_GAIN register
+int8_t getYFineGain();
+void MPU6050_setYFineGain(int8_t gain);
+
+// Z_FINE_GAIN register
+int8_t getZFineGain();
+void MPU6050_setZFineGain(int8_t gain);
+
+// XA_OFFS_* registers
+int16_t MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16_t offset);
+
+// YA_OFFS_* register
+int16_t MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16_t offset);
+
+// ZA_OFFS_* register
+int16_t MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16_t offset);
+
+// XG_OFFS_USR* registers
+int16_t MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16_t offset);
+
+// YG_OFFS_USR* register
+int16_t MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16_t offset);
+
+// ZG_OFFS_USR* register
+int16_t MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16_t offset);
+
+// INT_ENABLE register (DMP functions)
+bool MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+bool MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+bool MPU6050_getDMPInt5Status();
+bool MPU6050_getDMPInt4Status();
+bool MPU6050_getDMPInt3Status();
+bool MPU6050_getDMPInt2Status();
+bool MPU6050_getDMPInt1Status();
+bool MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+bool MPU6050_getIntPLLReadyStatus();
+bool MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+bool MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank);
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8_t address);
+
+// MEM_R_W register
+uint8_t MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8_t data);
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address);
+//bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem);
+//bool MPU6050_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify);
+
+//bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem);
+//bool MPU6050_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
+
+// DMP_CFG_1 register
+uint8_t MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8_t config);
+
+// DMP_CFG_2 register
+uint8_t MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8_t config);
+
+#endif /* _MPU6050_H_ */
diff --git a/STM32/QMC5883L/QMC5883L.c b/STM32/QMC5883L/QMC5883L.c
new file mode 100644
index 00000000..e0bf2232
--- /dev/null
+++ b/STM32/QMC5883L/QMC5883L.c
@@ -0,0 +1,69 @@
+// I2Cdev library collection - QMC5883L I2C device class header file
+// Based on QST QMC5883L datasheet 1.0, 02/2016
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 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 
+#include 
+
+static const uint8_t chip_addr = QMC5883L_DEFAULT_ADDR;
+
+bool QMC5883L_soft_reset() {
+  if (I2Cdev_writeByte(chip_addr, QMC5883L_REG_CTRL_2, 0x80) == 0) return false;
+  return true;
+}
+
+bool QMC5883L_fbr_set(uint8_t fbr) {
+  if (I2Cdev_writeByte(chip_addr, QMC5883L_REG_SR_PERIOD, fbr) == 0) return false;
+  return true;
+}
+
+bool QMC5883L_control_1_set(uint8_t value) {
+  if (I2Cdev_writeByte(chip_addr, QMC5883L_REG_CTRL_1, value) == 0) return false;
+  return true;
+}
+
+bool QMC5883L_statusGet(uint8_t *data) {
+  if (I2Cdev_readByte(chip_addr, QMC5883L_REG_STATUS, data) == 0) return false;
+  return true;
+}
+
+bool QMC5883L_magGet(int16_t *v_mag) {
+  uint8_t bytes[6];
+  if (I2Cdev_readBytes(chip_addr, QMC5883L_REG_DATA_X_LSB, 6, bytes) == 0) return false;
+
+  v_mag[0] = (((uint16_t)bytes[1]) << 8) | bytes[0];
+  v_mag[1] = (((uint16_t)bytes[3]) << 8) | bytes[2];
+  v_mag[2] = (((uint16_t)bytes[5]) << 8) | bytes[4];
+
+  return true;
+}
+
+bool QMC5883L_tempGet(int16_t *temp) {
+  uint8_t bytes[2];
+  if (I2Cdev_readBytes(chip_addr, QMC5883L_REG_TEMP_LSB, 2, bytes) == 0) return false;
+
+  *temp  = (((uint16_t)bytes[1]) << 8) | bytes[0];
+
+  return true;
+}
diff --git a/STM32/QMC5883L/QMC5883L.h b/STM32/QMC5883L/QMC5883L.h
new file mode 100644
index 00000000..db929656
--- /dev/null
+++ b/STM32/QMC5883L/QMC5883L.h
@@ -0,0 +1,91 @@
+// I2Cdev library collection - QMC5883L I2C device class header file
+// Based on QST QMC5883L datasheet 1.0, 02/2016
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 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 _QMC5883L_H_
+#define _QMC5883L_H_
+
+#include 
+#include 
+
+#define QMC5883L_DEFAULT_ADDR 0x0D
+
+/* register addresses */
+#define QMC5883L_REG_DATA_X_LSB 0x00
+#define QMC5883L_REG_DATA_X_MSB 0x01
+#define QMC5883L_REG_DATA_Y_LSB 0x02
+#define QMC5883L_REG_DATA_Y_MSB 0x03
+#define QMC5883L_REG_DATA_Z_LSB 0x04
+#define QMC5883L_REG_DATA_Z_MSB 0x05
+#define QMC5883L_REG_STATUS     0x06
+#define QMC5883L_REG_TEMP_LSB   0x07
+#define QMC5883L_REG_TEMP_MSB   0x08
+#define QMC5883L_REG_CTRL_1     0x09
+#define QMC5883L_REG_CTRL_2     0x0A
+#define QMC5883L_REG_SR_PERIOD  0x0B
+
+/* values for control_1 register */
+#define QMC5883L_OVERSAMPLE_512 0b00
+#define QMC5883L_OVERSAMPLE_256 0b01
+#define QMC5883L_OVERSAMPLE_128 0b10
+#define QMC5883L_OVERSAMPLE_64  0b11
+
+#define QMC5883L_SCALE_2G  0b00
+#define QMC5883L_SCALE_8G  0b01
+ 
+#define QMC5883L_OUTPUT_RATE_10HZ  0b00
+#define QMC5883L_OUTPUT_RATE_50HZ  0b01
+#define QMC5883L_OUTPUT_RATE_100HZ 0b10
+#define QMC5883L_OUTPUT_RATE_200HZ 0b11
+ 
+#define QMC5883L_MODE_STBY 0b00
+#define QMC5883L_MODE_CONT 0b01
+
+#define QMC5883L_CTRL1_VALUE(_mode_, _output_rate_, _scale_, _oversample_) \
+  ((_mode_) | ((_output_rate_) << 2) | ((_scale_) << 4) | ((_oversample_) << 6))
+
+/* QMC5883L_soft_reset: does a soft reset */
+bool QMC5883L_soft_reset();
+
+/* QMC5883L_fbr_set: SET/RESET Period FBR; recommended value: 1 */
+bool QMC5883L_fbr_set(uint8_t fbr);
+
+/* QMC5883L_control_1_set: set value for control register (mode, output rate, scale, oversampling) */
+bool QMC5883L_control_1_set(uint8_t value);
+
+bool QMC5883L_statusGet(uint8_t *data);
+
+/* QMC5883L_magGet:
+ * parameters:
+ *   v_mag: int16_t array of length 3
+ * returns:
+ *   success: true
+ *   failure: false
+ */
+bool QMC5883L_magGet(int16_t *v_mag);
+
+bool QMC5883L_tempGet(int16_t *temp);
+
+#endif
diff --git a/STM32/README.md b/STM32/README.md
new file mode 100644
index 00000000..20de58f8
--- /dev/null
+++ b/STM32/README.md
@@ -0,0 +1,15 @@
+# I2C Device Library for STM32
+
+## I2Cdev
+For implementing I2Cdev [Keil MDK Pack](https://www.keil.com/dd2/pack/) is used. It has many advantages comparing to HAL/SPL. The most
+crucial one is that of compatibility: using it makes I2Cdev compatible with any device listed on MDK Pack software page. All you need is to download pack for your device, enable your driver and specify used peripherial in I2Cdev.h.
+
+## Devices
+Supported devices are MPU6050 without DMP and HMC5883. Actually I did not find any differences in registers between HMC5883 and HMC5983 therefore the latter might work as well.
+
+Porting other devices should be easy and any help is appreciated.
+
+## Licence
+I2Cdev device library code is placed under the MIT license.
+
+_Copyright (c) 2011 Jeff Rowberg. Copyright (c) 2017 Kamnev Yuriy._
diff --git a/STM32HAL/APDS9960/APDS9960.c b/STM32HAL/APDS9960/APDS9960.c
new file mode 100644
index 00000000..f1fe86dc
--- /dev/null
+++ b/STM32HAL/APDS9960/APDS9960.c
@@ -0,0 +1,2154 @@
+/**
+ * @author      : shmakins (shmakins@gmail.com)
+ * @file        : APDS9960
+ * @created     : Thursday Oct 12, 2017 23:44:51 EEST
+ */
+
+#include "APDS9960.h"
+
+/* Gesture processing */
+void resetGestureParameters();
+bool processGestureData();
+bool decodeGesture();
+
+/* Proximity Interrupt Threshold */
+uint8_t getProxIntLowThresh();
+bool setProxIntLowThresh(uint8_t threshold);
+uint8_t getProxIntHighThresh();
+bool setProxIntHighThresh(uint8_t threshold);
+
+/* LED Boost Control */
+uint8_t getLEDBoost();
+bool setLEDBoost(uint8_t boost);
+
+/* Proximity photodiode select */
+uint8_t getProxGainCompEnable();
+bool setProxGainCompEnable(uint8_t enable);
+uint8_t getProxPhotoMask();
+bool setProxPhotoMask(uint8_t mask);
+
+/* Gesture threshold control */
+uint8_t getGestureEnterThresh();
+bool setGestureEnterThresh(uint8_t threshold);
+uint8_t getGestureExitThresh();
+bool setGestureExitThresh(uint8_t threshold);
+
+/* Gesture LED, gain, and time control */
+uint8_t getGestureWaitTime();
+bool setGestureWaitTime(uint8_t time);
+
+/* Gesture mode */
+uint8_t getGestureMode();
+bool setGestureMode(uint8_t mode);
+
+/* Raw I2C Commands */
+bool wireWriteByte(uint8_t val);
+bool wireWriteDataByte(uint8_t reg, uint8_t val);
+bool wireWriteDataBlock(uint8_t reg, uint8_t *val, unsigned int len);
+bool wireReadDataByte(uint8_t reg, uint8_t *val);
+int wireReadDataBlock(uint8_t reg, uint8_t *val, unsigned int len);
+
+/* Members */
+gesture_data_type gesture_data_;
+int gesture_ud_delta_;
+int gesture_lr_delta_;
+int gesture_ud_count_;
+int gesture_lr_count_;
+int gesture_near_count_;
+int gesture_far_count_;
+int gesture_state_;
+int gesture_motion_;
+
+/*
+SparkFun_APDS9960::SparkFun_APDS9960()
+{
+    gesture_ud_delta_ = 0;
+    gesture_lr_delta_ = 0;
+
+    gesture_ud_count_ = 0;
+    gesture_lr_count_ = 0;
+
+    gesture_near_count_ = 0;
+    gesture_far_count_ = 0;
+
+    gesture_state_ = 0;
+    gesture_motion_ = DIR_NONE;
+}
+*/
+
+
+/**
+ * @brief Configures I2C communications and initializes registers to defaults
+ *
+ * @return True if initialized successfully. False otherwise.
+ */
+bool APDS9960_init(I2C_HandleTypeDef *i2c_h)
+{
+    uint8_t id;
+
+    /* Initialize I2C */
+    I2Cdev_init(i2c_h);
+    //Wire.begin();
+
+    /* Read ID register and check against known values for APDS-9960 */
+    if( !wireReadDataByte(APDS9960_ID, &id) ) {
+        return false;
+    }
+    if( !(id == APDS9960_ID_1 || id == APDS9960_ID_2 || id == APDS9960_ID_3) ) {
+        return false;
+    }
+
+    /* Set ENABLE register to 0 (disable all features) */
+    if( !APDS9960_setMode(ALL, OFF) ) {
+        return false;
+    }
+
+    /* Set default values for ambient light and proximity registers */
+    if( !wireWriteDataByte(APDS9960_ATIME, DEFAULT_ATIME) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_WTIME, DEFAULT_WTIME) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_PPULSE, DEFAULT_PROX_PPULSE) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_POFFSET_UR, DEFAULT_POFFSET_UR) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_POFFSET_DL, DEFAULT_POFFSET_DL) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_CONFIG1, DEFAULT_CONFIG1) ) {
+        return false;
+    }
+    if( !APDS9960_setLEDDrive(DEFAULT_LDRIVE) ) {
+        return false;
+    }
+    if( !APDS9960_setProximityGain(DEFAULT_PGAIN) ) {
+        return false;
+    }
+    if( !APDS9960_setAmbientLightGain(DEFAULT_AGAIN) ) {
+        return false;
+    }
+    if( !setProxIntLowThresh(DEFAULT_PILT) ) {
+        return false;
+    }
+    if( !setProxIntHighThresh(DEFAULT_PIHT) ) {
+        return false;
+    }
+    if( !APDS9960_setLightIntLowThreshold(DEFAULT_AILT) ) {
+        return false;
+    }
+    if( !APDS9960_setLightIntHighThreshold(DEFAULT_AIHT) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_PERS, DEFAULT_PERS) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_CONFIG2, DEFAULT_CONFIG2) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_CONFIG3, DEFAULT_CONFIG3) ) {
+        return false;
+    }
+
+    /* Set default values for gesture sense registers */
+    if( !setGestureEnterThresh(DEFAULT_GPENTH) ) {
+        return false;
+    }
+    if( !setGestureExitThresh(DEFAULT_GEXTH) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GCONF1, DEFAULT_GCONF1) ) {
+        return false;
+    }
+    if( !APDS9960_setGestureGain(DEFAULT_GGAIN) ) {
+        return false;
+    }
+    if( !APDS9960_setGestureLEDDrive(DEFAULT_GLDRIVE) ) {
+        return false;
+    }
+    if( !setGestureWaitTime(DEFAULT_GWTIME) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GOFFSET_U, DEFAULT_GOFFSET) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GOFFSET_D, DEFAULT_GOFFSET) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GOFFSET_L, DEFAULT_GOFFSET) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GOFFSET_R, DEFAULT_GOFFSET) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GPULSE, DEFAULT_GPULSE) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_GCONF3, DEFAULT_GCONF3) ) {
+        return false;
+    }
+    if( !APDS9960_setGestureIntEnable(DEFAULT_GIEN) ) {
+        return false;
+    }
+
+#if 0
+    /* Gesture config register dump */
+    uint8_t reg;
+    uint8_t val;
+
+    for(reg = 0x80; reg <= 0xAF; reg++) {
+        if( (reg != 0x82) && \
+            (reg != 0x8A) && \
+            (reg != 0x91) && \
+            (reg != 0xA8) && \
+            (reg != 0xAC) && \
+            (reg != 0xAD) )
+        {
+            wireReadDataByte(reg, &val);
+            LCD_Printf("%x: 0x%x\n", reg, val);
+        }
+    }
+
+    for(reg = 0xE4; reg <= 0xE7; reg++) {
+        wireReadDataByte(reg, &val);
+        LCD_Printf("%x: 0x%x\n", reg, val);
+    }
+#endif
+
+    return true;
+}
+
+/*******************************************************************************
+ * Public methods for controlling the APDS-9960
+ ******************************************************************************/
+
+/**
+ * @brief Reads and returns the contents of the ENABLE register
+ *
+ * @return Contents of the ENABLE register. 0xFF if error.
+ */
+uint8_t APDS9960_getMode()
+{
+    uint8_t enable_value;
+
+    /* Read current ENABLE register */
+    if( !wireReadDataByte(APDS9960_ENABLE, &enable_value) ) {
+        return ERROR;
+    }
+
+    return enable_value;
+}
+
+/**
+ * @brief Enables or disables a feature in the APDS-9960
+ *
+ * @param[in] mode which feature to enable
+ * @param[in] enable ON (1) or OFF (0)
+ * @return True if operation success. False otherwise.
+ */
+bool APDS9960_setMode(uint8_t mode, uint8_t enable)
+{
+    uint8_t reg_val;
+
+    /* Read current ENABLE register */
+    reg_val = APDS9960_getMode();
+    if( reg_val == ERROR ) {
+        return false;
+    }
+
+    /* Change bit(s) in ENABLE register */
+    enable = enable & 0x01;
+    if( mode >= 0 && mode <= 6 ) {
+        if (enable) {
+            reg_val |= (1 << mode);
+        } else {
+            reg_val &= ~(1 << mode);
+        }
+    } else if( mode == ALL ) {
+        if (enable) {
+            reg_val = 0x7F;
+        } else {
+            reg_val = 0x00;
+        }
+    }
+
+    /* Write value back to ENABLE register */
+    if( !wireWriteDataByte(APDS9960_ENABLE, reg_val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Starts the light (R/G/B/Ambient) sensor on the APDS-9960
+ *
+ * @param[in] interrupts true to enable hardware interrupt on high or low light
+ * @return True if sensor enabled correctly. False on error.
+ */
+bool APDS9960_enableLightSensor()
+{
+
+    /* Set default gain, interrupts, enable power, and enable sensor */
+    if( !APDS9960_setAmbientLightGain(DEFAULT_AGAIN) ) {
+        return false;
+    }
+    if( !APDS9960_setAmbientLightIntEnable(1) ) {
+        return false;
+    }
+    if( !APDS9960_enablePower() ){
+        return false;
+    }
+    if( !APDS9960_setMode(AMBIENT_LIGHT, 1) ) {
+        return false;
+    }
+
+    return true;
+
+}
+
+/**
+ * @brief Ends the light sensor on the APDS-9960
+ *
+ * @return True if sensor disabled correctly. False on error.
+ */
+bool APDS9960_disableLightSensor()
+{
+    if( !APDS9960_setAmbientLightIntEnable(0) ) {
+        return false;
+    }
+    if( !APDS9960_setMode(AMBIENT_LIGHT, 0) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Starts the proximity sensor on the APDS-9960
+ *
+ * @param[in] interrupts true to enable hardware external interrupt on proximity
+ * @return True if sensor enabled correctly. False on error.
+ */
+bool APDS9960_enableProximitySensor()
+{
+    /* Set default gain, LED, interrupts, enable power, and enable sensor */
+    if( !APDS9960_setProximityGain(DEFAULT_PGAIN) ) {
+        return false;
+    }
+    if( !APDS9960_setLEDDrive(DEFAULT_LDRIVE) ) {
+        return false;
+    }
+    if( !APDS9960_setProximityIntEnable(1) ) {
+        return false;
+    }
+    if( !APDS9960_enablePower() ){
+        return false;
+    }
+    if( !APDS9960_setMode(PROXIMITY, 1) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Ends the proximity sensor on the APDS-9960
+ *
+ * @return True if sensor disabled correctly. False on error.
+ */
+bool APDS9960_disableProximitySensor()
+{
+	if( !APDS9960_setProximityIntEnable(0) ) {
+		return false;
+	}
+	if( !APDS9960_setMode(PROXIMITY, 0) ) {
+		return false;
+	}
+
+	return true;
+}
+
+/**
+ * @brief Starts the gesture recognition engine on the APDS-9960
+ *
+ * @param[in] interrupts true to enable hardware external interrupt on gesture
+ * @return True if engine enabled correctly. False on error.
+ */
+bool APDS9960_enableGestureSensor()
+{
+
+    /* Enable gesture mode
+       Set ENABLE to 0 (power off)
+       Set WTIME to 0xFF
+       Set AUX to LED_BOOST_300
+       Enable PON, WEN, PEN, GEN in ENABLE
+    */
+    resetGestureParameters();
+    if( !wireWriteDataByte(APDS9960_WTIME, 0xFF) ) {
+        return false;
+    }
+    if( !wireWriteDataByte(APDS9960_PPULSE, DEFAULT_GESTURE_PPULSE) ) {
+        return false;
+    }
+    if( !setLEDBoost(LED_BOOST_300) ) {
+        return false;
+    }
+    if( !APDS9960_setGestureIntEnable(1) ) {
+        return false;
+    }
+    if( !setGestureMode(1) ) {
+        return false;
+    }
+    if( !APDS9960_enablePower() ){
+        return false;
+    }
+    if( !APDS9960_setMode(WAIT, 1) ) {
+        return false;
+    }
+    if( !APDS9960_setMode(PROXIMITY, 1) ) {
+        return false;
+    }
+    if( !APDS9960_setMode(GESTURE, 1) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Ends the gesture recognition engine on the APDS-9960
+ *
+ * @return True if engine disabled correctly. False on error.
+ */
+bool APDS9960_disableGestureSensor()
+{
+    resetGestureParameters();
+    if( !APDS9960_setGestureIntEnable(0) ) {
+        return false;
+    }
+    if( !setGestureMode(0) ) {
+        return false;
+    }
+    if( !APDS9960_setMode(GESTURE, 0) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Determines if there is a gesture available for reading
+ *
+ * @return True if gesture available. False otherwise.
+ */
+bool APDS9960_isGestureAvailable()
+{
+    uint8_t val;
+
+    /* Read value from GSTATUS register */
+    if( !wireReadDataByte(APDS9960_GSTATUS, &val) ) {
+        // ERROR
+        return false;
+    }
+
+    /* Shift and mask out GVALID bit */
+    val &= APDS9960_GVALID;
+
+    /* Return true/false based on GVALID bit */
+    if( val == 1) {
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/**
+ * @brief Processes a gesture event and returns best guessed gesture
+ *
+ * @return Number corresponding to gesture. -1 on error.
+ */
+int APDS9960_readGesture(uint32_t timeout)
+{
+    uint8_t fifo_level = 0;
+    uint8_t bytes_read = 0;
+    uint8_t fifo_data[128];
+    uint8_t gstatus;
+    int motion;
+    int i;
+
+    /* Make sure that power and gesture is on and data is valid */
+    if( !APDS9960_isGestureAvailable() || !(APDS9960_getMode() & 0b01000001) ) {
+        return DIR_NONE;
+    }
+
+    /* Keep looping as long as gesture data is valid */
+    uint32_t time = HAL_GetTick();
+    while( !timeout ||  HAL_GetTick() < time + timeout ) {
+
+        /* Wait some time to collect next batch of FIFO data */
+        HAL_Delay(FIFO_PAUSE_TIME);
+
+        /* Get the contents of the STATUS register. Is data still valid? */
+        if( !wireReadDataByte(APDS9960_GSTATUS, &gstatus) ) {
+            return ERROR;
+        }
+
+        /* If we have valid data, read in FIFO */
+        if( (gstatus & APDS9960_GVALID) == APDS9960_GVALID ) {
+
+            /* Read the current FIFO level */
+            if( !wireReadDataByte(APDS9960_GFLVL, &fifo_level) ) {
+                return ERROR;
+            }
+
+#if DEBUG
+            LCD_Printf("FIFO level: %d\n", fifo_level);
+#endif
+
+            /* If there's stuff in the FIFO, read it into our data block */
+            if( fifo_level > 0) {
+                bytes_read = wireReadDataBlock(  APDS9960_GFIFO_U,
+                                                (uint8_t*)fifo_data,
+                                                (fifo_level * 4) );
+                if( bytes_read == -1 ) {
+                    return ERROR;
+                }
+#if DEBUG
+                LCD_Printf("FIFO Dump: ");
+                for ( i = 0; i < bytes_read; i++ ) {
+                    LCD_Printf("%d ",fifo_data[i]);
+                }
+                LCD_Printf("\n");
+#endif
+
+                /* If at least 1 set of data, sort the data into U/D/L/R */
+                if( bytes_read >= 4 ) {
+                    for( i = 0; i < bytes_read; i += 4 ) {
+                        gesture_data_.u_data[gesture_data_.index] = \
+                                                            fifo_data[i + 0];
+                        gesture_data_.d_data[gesture_data_.index] = \
+                                                            fifo_data[i + 1];
+                        gesture_data_.l_data[gesture_data_.index] = \
+                                                            fifo_data[i + 2];
+                        gesture_data_.r_data[gesture_data_.index] = \
+                                                            fifo_data[i + 3];
+                        gesture_data_.index++;
+                        gesture_data_.total_gestures++;
+                    }
+
+#if DEBUG
+                LCD_Printf("Up Data: ");
+                for ( i = 0; i < gesture_data_.total_gestures; i++ ) {
+                    LCD_Printf("%d ", gesture_data_.u_data[i]);
+                }
+                LCD_Printf("\n");
+#endif
+
+                    /* Filter and process gesture data. Decode near/far state */
+                    if( processGestureData() ) {
+                        if( decodeGesture() ) {
+                            //***TODO: U-Turn Gestures
+#if DEBUG
+                            //LCD_Printf("%d", gesture_motion_);
+#endif
+                        }
+                    }
+
+                    /* Reset data */
+                    gesture_data_.index = 0;
+                    gesture_data_.total_gestures = 0;
+                }
+            }
+        } else {
+
+            /* Determine best guessed gesture and clean up */
+            HAL_Delay(FIFO_PAUSE_TIME);
+            decodeGesture();
+            motion = gesture_motion_;
+#if DEBUG
+            LCD_Printf("END: %d\n", gesture_motion_);
+#endif
+            resetGestureParameters();
+            return motion;
+        }
+    }
+    return ERROR;
+}
+
+/**
+ * Turn the APDS-9960 on
+ *
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_enablePower()
+{
+    if( !APDS9960_setMode(POWER, 1) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * Turn the APDS-9960 off
+ *
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_disablePower()
+{
+    if( !APDS9960_setMode(POWER, 0) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/*******************************************************************************
+ * Ambient light and color sensor controls
+ ******************************************************************************/
+
+/**
+ * @brief Reads the ambient (clear) light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_readAmbientLight(uint16_t *val)
+{
+    uint8_t val_byte;
+    *val = 0;
+
+    /* Read value from clear channel, low byte register */
+    if( !wireReadDataByte(APDS9960_CDATAL, &val_byte) ) {
+        return false;
+    }
+    *val = val_byte;
+
+    /* Read value from clear channel, high byte register */
+    if( !wireReadDataByte(APDS9960_CDATAH, &val_byte) ) {
+        return false;
+    }
+    *val = *val + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/**
+ * @brief Reads the red light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_readRedLight(uint16_t *val)
+{
+    uint8_t val_byte;
+    *val = 0;
+
+    /* Read value from clear channel, low byte register */
+    if( !wireReadDataByte(APDS9960_RDATAL, &val_byte) ) {
+        return false;
+    }
+    *val = val_byte;
+
+    /* Read value from clear channel, high byte register */
+    if( !wireReadDataByte(APDS9960_RDATAH, &val_byte) ) {
+        return false;
+    }
+    *val = *val + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/**
+ * @brief Reads the green light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_readGreenLight(uint16_t *val)
+{
+    uint8_t val_byte;
+    *val = 0;
+
+    /* Read value from clear channel, low byte register */
+    if( !wireReadDataByte(APDS9960_GDATAL, &val_byte) ) {
+        return false;
+    }
+    *val = val_byte;
+
+    /* Read value from clear channel, high byte register */
+    if( !wireReadDataByte(APDS9960_GDATAH, &val_byte) ) {
+        return false;
+    }
+    *val = *val + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/**
+ * @brief Reads the red light level as a 16-bit value
+ *
+ * @param[out] val value of the light sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_readBlueLight(uint16_t *val)
+{
+    uint8_t val_byte;
+    *val = 0;
+
+    /* Read value from clear channel, low byte register */
+    if( !wireReadDataByte(APDS9960_BDATAL, &val_byte) ) {
+        return false;
+    }
+    *val = val_byte;
+
+    /* Read value from clear channel, high byte register */
+    if( !wireReadDataByte(APDS9960_BDATAH, &val_byte) ) {
+        return false;
+    }
+    *val = *val + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/*******************************************************************************
+ * Proximity sensor controls
+ ******************************************************************************/
+
+/**
+ * @brief Reads the proximity level as an 8-bit value
+ *
+ * @param[out] val value of the proximity sensor.
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_readProximity(uint8_t *val)
+{
+    uint8_t val_byte;
+    *val = 0;
+
+    /* Read value from proximity data register */
+    if( !wireReadDataByte(APDS9960_PDATA, &val_byte) ) {
+        return false;
+    }
+    *val = val_byte;
+    return true;
+}
+
+/*******************************************************************************
+ * High-level gesture controls
+ ******************************************************************************/
+
+/**
+ * @brief Resets all the parameters in the gesture data member
+ */
+void resetGestureParameters()
+{
+    gesture_data_.index = 0;
+    gesture_data_.total_gestures = 0;
+
+    gesture_ud_delta_ = 0;
+    gesture_lr_delta_ = 0;
+
+    gesture_ud_count_ = 0;
+    gesture_lr_count_ = 0;
+
+    gesture_near_count_ = 0;
+    gesture_far_count_ = 0;
+
+    gesture_state_ = 0;
+    gesture_motion_ = DIR_NONE;
+}
+
+/**
+ * @brief Processes the raw gesture data to determine swipe direction
+ *
+ * @return True if near or far state seen. False otherwise.
+ */
+bool processGestureData()
+{
+    uint8_t u_first = 0;
+    uint8_t d_first = 0;
+    uint8_t l_first = 0;
+    uint8_t r_first = 0;
+    uint8_t u_last = 0;
+    uint8_t d_last = 0;
+    uint8_t l_last = 0;
+    uint8_t r_last = 0;
+    int ud_ratio_first;
+    int lr_ratio_first;
+    int ud_ratio_last;
+    int lr_ratio_last;
+    int ud_delta;
+    int lr_delta;
+    int i;
+
+    /* If we have less than 4 total gestures, that's not enough */
+    if( gesture_data_.total_gestures <= 4 ) {
+        return false;
+    }
+
+    /* Check to make sure our data isn't out of bounds */
+    if( (gesture_data_.total_gestures <= 32) && \
+        (gesture_data_.total_gestures > 0) ) {
+
+        /* Find the first value in U/D/L/R above the threshold */
+        for( i = 0; i < gesture_data_.total_gestures; i++ ) {
+            if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
+
+                u_first = gesture_data_.u_data[i];
+                d_first = gesture_data_.d_data[i];
+                l_first = gesture_data_.l_data[i];
+                r_first = gesture_data_.r_data[i];
+                break;
+            }
+        }
+
+        /* If one of the _first values is 0, then there is no good data */
+        if( (u_first == 0) || (d_first == 0) || \
+            (l_first == 0) || (r_first == 0) ) {
+
+            return false;
+        }
+        /* Find the last value in U/D/L/R above the threshold */
+        for( i = gesture_data_.total_gestures - 1; i >= 0; i-- ) {
+#if DEBUG
+            LCD_Printf("Finding last: ");
+            LCD_Printf("U: %f ", gesture_data_.u_data[i]);
+            LCD_Printf("D: %f ", gesture_data_.d_data[i]);
+            LCD_Printf("L: %f ", gesture_data_.l_data[i]);
+            LCD_Printf("R: %f\n", gesture_data_.r_data[i]);
+#endif
+            if( (gesture_data_.u_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.d_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.l_data[i] > GESTURE_THRESHOLD_OUT) &&
+                (gesture_data_.r_data[i] > GESTURE_THRESHOLD_OUT) ) {
+
+                u_last = gesture_data_.u_data[i];
+                d_last = gesture_data_.d_data[i];
+                l_last = gesture_data_.l_data[i];
+                r_last = gesture_data_.r_data[i];
+                break;
+            }
+        }
+    }
+
+    /* Calculate the first vs. last ratio of up/down and left/right */
+    ud_ratio_first = ((u_first - d_first) * 100) / (u_first + d_first);
+    lr_ratio_first = ((l_first - r_first) * 100) / (l_first + r_first);
+    ud_ratio_last = ((u_last - d_last) * 100) / (u_last + d_last);
+    lr_ratio_last = ((l_last - r_last) * 100) / (l_last + r_last);
+
+#if DEBUG
+    LCD_Printf("Last Values: ");
+    LCD_Printf("U: %d ", u_last);
+    LCD_Printf("D: %d ", d_last);
+    LCD_Printf("L: %d ", l_last);
+    LCD_Printf("R: %d\n", r_last);
+
+    LCD_Printf("Ratios: ");
+    LCD_Printf("UD Fi: %d ", ud_ratio_first);
+    LCD_Printf("UD La: %d ", ud_ratio_last);
+    LCD_Printf("LR Fi: %d ", lr_ratio_first);
+    LCD_Printf("LR La: %d\n", lr_ratio_last);
+#endif
+
+    /* Determine the difference between the first and last ratios */
+    ud_delta = ud_ratio_last - ud_ratio_first;
+    lr_delta = lr_ratio_last - lr_ratio_first;
+
+#if DEBUG
+    LCD_Printf("Deltas: ");
+    LCD_Printf("UD: %d", ud_delta);
+    LCD_Printf("LR: %d", lr_delta);
+#endif
+
+    /* Accumulate the UD and LR delta values */
+    gesture_ud_delta_ += ud_delta;
+    gesture_lr_delta_ += lr_delta;
+
+#if DEBUG
+    LCD_Printf("Accumulations: ");
+    LCD_Printf("UD: %d", gesture_ud_delta_);
+    LCD_Printf("LR: %d", gesture_lr_delta_);
+#endif
+
+    /* Determine U/D gesture */
+    if( gesture_ud_delta_ >= GESTURE_SENSITIVITY_1 ) {
+        gesture_ud_count_ = 1;
+    } else if( gesture_ud_delta_ <= -GESTURE_SENSITIVITY_1 ) {
+        gesture_ud_count_ = -1;
+    } else {
+        gesture_ud_count_ = 0;
+    }
+
+    /* Determine L/R gesture */
+    if( gesture_lr_delta_ >= GESTURE_SENSITIVITY_1 ) {
+        gesture_lr_count_ = 1;
+    } else if( gesture_lr_delta_ <= -GESTURE_SENSITIVITY_1 ) {
+        gesture_lr_count_ = -1;
+    } else {
+        gesture_lr_count_ = 0;
+    }
+
+    /* Determine Near/Far gesture */
+    if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 0) ) {
+        if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
+            (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
+
+            if( (ud_delta == 0) && (lr_delta == 0) ) {
+                gesture_near_count_++;
+            } else if( (ud_delta != 0) || (lr_delta != 0) ) {
+                gesture_far_count_++;
+            }
+
+            if( (gesture_near_count_ >= 10) && (gesture_far_count_ >= 2) ) {
+                if( (ud_delta == 0) && (lr_delta == 0) ) {
+                    gesture_state_ = NEAR_STATE;
+                } else if( (ud_delta != 0) && (lr_delta != 0) ) {
+                    gesture_state_ = FAR_STATE;
+                }
+                return true;
+            }
+        }
+    } else {
+        if( (abs(ud_delta) < GESTURE_SENSITIVITY_2) && \
+            (abs(lr_delta) < GESTURE_SENSITIVITY_2) ) {
+
+            if( (ud_delta == 0) && (lr_delta == 0) ) {
+                gesture_near_count_++;
+            }
+
+            if( gesture_near_count_ >= 10 ) {
+                gesture_ud_count_ = 0;
+                gesture_lr_count_ = 0;
+                gesture_ud_delta_ = 0;
+                gesture_lr_delta_ = 0;
+            }
+        }
+    }
+
+#if DEBUG
+    LCD_Printf("UD_CT: %d", gesture_ud_count_);
+    LCD_Printf("LR_CT: %d", gesture_lr_count_);
+    LCD_Printf("NEAR_CT: %d", gesture_near_count_);
+    LCD_Printf("FAR_CT: %d", gesture_far_count_);
+    LCD_Printf("----------\n");
+#endif
+
+    return false;
+}
+
+/**
+ * @brief Determines swipe direction or near/far state
+ *
+ * @return True if near/far event. False otherwise.
+ */
+bool decodeGesture()
+{
+    /* Return if near or far event is detected */
+    if( gesture_state_ == NEAR_STATE ) {
+        gesture_motion_ = DIR_NEAR;
+        return true;
+    } else if ( gesture_state_ == FAR_STATE ) {
+        gesture_motion_ = DIR_FAR;
+        return true;
+    }
+
+    /* Determine swipe direction */
+    if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 0) ) {
+        gesture_motion_ = DIR_UP;
+    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 0) ) {
+        gesture_motion_ = DIR_DOWN;
+    } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == 1) ) {
+        gesture_motion_ = DIR_RIGHT;
+    } else if( (gesture_ud_count_ == 0) && (gesture_lr_count_ == -1) ) {
+        gesture_motion_ = DIR_LEFT;
+    } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == 1) ) {
+        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+            gesture_motion_ = DIR_UP;
+        } else {
+            gesture_motion_ = DIR_RIGHT;
+        }
+    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == -1) ) {
+        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+            gesture_motion_ = DIR_DOWN;
+        } else {
+            gesture_motion_ = DIR_LEFT;
+        }
+    } else if( (gesture_ud_count_ == -1) && (gesture_lr_count_ == -1) ) {
+        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+            gesture_motion_ = DIR_UP;
+        } else {
+            gesture_motion_ = DIR_LEFT;
+        }
+    } else if( (gesture_ud_count_ == 1) && (gesture_lr_count_ == 1) ) {
+        if( abs(gesture_ud_delta_) > abs(gesture_lr_delta_) ) {
+            gesture_motion_ = DIR_DOWN;
+        } else {
+            gesture_motion_ = DIR_RIGHT;
+        }
+    } else {
+        return false;
+    }
+
+    return true;
+}
+
+/*******************************************************************************
+ * Getters and setters for register values
+ ******************************************************************************/
+
+/**
+ * @brief Returns the lower threshold for proximity detection
+ *
+ * @return lower threshold
+ */
+uint8_t getProxIntLowThresh()
+{
+    uint8_t val;
+
+    /* Read value from PILT register */
+    if( !wireReadDataByte(APDS9960_PILT, &val) ) {
+        val = 0;
+    }
+
+    return val;
+}
+
+/**
+ * @brief Sets the lower threshold for proximity detection
+ *
+ * @param[in] threshold the lower proximity threshold
+ * @return True if operation successful. False otherwise.
+ */
+bool setProxIntLowThresh(uint8_t threshold)
+{
+    if( !wireWriteDataByte(APDS9960_PILT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Returns the high threshold for proximity detection
+ *
+ * @return high threshold
+ */
+uint8_t getProxIntHighThresh()
+{
+    uint8_t val;
+
+    /* Read value from PIHT register */
+    if( !wireReadDataByte(APDS9960_PIHT, &val) ) {
+        val = 0;
+    }
+
+    return val;
+}
+
+/**
+ * @brief Sets the high threshold for proximity detection
+ *
+ * @param[in] threshold the high proximity threshold
+ * @return True if operation successful. False otherwise.
+ */
+bool setProxIntHighThresh(uint8_t threshold)
+{
+    if( !wireWriteDataByte(APDS9960_PIHT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Returns LED drive strength for proximity and ALS
+ *
+ * Value    LED Current
+ *   0        100 mA
+ *   1         50 mA
+ *   2         25 mA
+ *   3         12.5 mA
+ *
+ * @return the value of the LED drive strength. 0xFF on failure.
+ */
+uint8_t APDS9960_getLEDDrive()
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out LED drive bits */
+    val = (val >> 6) & 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the LED drive strength for proximity and ALS
+ *
+ * Value    LED Current
+ *   0        100 mA
+ *   1         50 mA
+ *   2         25 mA
+ *   3         12.5 mA
+ *
+ * @param[in] drive the value (0-3) for the LED drive strength
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setLEDDrive(uint8_t drive)
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    drive &= 0b00000011;
+    drive = drive << 6;
+    val &= 0b00111111;
+    val |= drive;
+
+    /* Write register value back into CONTROL register */
+    if( !wireWriteDataByte(APDS9960_CONTROL, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Returns receiver gain for proximity detection
+ *
+ * Value    Gain
+ *   0       1x
+ *   1       2x
+ *   2       4x
+ *   3       8x
+ *
+ * @return the value of the proximity gain. 0xFF on failure.
+ */
+uint8_t APDS9960_getProximityGain()
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out PDRIVE bits */
+    val = (val >> 2) & 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the receiver gain for proximity detection
+ *
+ * Value    Gain
+ *   0       1x
+ *   1       2x
+ *   2       4x
+ *   3       8x
+ *
+ * @param[in] drive the value (0-3) for the gain
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setProximityGain(uint8_t drive)
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    drive &= 0b00000011;
+    drive = drive << 2;
+    val &= 0b11110011;
+    val |= drive;
+
+    /* Write register value back into CONTROL register */
+    if( !wireWriteDataByte(APDS9960_CONTROL, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Returns receiver gain for the ambient light sensor (ALS)
+ *
+ * Value    Gain
+ *   0        1x
+ *   1        4x
+ *   2       16x
+ *   3       64x
+ *
+ * @return the value of the ALS gain. 0xFF on failure.
+ */
+uint8_t APDS9960_getAmbientLightGain()
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out ADRIVE bits */
+    val &= 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the receiver gain for the ambient light sensor (ALS)
+ *
+ * Value    Gain
+ *   0        1x
+ *   1        4x
+ *   2       16x
+ *   3       64x
+ *
+ * @param[in] drive the value (0-3) for the gain
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setAmbientLightGain(uint8_t drive)
+{
+    uint8_t val;
+
+    /* Read value from CONTROL register */
+    if( !wireReadDataByte(APDS9960_CONTROL, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    drive &= 0b00000011;
+    val &= 0b11111100;
+    val |= drive;
+
+    /* Write register value back into CONTROL register */
+    if( !wireWriteDataByte(APDS9960_CONTROL, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Get the current LED boost value
+ *
+ * Value  Boost Current
+ *   0        100%
+ *   1        150%
+ *   2        200%
+ *   3        300%
+ *
+ * @return The LED boost value. 0xFF on failure.
+ */
+uint8_t getLEDBoost()
+{
+    uint8_t val;
+
+    /* Read value from CONFIG2 register */
+    if( !wireReadDataByte(APDS9960_CONFIG2, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out LED_BOOST bits */
+    val = (val >> 4) & 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the LED current boost value
+ *
+ * Value  Boost Current
+ *   0        100%
+ *   1        150%
+ *   2        200%
+ *   3        300%
+ *
+ * @param[in] drive the value (0-3) for current boost (100-300%)
+ * @return True if operation successful. False otherwise.
+ */
+bool setLEDBoost(uint8_t boost)
+{
+    uint8_t val;
+
+    /* Read value from CONFIG2 register */
+    if( !wireReadDataByte(APDS9960_CONFIG2, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    boost &= 0b00000011;
+    boost = boost << 4;
+    val &= 0b11001111;
+    val |= boost;
+
+    /* Write register value back into CONFIG2 register */
+    if( !wireWriteDataByte(APDS9960_CONFIG2, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets proximity gain compensation enable
+ *
+ * @return 1 if compensation is enabled. 0 if not. 0xFF on error.
+ */
+uint8_t getProxGainCompEnable()
+{
+    uint8_t val;
+
+    /* Read value from CONFIG3 register */
+    if( !wireReadDataByte(APDS9960_CONFIG3, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out PCMP bits */
+    val = (val >> 5) & 0b00000001;
+
+    return val;
+}
+
+/**
+ * @brief Sets the proximity gain compensation enable
+ *
+ * @param[in] enable 1 to enable compensation. 0 to disable compensation.
+ * @return True if operation successful. False otherwise.
+ */
+ bool setProxGainCompEnable(uint8_t enable)
+{
+    uint8_t val;
+
+    /* Read value from CONFIG3 register */
+    if( !wireReadDataByte(APDS9960_CONFIG3, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    enable &= 0b00000001;
+    enable = enable << 5;
+    val &= 0b11011111;
+    val |= enable;
+
+    /* Write register value back into CONFIG3 register */
+    if( !wireWriteDataByte(APDS9960_CONFIG3, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the current mask for enabled/disabled proximity photodiodes
+ *
+ * 1 = disabled, 0 = enabled
+ * Bit    Photodiode
+ *  3       UP
+ *  2       DOWN
+ *  1       LEFT
+ *  0       RIGHT
+ *
+ * @return Current proximity mask for photodiodes. 0xFF on error.
+ */
+uint8_t getProxPhotoMask()
+{
+    uint8_t val;
+
+    /* Read value from CONFIG3 register */
+    if( !wireReadDataByte(APDS9960_CONFIG3, &val) ) {
+        return ERROR;
+    }
+
+    /* Mask out photodiode enable mask bits */
+    val &= 0b00001111;
+
+    return val;
+}
+
+/**
+ * @brief Sets the mask for enabling/disabling proximity photodiodes
+ *
+ * 1 = disabled, 0 = enabled
+ * Bit    Photodiode
+ *  3       UP
+ *  2       DOWN
+ *  1       LEFT
+ *  0       RIGHT
+ *
+ * @param[in] mask 4-bit mask value
+ * @return True if operation successful. False otherwise.
+ */
+bool setProxPhotoMask(uint8_t mask)
+{
+    uint8_t val;
+
+    /* Read value from CONFIG3 register */
+    if( !wireReadDataByte(APDS9960_CONFIG3, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    mask &= 0b00001111;
+    val &= 0b11110000;
+    val |= mask;
+
+    /* Write register value back into CONFIG3 register */
+    if( !wireWriteDataByte(APDS9960_CONFIG3, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the entry proximity threshold for gesture sensing
+ *
+ * @return Current entry proximity threshold.
+ */
+uint8_t getGestureEnterThresh()
+{
+    uint8_t val;
+
+    /* Read value from GPENTH register */
+    if( !wireReadDataByte(APDS9960_GPENTH, &val) ) {
+        val = 0;
+    }
+
+    return val;
+}
+
+/**
+ * @brief Sets the entry proximity threshold for gesture sensing
+ *
+ * @param[in] threshold proximity value needed to start gesture mode
+ * @return True if operation successful. False otherwise.
+ */
+bool setGestureEnterThresh(uint8_t threshold)
+{
+    if( !wireWriteDataByte(APDS9960_GPENTH, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the exit proximity threshold for gesture sensing
+ *
+ * @return Current exit proximity threshold.
+ */
+uint8_t getGestureExitThresh()
+{
+    uint8_t val;
+
+    /* Read value from GEXTH register */
+    if( !wireReadDataByte(APDS9960_GEXTH, &val) ) {
+        val = 0;
+    }
+
+    return val;
+}
+
+/**
+ * @brief Sets the exit proximity threshold for gesture sensing
+ *
+ * @param[in] threshold proximity value needed to end gesture mode
+ * @return True if operation successful. False otherwise.
+ */
+bool setGestureExitThresh(uint8_t threshold)
+{
+    if( !wireWriteDataByte(APDS9960_GEXTH, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the gain of the photodiode during gesture mode
+ *
+ * Value    Gain
+ *   0       1x
+ *   1       2x
+ *   2       4x
+ *   3       8x
+ *
+ * @return the current photodiode gain. 0xFF on error.
+ */
+uint8_t APDS9960_getGestureGain()
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out GGAIN bits */
+    val = (val >> 5) & 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the gain of the photodiode during gesture mode
+ *
+ * Value    Gain
+ *   0       1x
+ *   1       2x
+ *   2       4x
+ *   3       8x
+ *
+ * @param[in] gain the value for the photodiode gain
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setGestureGain(uint8_t gain)
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    gain &= 0b00000011;
+    gain = gain << 5;
+    val &= 0b10011111;
+    val |= gain;
+
+    /* Write register value back into GCONF2 register */
+    if( !wireWriteDataByte(APDS9960_GCONF2, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the drive current of the LED during gesture mode
+ *
+ * Value    LED Current
+ *   0        100 mA
+ *   1         50 mA
+ *   2         25 mA
+ *   3         12.5 mA
+ *
+ * @return the LED drive current value. 0xFF on error.
+ */
+uint8_t APDS9960_getGestureLEDDrive()
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out GLDRIVE bits */
+    val = (val >> 3) & 0b00000011;
+
+    return val;
+}
+
+/**
+ * @brief Sets the LED drive current during gesture mode
+ *
+ * Value    LED Current
+ *   0        100 mA
+ *   1         50 mA
+ *   2         25 mA
+ *   3         12.5 mA
+ *
+ * @param[in] drive the value for the LED drive current
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setGestureLEDDrive(uint8_t drive)
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    drive &= 0b00000011;
+    drive = drive << 3;
+    val &= 0b11100111;
+    val |= drive;
+
+    /* Write register value back into GCONF2 register */
+    if( !wireWriteDataByte(APDS9960_GCONF2, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the time in low power mode between gesture detections
+ *
+ * Value    Wait time
+ *   0          0 ms
+ *   1          2.8 ms
+ *   2          5.6 ms
+ *   3          8.4 ms
+ *   4         14.0 ms
+ *   5         22.4 ms
+ *   6         30.8 ms
+ *   7         39.2 ms
+ *
+ * @return the current wait time between gestures. 0xFF on error.
+ */
+uint8_t getGestureWaitTime()
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return ERROR;
+    }
+
+    /* Mask out GWTIME bits */
+    val &= 0b00000111;
+
+    return val;
+}
+
+/**
+ * @brief Sets the time in low power mode between gesture detections
+ *
+ * Value    Wait time
+ *   0          0 ms
+ *   1          2.8 ms
+ *   2          5.6 ms
+ *   3          8.4 ms
+ *   4         14.0 ms
+ *   5         22.4 ms
+ *   6         30.8 ms
+ *   7         39.2 ms
+ *
+ * @param[in] the value for the wait time
+ * @return True if operation successful. False otherwise.
+ */
+bool setGestureWaitTime(uint8_t time)
+{
+    uint8_t val;
+
+    /* Read value from GCONF2 register */
+    if( !wireReadDataByte(APDS9960_GCONF2, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    time &= 0b00000111;
+    val &= 0b11111000;
+    val |= time;
+
+    /* Write register value back into GCONF2 register */
+    if( !wireWriteDataByte(APDS9960_GCONF2, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the low threshold for ambient light interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_getLightIntLowThreshold(uint16_t *threshold)
+{
+    uint8_t val_byte;
+    *threshold = 0;
+
+    /* Read value from ambient light low threshold, low byte register */
+    if( !wireReadDataByte(APDS9960_AILTL, &val_byte) ) {
+        return false;
+    }
+    *threshold = val_byte;
+
+    /* Read value from ambient light low threshold, high byte register */
+    if( !wireReadDataByte(APDS9960_AILTH, &val_byte) ) {
+        return false;
+    }
+    *threshold = *threshold + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/**
+ * @brief Sets the low threshold for ambient light interrupts
+ *
+ * @param[in] threshold low threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setLightIntLowThreshold(uint16_t threshold)
+{
+    uint8_t val_low;
+    uint8_t val_high;
+
+    /* Break 16-bit threshold into 2 8-bit values */
+    val_low = threshold & 0x00FF;
+    val_high = (threshold & 0xFF00) >> 8;
+
+    /* Write low byte */
+    if( !wireWriteDataByte(APDS9960_AILTL, val_low) ) {
+        return false;
+    }
+
+    /* Write high byte */
+    if( !wireWriteDataByte(APDS9960_AILTH, val_high) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the high threshold for ambient light interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_getLightIntHighThreshold(uint16_t *threshold)
+{
+    uint8_t val_byte;
+    *threshold = 0;
+
+    /* Read value from ambient light high threshold, low byte register */
+    if( !wireReadDataByte(APDS9960_AIHTL, &val_byte) ) {
+        return false;
+    }
+    *threshold = val_byte;
+
+    /* Read value from ambient light high threshold, high byte register */
+    if( !wireReadDataByte(APDS9960_AIHTH, &val_byte) ) {
+        return false;
+    }
+    *threshold = *threshold + ((uint16_t)val_byte << 8);
+
+    return true;
+}
+
+/**
+ * @brief Sets the high threshold for ambient light interrupts
+ *
+ * @param[in] threshold high threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setLightIntHighThreshold(uint16_t threshold)
+{
+    uint8_t val_low;
+    uint8_t val_high;
+
+    /* Break 16-bit threshold into 2 8-bit values */
+    val_low = threshold & 0x00FF;
+    val_high = (threshold & 0xFF00) >> 8;
+
+    /* Write low byte */
+    if( !wireWriteDataByte(APDS9960_AIHTL, val_low) ) {
+        return false;
+    }
+
+    /* Write high byte */
+    if( !wireWriteDataByte(APDS9960_AIHTH, val_high) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the low threshold for proximity interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_getProximityIntLowThreshold(uint8_t *threshold)
+{
+    threshold = 0;
+
+    /* Read value from proximity low threshold register */
+    if( !wireReadDataByte(APDS9960_PILT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Sets the low threshold for proximity interrupts
+ *
+ * @param[in] threshold low threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setProximityIntLowThreshold(uint8_t threshold)
+{
+
+    /* Write threshold value to register */
+    if( !wireWriteDataByte(APDS9960_PILT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets the high threshold for proximity interrupts
+ *
+ * @param[out] threshold current low threshold stored on the APDS-9960
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_getProximityIntHighThreshold(uint8_t *threshold)
+{
+    threshold = 0;
+
+    /* Read value from proximity low threshold register */
+    if( !wireReadDataByte(APDS9960_PIHT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Sets the high threshold for proximity interrupts
+ *
+ * @param[in] threshold high threshold value for interrupt to trigger
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setProximityIntHighThreshold(uint8_t threshold)
+{
+
+    /* Write threshold value to register */
+    if( !wireWriteDataByte(APDS9960_PIHT, threshold) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets if ambient light interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t APDS9960_getAmbientLightIntEnable()
+{
+    uint8_t val;
+
+    /* Read value from ENABLE register */
+    if( !wireReadDataByte(APDS9960_ENABLE, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out AIEN bit */
+    val = (val >> 4) & 0b00000001;
+
+    return val;
+}
+
+/**
+ * @brief Turns ambient light interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setAmbientLightIntEnable(uint8_t enable)
+{
+    uint8_t val;
+
+    /* Read value from ENABLE register */
+    if( !wireReadDataByte(APDS9960_ENABLE, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    enable &= 0b00000001;
+    enable = enable << 4;
+    val &= 0b11101111;
+    val |= enable;
+
+    /* Write register value back into ENABLE register */
+    if( !wireWriteDataByte(APDS9960_ENABLE, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets if proximity interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t APDS9960_getProximityIntEnable()
+{
+    uint8_t val;
+
+    /* Read value from ENABLE register */
+    if( !wireReadDataByte(APDS9960_ENABLE, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out PIEN bit */
+    val = (val >> 5) & 0b00000001;
+
+    return val;
+}
+
+/**
+ * @brief Turns proximity interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setProximityIntEnable(uint8_t enable)
+{
+    uint8_t val;
+
+    /* Read value from ENABLE register */
+    if( !wireReadDataByte(APDS9960_ENABLE, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    enable &= 0b00000001;
+    enable = enable << 5;
+    val &= 0b11011111;
+    val |= enable;
+
+    /* Write register value back into ENABLE register */
+    if( !wireWriteDataByte(APDS9960_ENABLE, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Gets if gesture interrupts are enabled or not
+ *
+ * @return 1 if interrupts are enabled, 0 if not. 0xFF on error.
+ */
+uint8_t APDS9960_getGestureIntEnable()
+{
+    uint8_t val;
+
+    /* Read value from GCONF4 register */
+    if( !wireReadDataByte(APDS9960_GCONF4, &val) ) {
+        return ERROR;
+    }
+
+    /* Shift and mask out GIEN bit */
+    val = (val >> 1) & 0b00000001;
+
+    return val;
+}
+
+/**
+ * @brief Turns gesture-related interrupts on or off
+ *
+ * @param[in] enable 1 to enable interrupts, 0 to turn them off
+ * @return True if operation successful. False otherwise.
+ */
+bool APDS9960_setGestureIntEnable(uint8_t enable)
+{
+    uint8_t val;
+
+    /* Read value from GCONF4 register */
+    if( !wireReadDataByte(APDS9960_GCONF4, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    enable &= 0b00000001;
+    enable = enable << 1;
+    val &= 0b11111101;
+    val |= enable;
+
+    /* Write register value back into GCONF4 register */
+    if( !wireWriteDataByte(APDS9960_GCONF4, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Clears the ambient light interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool APDS9960_clearAmbientLightInt()
+{
+    uint8_t throwaway;
+    if( !wireReadDataByte(APDS9960_AICLEAR, &throwaway) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Clears the proximity interrupt
+ *
+ * @return True if operation completed successfully. False otherwise.
+ */
+bool APDS9960_clearProximityInt()
+{
+    uint8_t throwaway;
+    if( !wireReadDataByte(APDS9960_PICLEAR, &throwaway) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/**
+ * @brief Tells if the gesture state machine is currently running
+ *
+ * @return 1 if gesture state machine is running, 0 if not. 0xFF on error.
+ */
+uint8_t getGestureMode()
+{
+    uint8_t val;
+
+    /* Read value from GCONF4 register */
+    if( !wireReadDataByte(APDS9960_GCONF4, &val) ) {
+        return ERROR;
+    }
+
+    /* Mask out GMODE bit */
+    val &= 0b00000001;
+
+    return val;
+}
+
+/**
+ * @brief Tells the state machine to either enter or exit gesture state machine
+ *
+ * @param[in] mode 1 to enter gesture state machine, 0 to exit.
+ * @return True if operation successful. False otherwise.
+ */
+bool setGestureMode(uint8_t mode)
+{
+    uint8_t val;
+
+    /* Read value from GCONF4 register */
+    if( !wireReadDataByte(APDS9960_GCONF4, &val) ) {
+        return false;
+    }
+
+    /* Set bits in register to given value */
+    mode &= 0b00000001;
+    val &= 0b11111110;
+    val |= mode;
+
+    /* Write register value back into GCONF4 register */
+    if( !wireWriteDataByte(APDS9960_GCONF4, val) ) {
+        return false;
+    }
+
+    return true;
+}
+
+/*******************************************************************************
+ * Raw I2C Reads and Writes
+ ******************************************************************************/
+
+/**
+ * @brief Writes a single byte to the I2C device (no register)
+ *
+ * @param[in] val the 1-byte value to write to the I2C device
+ * @return True if successful write operation. False otherwise.
+ */
+bool wireWriteByte(uint8_t val)
+{
+
+    return I2Cdev_writeByte(APDS9960_I2C_ADDR, 0, val) != I2C_ERROR;
+}
+
+/**
+ * @brief Writes a single byte to the I2C device and specified register
+ *
+ * @param[in] reg the register in the I2C device to write to
+ * @param[in] val the 1-byte value to write to the I2C device
+ * @return True if successful write operation. False otherwise.
+ */
+bool wireWriteDataByte(uint8_t reg, uint8_t val)
+{
+    return I2Cdev_writeByte(APDS9960_I2C_ADDR, reg, val) != I2C_ERROR;
+}
+
+/**
+ * @brief Writes a block (array) of bytes to the I2C device and register
+ *
+ * @param[in] reg the register in the I2C device to write to
+ * @param[in] val pointer to the beginning of the data byte array
+ * @param[in] len the length (in bytes) of the data to write
+ * @return True if successful write operation. False otherwise.
+ */
+bool wireWriteDataBlock(  uint8_t reg,
+                                        uint8_t *val,
+                                        unsigned int len)
+{
+
+    return I2Cdev_writeBytes(APDS9960_I2C_ADDR, reg, len, val) != I2C_ERROR;
+}
+
+/**
+ * @brief Reads a single byte from the I2C device and specified register
+ *
+ * @param[in] reg the register to read from
+ * @param[out] the value returned from the register
+ * @return True if successful read operation. False otherwise.
+ */
+bool wireReadDataByte(uint8_t reg, uint8_t *val)
+{
+
+    return I2Cdev_readByte(APDS9960_I2C_ADDR, reg, val, 1000);
+}
+
+/**
+ * @brief Reads a block (array) of bytes from the I2C device and register
+ *
+ * @param[in] reg the register to read from
+ * @param[out] val pointer to the beginning of the data
+ * @param[in] len number of bytes to read
+ * @return Number of bytes read. -1 on read error.
+ */
+int wireReadDataBlock(   uint8_t reg,
+                                        uint8_t *val,
+                                        unsigned int len)
+{
+    return I2Cdev_readBytes(APDS9960_I2C_ADDR, reg, len, val, 1000);
+}
+
diff --git a/STM32HAL/APDS9960/APDS9960.h b/STM32HAL/APDS9960/APDS9960.h
new file mode 100644
index 00000000..2244bd21
--- /dev/null
+++ b/STM32HAL/APDS9960/APDS9960.h
@@ -0,0 +1,285 @@
+/**
+ * @author      : shmakins (shmakins@gmail.com)
+ * @file        : APDS9960
+ * @created     : Thursday Oct 12, 2017 23:22:50 EEST
+ */
+
+#ifndef APDS9960_H
+
+#define APDS9960_H
+#include 
+#include "I2Cdev.h"
+//#define DEBUG 1
+
+#ifdef DEBUG
+//#include "lcd.h"
+#endif
+
+/* APDS-9960 I2C address */
+#define APDS9960_I2C_ADDR       0x39
+
+/* Gesture parameters */
+#define GESTURE_THRESHOLD_OUT   10
+#define GESTURE_SENSITIVITY_1   50
+#define GESTURE_SENSITIVITY_2   20
+
+/* Success code for returned values */
+#define ERROR                   0xFF
+#define I2C_ERROR               ((int8_t)-1)
+
+/* Acceptable device IDs */
+#define APDS9960_ID_1           0xAB
+#define APDS9960_ID_2           0x9C
+#define APDS9960_ID_3           0xA8
+
+/* Misc parameters */
+#define FIFO_PAUSE_TIME         30      // Wait period (ms) between FIFO reads
+
+/* APDS-9960 register addresses */
+#define APDS9960_ENABLE         0x80
+#define APDS9960_ATIME          0x81
+#define APDS9960_WTIME          0x83
+#define APDS9960_AILTL          0x84
+#define APDS9960_AILTH          0x85
+#define APDS9960_AIHTL          0x86
+#define APDS9960_AIHTH          0x87
+#define APDS9960_PILT           0x89
+#define APDS9960_PIHT           0x8B
+#define APDS9960_PERS           0x8C
+#define APDS9960_CONFIG1        0x8D
+#define APDS9960_PPULSE         0x8E
+#define APDS9960_CONTROL        0x8F
+#define APDS9960_CONFIG2        0x90
+#define APDS9960_ID             0x92
+#define APDS9960_STATUS         0x93
+#define APDS9960_CDATAL         0x94
+#define APDS9960_CDATAH         0x95
+#define APDS9960_RDATAL         0x96
+#define APDS9960_RDATAH         0x97
+#define APDS9960_GDATAL         0x98
+#define APDS9960_GDATAH         0x99
+#define APDS9960_BDATAL         0x9A
+#define APDS9960_BDATAH         0x9B
+#define APDS9960_PDATA          0x9C
+#define APDS9960_POFFSET_UR     0x9D
+#define APDS9960_POFFSET_DL     0x9E
+#define APDS9960_CONFIG3        0x9F
+#define APDS9960_GPENTH         0xA0
+#define APDS9960_GEXTH          0xA1
+#define APDS9960_GCONF1         0xA2
+#define APDS9960_GCONF2         0xA3
+#define APDS9960_GOFFSET_U      0xA4
+#define APDS9960_GOFFSET_D      0xA5
+#define APDS9960_GOFFSET_L      0xA7
+#define APDS9960_GOFFSET_R      0xA9
+#define APDS9960_GPULSE         0xA6
+#define APDS9960_GCONF3         0xAA
+#define APDS9960_GCONF4         0xAB
+#define APDS9960_GFLVL          0xAE
+#define APDS9960_GSTATUS        0xAF
+#define APDS9960_IFORCE         0xE4
+#define APDS9960_PICLEAR        0xE5
+#define APDS9960_CICLEAR        0xE6
+#define APDS9960_AICLEAR        0xE7
+#define APDS9960_GFIFO_U        0xFC
+#define APDS9960_GFIFO_D        0xFD
+#define APDS9960_GFIFO_L        0xFE
+#define APDS9960_GFIFO_R        0xFF
+
+/* Bit fields */
+#define APDS9960_PON            0b00000001
+#define APDS9960_AEN            0b00000010
+#define APDS9960_PEN            0b00000100
+#define APDS9960_WEN            0b00001000
+#define APSD9960_AIEN           0b00010000
+#define APDS9960_PIEN           0b00100000
+#define APDS9960_GEN            0b01000000
+#define APDS9960_GVALID         0b00000001
+
+/* On/Off definitions */
+#define OFF                     0
+#define ON                      1
+
+/* Acceptable parameters for setMode */
+#define POWER                   0
+#define AMBIENT_LIGHT           1
+#define PROXIMITY               2
+#define WAIT                    3
+#define AMBIENT_LIGHT_INT       4
+#define PROXIMITY_INT           5
+#define GESTURE                 6
+#define ALL                     7
+
+/* LED Drive values */
+#define LED_DRIVE_100MA         0
+#define LED_DRIVE_50MA          1
+#define LED_DRIVE_25MA          2
+#define LED_DRIVE_12_5MA        3
+
+/* Proximity Gain (PGAIN) values */
+#define PGAIN_1X                0
+#define PGAIN_2X                1
+#define PGAIN_4X                2
+#define PGAIN_8X                3
+
+/* ALS Gain (AGAIN) values */
+#define AGAIN_1X                0
+#define AGAIN_4X                1
+#define AGAIN_16X               2
+#define AGAIN_64X               3
+
+/* Gesture Gain (GGAIN) values */
+#define GGAIN_1X                0
+#define GGAIN_2X                1
+#define GGAIN_4X                2
+#define GGAIN_8X                3
+
+/* LED Boost values */
+#define LED_BOOST_100           0
+#define LED_BOOST_150           1
+#define LED_BOOST_200           2
+#define LED_BOOST_300           3
+
+/* Gesture wait time values */
+#define GWTIME_0MS              0
+#define GWTIME_2_8MS            1
+#define GWTIME_5_6MS            2
+#define GWTIME_8_4MS            3
+#define GWTIME_14_0MS           4
+#define GWTIME_22_4MS           5
+#define GWTIME_30_8MS           6
+#define GWTIME_39_2MS           7
+
+/* Default values */
+#define DEFAULT_ATIME           219     // 103ms
+#define DEFAULT_WTIME           246     // 27ms
+#define DEFAULT_PROX_PPULSE     0x87    // 16us, 8 pulses
+#define DEFAULT_GESTURE_PPULSE  0x89    // 16us, 10 pulses
+#define DEFAULT_POFFSET_UR      0       // 0 offset
+#define DEFAULT_POFFSET_DL      0       // 0 offset
+#define DEFAULT_CONFIG1         0x60    // No 12x wait (WTIME) factor
+#define DEFAULT_LDRIVE          LED_DRIVE_100MA
+#define DEFAULT_PGAIN           PGAIN_4X
+#define DEFAULT_AGAIN           AGAIN_4X
+#define DEFAULT_PILT            0       // Low proximity threshold
+#define DEFAULT_PIHT            50      // High proximity threshold
+#define DEFAULT_AILT            0xFFFF  // Force interrupt for calibration
+#define DEFAULT_AIHT            0
+#define DEFAULT_PERS            0x11    // 2 consecutive prox or ALS for int.
+#define DEFAULT_CONFIG2         0x01    // No saturation interrupts or LED boost
+#define DEFAULT_CONFIG3         0       // Enable all photodiodes, no SAI
+#define DEFAULT_GPENTH          40      // Threshold for entering gesture mode
+#define DEFAULT_GEXTH           30      // Threshold for exiting gesture mode
+#define DEFAULT_GCONF1          0x40    // 4 gesture events for int., 1 for exit
+#define DEFAULT_GGAIN           GGAIN_4X
+#define DEFAULT_GLDRIVE         LED_DRIVE_100MA
+#define DEFAULT_GWTIME          GWTIME_2_8MS
+#define DEFAULT_GOFFSET         0       // No offset scaling for gesture mode
+#define DEFAULT_GPULSE          0xC9    // 32us, 10 pulses
+#define DEFAULT_GCONF3          0       // All photodiodes active during gesture
+#define DEFAULT_GIEN            0       // Disable gesture interrupts
+
+/* Direction definitions */
+enum {
+  DIR_NONE,
+  DIR_LEFT,
+  DIR_RIGHT,
+  DIR_UP,
+  DIR_DOWN,
+  DIR_NEAR,
+  DIR_FAR,
+  DIR_ALL
+};
+
+/* State definitions */
+enum {
+  NA_STATE,
+  NEAR_STATE,
+  FAR_STATE,
+  ALL_STATE
+};
+
+/* Container for gesture data */
+typedef struct gesture_data_type {
+    uint8_t u_data[32];
+    uint8_t d_data[32];
+    uint8_t l_data[32];
+    uint8_t r_data[32];
+    uint8_t index;
+    uint8_t total_gestures;
+    uint8_t in_threshold;
+    uint8_t out_threshold;
+} gesture_data_type;
+
+/* APDS9960 Class */
+
+bool APDS9960_init(I2C_HandleTypeDef * hi2c);
+uint8_t APDS9960_getMode();
+bool APDS9960_setMode(uint8_t mode, uint8_t enable);
+
+/* Turn the APDS-9960 on and off */
+bool APDS9960_enablePower();
+bool APDS9960_disablePower();
+
+/* Enable or disable specific sensors */
+bool APDS9960_enableLightSensor();
+bool APDS9960_disableLightSensor();
+bool APDS9960_enableProximitySensor();
+bool APDS9960_disableProximitySensor();
+bool APDS9960_enableGestureSensor();
+bool APDS9960_disableGestureSensor();
+
+/* LED drive strength control */
+uint8_t APDS9960_getLEDDrive();
+bool APDS9960_setLEDDrive(uint8_t drive);
+uint8_t APDS9960_getGestureLEDDrive();
+bool APDS9960_setGestureLEDDrive(uint8_t drive);
+
+/* Gain control */
+uint8_t APDS9960_getAmbientLightGain();
+bool APDS9960_setAmbientLightGain(uint8_t gain);
+uint8_t APDS9960_getProximityGain();
+bool APDS9960_setProximityGain(uint8_t gain);
+uint8_t APDS9960_getGestureGain();
+bool APDS9960_setGestureGain(uint8_t gain);
+
+/* Get and set light interrupt thresholds */
+bool APDS9960_getLightIntLowThreshold(uint16_t *threshold);
+bool APDS9960_setLightIntLowThreshold(uint16_t threshold);
+bool APDS9960_getLightIntHighThreshold(uint16_t *threshold);
+bool APDS9960_setLightIntHighThreshold(uint16_t threshold);
+
+/* Get and set proximity interrupt thresholds */
+bool APDS9960_getProximityIntLowThreshold(uint8_t *threshold);
+bool APDS9960_setProximityIntLowThreshold(uint8_t threshold);
+bool APDS9960_getProximityIntHighThreshold(uint8_t *threshold);
+bool APDS9960_setProximityIntHighThreshold(uint8_t threshold);
+
+/* Get and set interrupt enables */
+uint8_t APDS9960_getAmbientLightIntEnable();
+bool APDS9960_setAmbientLightIntEnable(uint8_t enable);
+uint8_t APDS9960_getProximityIntEnable();
+bool APDS9960_setProximityIntEnable(uint8_t enable);
+uint8_t APDS9960_getGestureIntEnable();
+bool APDS9960_setGestureIntEnable(uint8_t enable);
+
+/* Clear interrupts */
+bool APDS9960_clearAmbientLightInt();
+bool APDS9960_clearProximityInt();
+
+/* Ambient light methods */
+bool APDS9960_readAmbientLight(uint16_t *val);
+bool APDS9960_readRedLight(uint16_t *val);
+bool APDS9960_readGreenLight(uint16_t *val);
+bool APDS9960_readBlueLight(uint16_t *val);
+
+/* Proximity methods */
+bool APDS9960_readProximity(uint8_t *val);
+
+/* Gesture methods */
+bool APDS9960_isGestureAvailable();
+int APDS9960_readGesture();
+
+
+#endif /* end of include guard APDS9960_H */
+
diff --git a/STM32HAL/APDS9960/README.md b/STM32HAL/APDS9960/README.md
new file mode 100755
index 00000000..9dce2115
--- /dev/null
+++ b/STM32HAL/APDS9960/README.md
@@ -0,0 +1 @@
+Ported to C and HAL for STM32 from [Arduino driver](https://github.com/sparkfun/SparkFun_APDS-9960_Sensor_Arduino_Library)
diff --git a/STM32HAL/BMP085/BMP085.c b/STM32HAL/BMP085/BMP085.c
new file mode 100644
index 00000000..20193842
--- /dev/null
+++ b/STM32HAL/BMP085/BMP085.c
@@ -0,0 +1,306 @@
+// I2Cdev library collection - BMP085 I2C device class
+// Based on register information stored in the I2Cdevlib internal database
+// 2012-06-28 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+// 2015-06-06 by Andrey Voloshin 
+//
+// Changelog:
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+
+/* ============================================
+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 "BMP085.h"
+
+static uint8_t devAddr = BMP085_DEFAULT_ADDRESS;
+static uint8_t buffer[3];
+
+static bool calibrationLoaded;
+static int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
+static uint16_t ac4, ac5, ac6;
+static int32_t b5;
+static uint8_t measureMode;
+
+/**
+ * Specific address constructor.
+ * @param address Specific device address
+ * @see BMP085_DEFAULT_ADDRESS
+ */
+void BMP085_setAddress(uint8_t address)
+{
+    devAddr = address;
+}
+
+/**
+ * Prepare device for normal usage.
+ */
+void BMP085_initialize()
+{
+    if (devAddr==0) devAddr = BMP085_DEFAULT_ADDRESS;
+    // load sensor's calibration constants
+    BMP085_loadCalibration();
+}
+
+/**
+ * Verify the device is connected and available.
+ */
+bool BMP085_testConnection()
+{
+    // test for a response, though this is very basic
+    return I2Cdev_readByte(devAddr, BMP085_RA_AC1_H, buffer, 100) == 1;
+}
+
+/* calibration register methods */
+
+void BMP085_loadCalibration()
+{
+    uint8_t buf2[22];
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC1_H, 22, buf2, 1000);
+    ac1 = ((int16_t)buf2[0] << 8) + buf2[1];
+    ac2 = ((int16_t)buf2[2] << 8) + buf2[3];
+    ac3 = ((int16_t)buf2[4] << 8) + buf2[5];
+    ac4 = ((uint16_t)buf2[6] << 8) + buf2[7];
+    ac5 = ((uint16_t)buf2[8] << 8) + buf2[9];
+    ac6 = ((uint16_t)buf2[10] << 8) + buf2[11];
+    b1 = ((int16_t)buf2[12] << 8) + buf2[13];
+    b2 = ((int16_t)buf2[14] << 8) + buf2[15];
+    mb = ((int16_t)buf2[16] << 8) + buf2[17];
+    mc = ((int16_t)buf2[18] << 8) + buf2[19];
+    md = ((int16_t)buf2[20] << 8) + buf2[21];
+    calibrationLoaded = true;
+}
+
+#ifdef BMP085_INCLUDE_INDIVIDUAL_CALIBRATION_ACCESS
+int16_t BMP085_getAC1()
+{
+    if (calibrationLoaded) return ac1;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC1_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getAC2()
+{
+    if (calibrationLoaded) return ac2;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC2_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getAC3()
+{
+    if (calibrationLoaded) return ac3;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC3_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+uint16_t BMP085_getAC4()
+{
+    if (calibrationLoaded) return ac4;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC4_H, 2, buffer);
+    return ((uint16_t)buffer[1] << 8) + buffer[0];
+}
+
+uint16_t BMP085_getAC5()
+{
+    if (calibrationLoaded) return ac5;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC5_H, 2, buffer);
+    return ((uint16_t)buffer[1] << 8) + buffer[0];
+}
+
+uint16_t BMP085_getAC6()
+{
+    if (calibrationLoaded) return ac6;
+    I2Cdev_readBytes(devAddr, BMP085_RA_AC6_H, 2, buffer);
+    return ((uint16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getB1()
+{
+    if (calibrationLoaded) return b1;
+    I2Cdev_readBytes(devAddr, BMP085_RA_B1_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getB2()
+{
+    if (calibrationLoaded) return b2;
+    I2Cdev_readBytes(devAddr, BMP085_RA_B2_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getMB()
+{
+    if (calibrationLoaded) return mb;
+    I2Cdev_readBytes(devAddr, BMP085_RA_MB_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getMC()
+{
+    if (calibrationLoaded) return mc;
+    I2Cdev_readBytes(devAddr, BMP085_RA_MC_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+
+int16_t BMP085_getMD()
+{
+    if (calibrationLoaded) return md;
+    I2Cdev_readBytes(devAddr, BMP085_RA_MD_H, 2, buffer);
+    return ((int16_t)buffer[1] << 8) + buffer[0];
+}
+#endif
+
+/* control register methods */
+
+uint8_t BMP085_getControl()
+{
+    I2Cdev_readByte(devAddr, BMP085_RA_CONTROL, buffer, 1000);
+    return buffer[0];
+}
+void BMP085_setControl(uint8_t value)
+{
+    I2Cdev_writeByte(devAddr, BMP085_RA_CONTROL, value);
+    measureMode = value;
+}
+
+/* measurement register methods */
+
+uint16_t BMP085_getMeasurement2()
+{
+    I2Cdev_readBytes(devAddr, BMP085_RA_MSB, 2, buffer, 1000);
+    return ((uint16_t) buffer[0] << 8) + buffer[1];
+}
+uint32_t BMP085_getMeasurement3()
+{
+    I2Cdev_readBytes(devAddr, BMP085_RA_MSB, 3, buffer, 1000);
+    return ((uint32_t)buffer[0] << 16) + ((uint16_t)buffer[1] << 8) + buffer[2];
+}
+uint8_t BMP085_getMeasureDelayMilliseconds(uint8_t mode)
+{
+    if (mode == 0) mode = measureMode;
+    if (measureMode == 0x2E) return 5;
+    else if (measureMode == 0x34) return 5;
+    else if (measureMode == 0x74) return 8;
+    else if (measureMode == 0xB4) return 14;
+    else if (measureMode == 0xF4) return 26;
+    return 0; // invalid mode
+}
+uint16_t BMP085_getMeasureDelayMicroseconds(uint8_t mode)
+{
+    if (mode == 0) mode = measureMode;
+    if (measureMode == 0x2E) return 4500;
+    else if (measureMode == 0x34) return 4500;
+    else if (measureMode == 0x74) return 7500;
+    else if (measureMode == 0xB4) return 13500;
+    else if (measureMode == 0xF4) return 25500;
+    return 0; // invalid mode
+}
+
+uint16_t BMP085_getRawTemperature()
+{
+    if (measureMode == 0x2E) return BMP085_getMeasurement2();
+    return 0; // wrong measurement mode for temperature request
+}
+
+float BMP085_getTemperatureC()
+{
+    /*
+    Datasheet formula:
+        UT = raw temperature
+        X1 = (UT - AC6) * AC5 / 2^15
+        X2 = MC * 2^11 / (X1 + MD)
+        B5 = X1 + X2
+        T = (B5 + 8) / 2^4
+    */
+    int32_t ut = BMP085_getRawTemperature();
+    int32_t x1 = ((ut - (int32_t)ac6) * (int32_t)ac5) >> 15;
+    int32_t x2 = ((int32_t)mc << 11) / (x1 + md);
+    b5 = x1 + x2;
+    return (float)((b5 + 8) >> 4) / 10.0f;
+}
+
+float BMP085_getTemperatureF()
+{
+    return BMP085_getTemperatureC() * 9.0f / 5.0f + 32;
+}
+
+uint32_t BMP085_getRawPressure()
+{
+    if (measureMode & 0x34) return BMP085_getMeasurement3() >> (8 - ((measureMode & 0xC0) >> 6));
+    return 0; // wrong measurement mode for pressure request
+}
+
+float BMP085_getPressure()
+{
+    /*
+    Datasheet forumla
+        UP = raw pressure
+        B6 = B5 - 4000
+        X1 = (B2 * (B6 * B6 / 2^12)) / 2^11
+        X2 = AC2 * B6 / 2^11
+        X3 = X1 + X2
+        B3 = ((AC1 * 4 + X3) << oss + 2) / 4
+        X1 = AC3 * B6 / 2^13
+        X2 = (B1 * (B6 * B6 / 2^12)) / 2^16
+        X3 = ((X1 + X2) + 2) / 2^2
+        B4 = AC4 * (unsigned long)(X3 + 32768) / 2^15
+        B7 = ((unsigned long)UP - B3) * (50000 >> oss)
+        if (B7 < 0x80000000) { p = (B7 * 2) / B4 }
+        else { p = (B7 / B4) * 2 }
+        X1 = (p / 2^8) * (p / 2^8)
+        X1 = (X1 * 3038) / 2^16
+        X2 = (-7357 * p) / 2^16
+        p = p + (X1 + X2 + 3791) / 2^4
+    */
+    uint32_t up = BMP085_getRawPressure();
+    uint8_t oss = (measureMode & 0xC0) >> 6;
+    int32_t p;
+    int32_t b6 = b5 - 4000;
+    int32_t x1 = ((int32_t)b2 * ((b6 * b6) >> 12)) >> 11;
+    int32_t x2 = ((int32_t)ac2 * b6) >> 11;
+    int32_t x3 = x1 + x2;
+    int32_t b3 = ((((int32_t)ac1 * 4 + x3) << oss) + 2) >> 2;
+    x1 = ((int32_t)ac3 * b6) >> 13;
+    x2 = ((int32_t)b1 * ((b6 * b6) >> 12)) >> 16;
+    x3 = ((x1 + x2) + 2) >> 2;
+    uint32_t b4 = ((uint32_t)ac4 * (uint32_t)(x3 + 32768)) >> 15;
+    uint32_t b7 = ((uint32_t)up - b3) * (uint32_t)(50000UL >> oss);
+    if (b7 < 0x80000000)
+    {
+        p = (b7 << 1) / b4;
+    }
+    else
+    {
+        p = (b7 / b4) << 1;
+    }
+    x1 = (p >> 8) * (p >> 8);
+    x1 = (x1 * 3038) >> 16;
+    x2 = (-7357 * p) >> 16;
+    return p + ((x1 + x2 + (int32_t)3791) >> 4);
+}
+
+float BMP085_getAltitude(float pressure, float seaLevelPressure)
+{
+    if (seaLevelPressure == 0) seaLevelPressure = 101325;
+    return 44330 * (1.0 - pow(pressure / seaLevelPressure, 0.1903));
+}
diff --git a/STM32HAL/BMP085/BMP085.h b/STM32HAL/BMP085/BMP085.h
new file mode 100644
index 00000000..ab1b6af8
--- /dev/null
+++ b/STM32HAL/BMP085/BMP085.h
@@ -0,0 +1,113 @@
+// I2Cdev library collection - BMP085 I2C device class
+// Based on register information stored in the I2Cdevlib internal database
+// 2012-06-28 by Jeff Rowberg 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+// 2015-06-06 by Andrey Voloshin 
+//
+// Changelog:
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+
+/* ============================================
+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 _BMP085_H_
+#define _BMP085_H_
+
+#include "I2Cdev.h"
+#include 
+
+#define BMP085_ADDRESS              0x77
+#define BMP085_DEFAULT_ADDRESS      BMP085_ADDRESS
+
+#define BMP085_RA_AC1_H     0xAA    /* AC1_H */
+#define BMP085_RA_AC1_L     0xAB    /* AC1_L */
+#define BMP085_RA_AC2_H     0xAC    /* AC2_H */
+#define BMP085_RA_AC2_L     0xAD    /* AC2_L */
+#define BMP085_RA_AC3_H     0xAE    /* AC3_H */
+#define BMP085_RA_AC3_L     0xAF    /* AC3_L */
+#define BMP085_RA_AC4_H     0xB0    /* AC4_H */
+#define BMP085_RA_AC4_L     0xB1    /* AC4_L */
+#define BMP085_RA_AC5_H     0xB2    /* AC5_H */
+#define BMP085_RA_AC5_L     0xB3    /* AC5_L */
+#define BMP085_RA_AC6_H     0xB4    /* AC6_H */
+#define BMP085_RA_AC6_L     0xB5    /* AC6_L */
+#define BMP085_RA_B1_H      0xB6    /* B1_H */
+#define BMP085_RA_B1_L      0xB7    /* B1_L */
+#define BMP085_RA_B2_H      0xB8    /* B2_H */
+#define BMP085_RA_B2_L      0xB9    /* B2_L */
+#define BMP085_RA_MB_H      0xBA    /* MB_H */
+#define BMP085_RA_MB_L      0xBB    /* MB_L */
+#define BMP085_RA_MC_H      0xBC    /* MC_H */
+#define BMP085_RA_MC_L      0xBD    /* MC_L */
+#define BMP085_RA_MD_H      0xBE    /* MD_H */
+#define BMP085_RA_MD_L      0xBF    /* MD_L */
+#define BMP085_RA_CONTROL   0xF4    /* CONTROL */
+#define BMP085_RA_MSB       0xF6    /* MSB */
+#define BMP085_RA_LSB       0xF7    /* LSB */
+#define BMP085_RA_XLSB      0xF8    /* XLSB */
+
+#define BMP085_MODE_TEMPERATURE     0x2E
+#define BMP085_MODE_PRESSURE_0      0x34
+#define BMP085_MODE_PRESSURE_1      0x74
+#define BMP085_MODE_PRESSURE_2      0xB4
+#define BMP085_MODE_PRESSURE_3      0xF4
+
+void BMP085_initialize();
+bool BMP085_testConnection();
+
+/* calibration register methods */
+int16_t     BMP085_getAC1();
+int16_t     BMP085_getAC2();
+int16_t     BMP085_getAC3();
+uint16_t    BMP085_getAC4();
+uint16_t    BMP085_getAC5();
+uint16_t    BMP085_getAC6();
+int16_t     BMP085_getB1();
+int16_t     BMP085_getB2();
+int16_t     BMP085_getMB();
+int16_t     BMP085_getMC();
+int16_t     BMP085_getMD();
+
+/* CONTROL register methods */
+uint8_t     BMP085_getControl();
+void        BMP085_setControl(uint8_t value);
+
+/* MEASURE register methods */
+uint16_t    BMP085_getMeasurement2(); // 16-bit data
+uint32_t    BMP085_getMeasurement3(); // 24-bit data
+uint8_t     BMP085_getMeasureDelayMilliseconds(uint8_t mode);
+uint16_t    BMP085_getMeasureDelayMicroseconds(uint8_t mode);
+
+// convenience methods
+void        BMP085_loadCalibration();
+uint16_t    BMP085_getRawTemperature();
+float       BMP085_getTemperatureC();
+float       BMP085_getTemperatureF();
+uint32_t    BMP085_getRawPressure();
+float       BMP085_getPressure();
+float       BMP085_getAltitude(float pressure, float seaLevelPressure);
+
+
+
+#endif /* _BMP085_H_ */
diff --git a/STM32HAL/HMC5883L/HMC5883L.c b/STM32HAL/HMC5883L/HMC5883L.c
new file mode 100644
index 00000000..9bd2a598
--- /dev/null
+++ b/STM32HAL/HMC5883L/HMC5883L.c
@@ -0,0 +1,356 @@
+// I2Cdev library collection - HMC5883L I2C device class
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+//
+// Changelog:
+//      2015-06-06 - ported to STM32 HAL library from Arduino code
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+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 "HMC5883L.h"
+
+static uint8_t devAddr;
+static uint8_t buffer[6];
+static uint8_t mode;
+
+
+/** Power on and prepare for general usage.
+ * This will prepare the magnetometer with default settings, ready for single-
+ * use mode (very low power requirements). Default settings include 8-sample
+ * averaging, 15 Hz data output rate, normal measurement bias, a,d 1090 gain (in
+ * terms of LSB/Gauss). Be sure to adjust any settings you need specifically
+ * after initialization, especially the gain settings if you happen to be seeing
+ * a lot of -4096 values (see the datasheet for mor information).
+ */
+void HMC5883L_initialize() {
+    devAddr = HMC5883L_DEFAULT_ADDRESS;
+    // write CONFIG_A register
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_CONFIG_A,
+        (HMC5883L_AVERAGING_8 << (HMC5883L_CRA_AVERAGE_BIT - HMC5883L_CRA_AVERAGE_LENGTH + 1)) |
+        (HMC5883L_RATE_15     << (HMC5883L_CRA_RATE_BIT - HMC5883L_CRA_RATE_LENGTH + 1)) |
+        (HMC5883L_BIAS_NORMAL << (HMC5883L_CRA_BIAS_BIT - HMC5883L_CRA_BIAS_LENGTH + 1)));
+
+    // write CONFIG_B register
+    HMC5883L_setGain(HMC5883L_GAIN_1090);
+
+    // write MODE register
+    HMC5883L_setMode(HMC5883L_MODE_SINGLE);
+}
+
+/** Verify the I2C connection.
+ * Make sure the device is connected and responds as expected.
+ * @return True if connection is valid, false otherwise
+ */
+bool HMC5883L_testConnection() {
+    if (I2Cdev_readBytes(devAddr, HMC5883L_RA_ID_A, 3, buffer, 0) == 3) {
+        return (buffer[0] == 'H' && buffer[1] == '4' && buffer[2] == '3');
+    }
+    return false;
+}
+
+// CONFIG_A register
+
+/** Get number of samples averaged per measurement.
+ * @return Current samples averaged per measurement (0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_AVERAGING_8
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+uint8_t HMC5883L_getSampleAveraging() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, buffer, 0);
+    return buffer[0];
+}
+/** Set number of samples averaged per measurement.
+ * @param averaging New samples averaged per measurement setting(0-3 for 1/2/4/8 respectively)
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_AVERAGE_BIT
+ * @see HMC5883L_CRA_AVERAGE_LENGTH
+ */
+void HMC5883L_setSampleAveraging(uint8_t averaging) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_AVERAGE_BIT, HMC5883L_CRA_AVERAGE_LENGTH, averaging);
+}
+/** Get data output rate value.
+ * The Table below shows all selectable output rates in continuous measurement
+ * mode. All three channels shall be measured within a given output rate. Other
+ * output rates with maximum rate of 160 Hz can be achieved by monitoring DRDY
+ * interrupt pin in single measurement mode.
+ *
+ * Value | Typical Data Output Rate (Hz)
+ * ------+------------------------------
+ * 0     | 0.75
+ * 1     | 1.5
+ * 2     | 3
+ * 3     | 7.5
+ * 4     | 15 (Default)
+ * 5     | 30
+ * 6     | 75
+ * 7     | Not used
+ *
+ * @return Current rate of data output to registers
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+uint8_t HMC5883L_getDataRate() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, buffer, 0);
+    return buffer[0];
+}
+/** Set data output rate value.
+ * @param rate Rate of data output to registers
+ * @see getDataRate()
+ * @see HMC5883L_RATE_15
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_RATE_BIT
+ * @see HMC5883L_CRA_RATE_LENGTH
+ */
+void HMC5883L_setDataRate(uint8_t rate) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_RATE_BIT, HMC5883L_CRA_RATE_LENGTH, rate);
+}
+/** Get measurement bias value.
+ * @return Current bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+uint8_t HMC5883L_getMeasurementBias() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, buffer, 0);
+    return buffer[0];
+}
+/** Set measurement bias value.
+ * @param bias New bias value (0-2 for normal/positive/negative respectively)
+ * @see HMC5883L_BIAS_NORMAL
+ * @see HMC5883L_RA_CONFIG_A
+ * @see HMC5883L_CRA_BIAS_BIT
+ * @see HMC5883L_CRA_BIAS_LENGTH
+ */
+void HMC5883L_setMeasurementBias(uint8_t bias) {
+    I2Cdev_writeBits(devAddr, HMC5883L_RA_CONFIG_A, HMC5883L_CRA_BIAS_BIT, HMC5883L_CRA_BIAS_LENGTH, bias);
+}
+
+// CONFIG_B register
+
+/** Get magnetic field gain value.
+ * The table below shows nominal gain settings. Use the "Gain" column to convert
+ * counts to Gauss. Choose a lower gain value (higher GN#) when total field
+ * strength causes overflow in one of the data output registers (saturation).
+ * The data output range for all settings is 0xF800-0x07FF (-2048 - 2047).
+ *
+ * Value | Field Range | Gain (LSB/Gauss)
+ * ------+-------------+-----------------
+ * 0     | +/- 0.88 Ga | 1370
+ * 1     | +/- 1.3 Ga  | 1090 (Default)
+ * 2     | +/- 1.9 Ga  | 820
+ * 3     | +/- 2.5 Ga  | 660
+ * 4     | +/- 4.0 Ga  | 440
+ * 5     | +/- 4.7 Ga  | 390
+ * 6     | +/- 5.6 Ga  | 330
+ * 7     | +/- 8.1 Ga  | 230
+ *
+ * @return Current magnetic field gain value
+ * @see HMC5883L_GAIN_1090
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+uint8_t HMC5883L_getGain() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_CONFIG_B, HMC5883L_CRB_GAIN_BIT, HMC5883L_CRB_GAIN_LENGTH, buffer, 0);
+    return buffer[0];
+}
+/** Set magnetic field gain value.
+ * @param gain New magnetic field gain value
+ * @see getGain()
+ * @see HMC5883L_RA_CONFIG_B
+ * @see HMC5883L_CRB_GAIN_BIT
+ * @see HMC5883L_CRB_GAIN_LENGTH
+ */
+void HMC5883L_setGain(uint8_t gain) {
+    // use this method to guarantee that bits 4-0 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_CONFIG_B, gain << (HMC5883L_CRB_GAIN_BIT - HMC5883L_CRB_GAIN_LENGTH + 1));
+}
+
+// MODE register
+
+/** Get measurement mode.
+ * In continuous-measurement mode, the device continuously performs measurements
+ * and places the result in the data register. RDY goes high when new data is
+ * placed in all three registers. After a power-on or a write to the mode or
+ * configuration register, the first measurement set is available from all three
+ * data output registers after a period of 2/fDO and subsequent measurements are
+ * available at a frequency of fDO, where fDO is the frequency of data output.
+ *
+ * When single-measurement mode (default) is selected, device performs a single
+ * measurement, sets RDY high and returned to idle mode. Mode register returns
+ * to idle mode bit values. The measurement remains in the data output register
+ * and RDY remains high until the data output register is read or another
+ * measurement is performed.
+ *
+ * @return Current measurement mode
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+uint8_t HMC5883L_getMode() {
+    I2Cdev_readBits(devAddr, HMC5883L_RA_MODE, HMC5883L_MODEREG_BIT, HMC5883L_MODEREG_LENGTH, buffer, 0);
+    return buffer[0];
+}
+/** Set measurement mode.
+ * @param newMode New measurement mode
+ * @see getMode()
+ * @see HMC5883L_MODE_CONTINUOUS
+ * @see HMC5883L_MODE_SINGLE
+ * @see HMC5883L_MODE_IDLE
+ * @see HMC5883L_RA_MODE
+ * @see HMC5883L_MODEREG_BIT
+ * @see HMC5883L_MODEREG_LENGTH
+ */
+void HMC5883L_setMode(uint8_t newMode) {
+    // use this method to guarantee that bits 7-2 are set to zero, which is a
+    // requirement specified in the datasheet; it's actually more efficient than
+    // using the I2Cdev.writeBits method
+    I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, newMode << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    mode = newMode; // track to tell if we have to clear bit 7 after a read
+}
+
+// DATA* registers
+
+/** Get 3-axis heading measurements.
+ * In the event the ADC reading overflows or underflows for the given channel,
+ * or if there is a math overflow during the bias measurement, this data
+ * register will contain the value -4096. This register value will clear when
+ * after the next valid measurement is made. Note that this method automatically
+ * clears the appropriate bit in the MODE register if Single mode is active.
+ * @param x 16-bit signed integer container for X-axis heading
+ * @param y 16-bit signed integer container for Y-axis heading
+ * @param z 16-bit signed integer container for Z-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+void HMC5883L_getHeading(int16_t *x, int16_t *y, int16_t *z) {
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer, 0);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
+    *y = (((int16_t)buffer[4]) << 8) | buffer[5];
+    *z = (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+/** Get X-axis heading measurement.
+ * @return 16-bit signed integer with X-axis heading
+ * @see HMC5883L_RA_DATAX_H
+ */
+int16_t HMC5883L_getHeadingX() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer, 0);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[0]) << 8) | buffer[1];
+}
+/** Get Y-axis heading measurement.
+ * @return 16-bit signed integer with Y-axis heading
+ * @see HMC5883L_RA_DATAY_H
+ */
+int16_t HMC5883L_getHeadingY() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer, 0);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[4]) << 8) | buffer[5];
+}
+/** Get Z-axis heading measurement.
+ * @return 16-bit signed integer with Z-axis heading
+ * @see HMC5883L_RA_DATAZ_H
+ */
+int16_t HMC5883L_getHeadingZ() {
+    // each axis read requires that ALL axis registers be read, even if only
+    // one is used; this was not done ineffiently in the code by accident
+    I2Cdev_readBytes(devAddr, HMC5883L_RA_DATAX_H, 6, buffer, 0);
+    if (mode == HMC5883L_MODE_SINGLE) I2Cdev_writeByte(devAddr, HMC5883L_RA_MODE, HMC5883L_MODE_SINGLE << (HMC5883L_MODEREG_BIT - HMC5883L_MODEREG_LENGTH + 1));
+    return (((int16_t)buffer[2]) << 8) | buffer[3];
+}
+
+// STATUS register
+
+/** Get data output register lock status.
+ * This bit is set when this some but not all for of the six data output
+ * registers have been read. When this bit is set, the six data output registers
+ * are locked and any new data will not be placed in these register until one of
+ * three conditions are met: one, all six bytes have been read or the mode
+ * changed, two, the mode is changed, or three, the measurement configuration is
+ * changed.
+ * @return Data output register lock status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_LOCK_BIT
+ */
+bool HMC5883L_getLockStatus() {
+    I2Cdev_readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_LOCK_BIT, buffer, 0);
+    return buffer[0];
+}
+/** Get data ready status.
+ * This bit is set when data is written to all six data registers, and cleared
+ * when the device initiates a write to the data output registers and after one
+ * or more of the data output registers are written to. When RDY bit is clear it
+ * shall remain cleared for 250 us. DRDY pin can be used as an alternative to
+ * the status register for monitoring the device for measurement data.
+ * @return Data ready status
+ * @see HMC5883L_RA_STATUS
+ * @see HMC5883L_STATUS_READY_BIT
+ */
+bool HMC5883L_getReadyStatus() {
+    I2Cdev_readBit(devAddr, HMC5883L_RA_STATUS, HMC5883L_STATUS_READY_BIT, buffer, 0);
+    return buffer[0];
+}
+
+// ID_* registers
+
+/** Get identification byte A
+ * @return ID_A byte (should be 01001000, ASCII value 'H')
+ */
+uint8_t HMC5883L_getIDA() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_A, buffer, 0);
+    return buffer[0];
+}
+/** Get identification byte B
+ * @return ID_A byte (should be 00110100, ASCII value '4')
+ */
+uint8_t HMC5883L_getIDB() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_B, buffer, 0);
+    return buffer[0];
+}
+/** Get identification byte C
+ * @return ID_A byte (should be 00110011, ASCII value '3')
+ */
+uint8_t HMC5883L_getIDC() {
+    I2Cdev_readByte(devAddr, HMC5883L_RA_ID_C, buffer, 0);
+    return buffer[0];
+}
diff --git a/STM32HAL/HMC5883L/HMC5883L.h b/STM32HAL/HMC5883L/HMC5883L.h
new file mode 100644
index 00000000..cd506f47
--- /dev/null
+++ b/STM32HAL/HMC5883L/HMC5883L.h
@@ -0,0 +1,139 @@
+// I2Cdev library collection - HMC5883L I2C device class header file
+// Based on Honeywell HMC5883L datasheet, 10/2010 (Form #900405 Rev B)
+// 6/12/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+//
+// Changelog:
+//      2015-06-06 - ported to STM32 HAL library from Arduino code
+//     2012-06-12 - fixed swapped Y/Z axes
+//     2011-08-22 - small Doxygen comment fixes
+//     2011-07-31 - initial release
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2011 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 _HMC5883L_H_
+#define _HMC5883L_H_
+
+#include "I2Cdev.h"
+
+#define HMC5883L_ADDRESS            0x1E // this device only has one address
+#define HMC5883L_DEFAULT_ADDRESS    0x1E
+
+#define HMC5883L_RA_CONFIG_A        0x00
+#define HMC5883L_RA_CONFIG_B        0x01
+#define HMC5883L_RA_MODE            0x02
+#define HMC5883L_RA_DATAX_H         0x03
+#define HMC5883L_RA_DATAX_L         0x04
+#define HMC5883L_RA_DATAZ_H         0x05
+#define HMC5883L_RA_DATAZ_L         0x06
+#define HMC5883L_RA_DATAY_H         0x07
+#define HMC5883L_RA_DATAY_L         0x08
+#define HMC5883L_RA_STATUS          0x09
+#define HMC5883L_RA_ID_A            0x0A
+#define HMC5883L_RA_ID_B            0x0B
+#define HMC5883L_RA_ID_C            0x0C
+
+#define HMC5883L_CRA_AVERAGE_BIT    6
+#define HMC5883L_CRA_AVERAGE_LENGTH 2
+#define HMC5883L_CRA_RATE_BIT       4
+#define HMC5883L_CRA_RATE_LENGTH    3
+#define HMC5883L_CRA_BIAS_BIT       1
+#define HMC5883L_CRA_BIAS_LENGTH    2
+
+#define HMC5883L_AVERAGING_1        0x00
+#define HMC5883L_AVERAGING_2        0x01
+#define HMC5883L_AVERAGING_4        0x02
+#define HMC5883L_AVERAGING_8        0x03
+
+#define HMC5883L_RATE_0P75          0x00
+#define HMC5883L_RATE_1P5           0x01
+#define HMC5883L_RATE_3             0x02
+#define HMC5883L_RATE_7P5           0x03
+#define HMC5883L_RATE_15            0x04
+#define HMC5883L_RATE_30            0x05
+#define HMC5883L_RATE_75            0x06
+
+#define HMC5883L_BIAS_NORMAL        0x00
+#define HMC5883L_BIAS_POSITIVE      0x01
+#define HMC5883L_BIAS_NEGATIVE      0x02
+
+#define HMC5883L_CRB_GAIN_BIT       7
+#define HMC5883L_CRB_GAIN_LENGTH    3
+
+#define HMC5883L_GAIN_1370          0x00
+#define HMC5883L_GAIN_1090          0x01
+#define HMC5883L_GAIN_820           0x02
+#define HMC5883L_GAIN_660           0x03
+#define HMC5883L_GAIN_440           0x04
+#define HMC5883L_GAIN_390           0x05
+#define HMC5883L_GAIN_330           0x06
+#define HMC5883L_GAIN_220           0x07
+
+#define HMC5883L_MODEREG_BIT        1
+#define HMC5883L_MODEREG_LENGTH     2
+
+#define HMC5883L_MODE_CONTINUOUS    0x00
+#define HMC5883L_MODE_SINGLE        0x01
+#define HMC5883L_MODE_IDLE          0x02
+
+#define HMC5883L_STATUS_LOCK_BIT    1
+#define HMC5883L_STATUS_READY_BIT   0
+
+void HMC5883L_initialize();
+bool HMC5883L_testConnection();
+
+// CONFIG_A register
+uint8_t HMC5883L_getSampleAveraging();
+void HMC5883L_setSampleAveraging(uint8_t averaging);
+uint8_t HMC5883L_getDataRate();
+void HMC5883L_setDataRate(uint8_t rate);
+uint8_t HMC5883L_getMeasurementBias();
+void HMC5883L_setMeasurementBias(uint8_t bias);
+
+// CONFIG_B register
+uint8_t HMC5883L_getGain();
+void HMC5883L_setGain(uint8_t gain);
+
+// MODE register
+uint8_t HMC5883L_getMode();
+void HMC5883L_setMode(uint8_t mode);
+
+// DATA* registers
+void HMC5883L_getHeading(int16_t *x, int16_t *y, int16_t *z);
+int16_t HMC5883L_getHeadingX();
+int16_t HMC5883L_getHeadingY();
+int16_t HMC5883L_getHeadingZ();
+
+// STATUS register
+bool HMC5883L_getLockStatus();
+bool HMC5883L_getReadyStatus();
+
+// ID_* registers
+uint8_t HMC5883L_getIDA();
+uint8_t HMC5883L_getIDB();
+uint8_t HMC5883L_getIDC();
+
+
+#endif /* _HMC5883L_H_ */
diff --git a/STM32HAL/I2Cdev/I2Cdev.c b/STM32HAL/I2Cdev/I2Cdev.c
new file mode 100644
index 00000000..29bdb1b3
--- /dev/null
+++ b/STM32HAL/I2Cdev/I2Cdev.c
@@ -0,0 +1,339 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+//
+// Changelog:
+//      2015-06-06 - ported to STM32 HAL library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 "I2Cdev.h"
+
+// Hold pointer to inited HAL I2C device
+static I2C_HandleTypeDef * I2Cdev_hi2c;
+
+/** Default timeout value for read operations.
+ * Set this to 0 to disable timeout detection.
+ */
+uint16_t I2Cdev_readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
+
+/** Sets device handle to use for communications
+ * You can call this function and set any other device at any moment
+ */
+void I2Cdev_init(I2C_HandleTypeDef * hi2c){
+	I2Cdev_hi2c = hi2c;
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (true = success)
+ */
+uint8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout)
+{
+    uint8_t b;
+    uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (true = success)
+ */
+uint8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout)
+{
+    uint16_t b;
+    uint8_t count = I2Cdev_readWord(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (true = success)
+ */
+uint8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout)
+{
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = I2Cdev_readByte(devAddr, regAddr, &b, timeout)) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+uint8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout)
+{
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = I2Cdev_readWord(devAddr, regAddr, &w, timeout)) != 0)
+    {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (true = success)
+ */
+uint8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout)
+{
+    return I2Cdev_readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Status of read operation (true = success)
+ */
+uint8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout)
+{
+    return I2Cdev_readWords(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Number of bytes read (-1 indicates failure)
+ */
+uint8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout)
+{
+    uint16_t tout = timeout > 0 ? timeout : I2CDEV_DEFAULT_READ_TIMEOUT;
+
+    HAL_I2C_Master_Transmit(I2Cdev_hi2c, devAddr << 1, ®Addr, 1, tout);
+    if (HAL_I2C_Master_Receive(I2Cdev_hi2c, devAddr << 1, data, length, tout) == HAL_OK) return length;
+    return -1;
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev_readTimeout)
+ * @return Number of words read (-1 indicates failure)
+ */
+uint8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout)
+{
+    uint16_t tout = timeout > 0 ? timeout : I2CDEV_DEFAULT_READ_TIMEOUT;
+
+    HAL_I2C_Master_Transmit(I2Cdev_hi2c, devAddr << 1, ®Addr, 1, tout);
+    if (HAL_I2C_Master_Receive(I2Cdev_hi2c, devAddr << 1, (uint8_t *)data, length*2, tout) == HAL_OK)
+        return length;
+    else
+        return -1;
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data)
+{
+    uint8_t b;
+    I2Cdev_readByte(devAddr, regAddr, &b, I2Cdev_readTimeout);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return I2Cdev_writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data)
+{
+    uint16_t w;
+    I2Cdev_readWord(devAddr, regAddr, &w, 100);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return I2Cdev_writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data)
+{
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (I2Cdev_readByte(devAddr, regAddr, &b, 100) != 0)
+    {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return I2Cdev_writeByte(devAddr, regAddr, b);
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data)
+{
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (I2Cdev_readWord(devAddr, regAddr, &w, 100) != 0)
+    {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return I2Cdev_writeWord(devAddr, regAddr, w);
+    }
+    else
+    {
+        return 0;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data)
+{
+    return I2Cdev_writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data)
+{
+    return I2Cdev_writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* pData)
+{
+    HAL_StatusTypeDef status = HAL_I2C_Mem_Write(I2Cdev_hi2c, devAddr << 1, regAddr, I2C_MEMADD_SIZE_8BIT, pData, length, 1000);
+    return status == HAL_OK;
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+uint16_t I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* pData)
+{
+    HAL_StatusTypeDef status = HAL_I2C_Mem_Write(I2Cdev_hi2c, devAddr << 1, regAddr, I2C_MEMADD_SIZE_8BIT, (uint8_t *)pData, sizeof(uint16_t) * length, 1000);
+    return status == HAL_OK;
+}
diff --git a/STM32HAL/I2Cdev/I2Cdev.h b/STM32HAL/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..4302a63f
--- /dev/null
+++ b/STM32HAL/I2Cdev/I2Cdev.h
@@ -0,0 +1,75 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 6/6/2015 by Andrey Voloshin 
+//
+// Changelog:
+//      2015-06-06 - ported to STM32 HAL library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#include 
+#include 
+#include 
+
+// TODO: include depending on chip family
+//#include "stm32f1xx_hal.h"
+//#include "stm32f2xx_hal.h"
+//#include "stm32f3xx_hal.h"
+#include "stm32f4xx_hal.h"
+
+typedef int bool;
+#define true 1
+#define false 0
+
+extern uint16_t I2Cdev_readTimeout;
+
+// 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];")
+#define I2CDEV_DEFAULT_READ_TIMEOUT     1000
+
+void I2Cdev_init(I2C_HandleTypeDef * hi2c);
+
+uint8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout);
+uint8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout);
+uint8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout);
+uint8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout);
+uint8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout);
+uint8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout);
+uint8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout);
+uint8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout);
+
+uint16_t I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+uint16_t I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+uint16_t I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+uint16_t I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+uint16_t I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+uint16_t I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+uint16_t I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+uint16_t I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+#endif /* _I2CDEV_H_ */
diff --git a/STM32HAL/MPU6050/MPU6050.c b/STM32HAL/MPU6050/MPU6050.c
new file mode 100644
index 00000000..66e7daf1
--- /dev/null
+++ b/STM32HAL/MPU6050/MPU6050.c
@@ -0,0 +1,3150 @@
+// 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 
+// 6/6/2015 by Andrey Voloshin 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+
+// 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"
+
+static uint8_t devAddr;
+static uint8_t buffer[14];
+
+
+/** Valid addresses
+ *  MPU6050_ADDRESS_AD0_LOW
+ *  MPU6050_ADDRESS_AD0_HIGH
+ */
+
+void MPU6050_setAddress(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() {
+    devAddr = MPU6050_ADDRESS_AD0_LOW;
+    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    MPU6050_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 MPU6050_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, 0);
+    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, 0);
+    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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @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, 0);
+    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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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, 0);
+    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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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, 0);
+    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);
+}
+
+// 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, 0);
+    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, 0);
+    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, 0);
+    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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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, 0);
+    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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    return buffer[0];
+}
+/** Set free-fall 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, 0);
+    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, 0);
+    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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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 MPU6050_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, 0);
+    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, 0);
+    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, 0);
+    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 MPU6050_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, 0);
+    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, 0);
+    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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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 MPU6050_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, 0);
+    return buffer[0];
+}
+/** Set FIFO MPU6050_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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    return buffer[0];
+}
+/** Get FIFO MPU6050_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, 0);
+    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, 0);
+    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, 0);
+    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) {
+    MPU6050_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, 0);
+    *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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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, 0);
+    *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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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, 0);
+    *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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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, 0);
+    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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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, 0);
+    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 MPU6050_buffer is disabled. The FIFO MPU6050_buffer
+ * cannot be written to or read from while disabled. The FIFO MPU6050_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, 0);
+    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, 0);
+    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 MPU6050_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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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 MPU6050_buffer size.
+ * This value indicates the number of bytes stored in the FIFO MPU6050_buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO MPU6050_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 MPU6050_buffer size
+ */
+uint16_t MPU6050_getFIFOCount() {
+    I2Cdev_readBytes(devAddr, MPU6050_RA_FIFO_COUNTH, 2, buffer, 0);
+    return (((uint16_t)buffer[0]) << 8) | buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO MPU6050_buffer.
+ * This register is used to read and write data from the FIFO MPU6050_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 MPU6050_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 MPU6050_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 MPU6050_buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO MPU6050_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 MPU6050_buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO MPU6050_buffer
+ */
+uint8_t MPU6050_getFIFOByte() {
+    I2Cdev_readByte(devAddr, MPU6050_RA_FIFO_R_W, buffer, 0);
+    return buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev_readBytes(devAddr, MPU6050_RA_FIFO_R_W, length, data, 0);
+}
+/** Write byte to FIFO MPU6050_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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    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, 0);
+    return buffer[0];
+}
+bool MPU6050_getDMPInt4Status() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer, 0);
+    return buffer[0];
+}
+bool MPU6050_getDMPInt3Status() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, buffer, 0);
+    return buffer[0];
+}
+bool MPU6050_getDMPInt2Status() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer, 0);
+    return buffer[0];
+}
+bool MPU6050_getDMPInt1Status() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, buffer, 0);
+    return buffer[0];
+}
+bool MPU6050_getDMPInt0Status() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, buffer, 0);
+    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, 0);
+    return buffer[0];
+}
+bool MPU6050_getIntDMPStatus() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, buffer, 0);
+    return buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_getDMPEnabled() {
+    I2Cdev_readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer, 0);
+    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, 0);
+    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) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint16_t i;
+    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;
+
+        // read the chunk of data as specified
+        I2Cdev_readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, data + i, 0);
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+        }
+    }
+}
+bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer;
+    uint8_t *progBuffer;
+    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) {
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+            I2Cdev_readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer, 0);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                /*Serial.print("Block write verification error, bank ");
+                Serial.print(bank, DEC);
+                Serial.print(", address ");
+                Serial.print(address, DEC);
+                Serial.print("!\nExpected:");
+                for (j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (progBuffer[j] < 16) Serial.print("0");
+                    Serial.print(progBuffer[j], HEX);
+                }
+                Serial.print("\nReceived:");
+                for (uint8_t j = 0; j < chunkSize; j++) {
+                    Serial.print(" 0x");
+                    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                    Serial.print(verifyBuffer[i + j], HEX);
+                }
+                Serial.print("\n");*/
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_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 MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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 = MPU6050_writeMemoryBlock(progBuffer, length, bank, offset, true, false);
+            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 MPU6050_writeDMPConfigurationSet(data, dataSize, true);
+}
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_getDMPConfig1() {
+    I2Cdev_readByte(devAddr, MPU6050_RA_DMP_CFG_1, buffer, 0);
+    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, 0);
+    return buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8_t config) {
+    I2Cdev_writeByte(devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
diff --git a/STM32HAL/MPU6050/MPU6050.h b/STM32HAL/MPU6050/MPU6050.h
new file mode 100644
index 00000000..6bb456c6
--- /dev/null
+++ b/STM32HAL/MPU6050/MPU6050.h
@@ -0,0 +1,989 @@
+// 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 
+// 6/6/2015 by Andrey Voloshin 
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     2015-06-06 - ported to STM32 HAL library from Arduino code
+
+// 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 "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
+#ifndef __arm__
+#include 
+#else
+#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
+#endif
+
+
+#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_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_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
+void MPU6050_setAddress(uint8_t address);
+void MPU6050_initialize();
+bool MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8_t MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8_t level);
+
+// SMPLRT_DIV register
+uint8_t MPU6050_getRate();
+void MPU6050_setRate(uint8_t rate);
+
+// CONFIG register
+uint8_t MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8_t sync);
+uint8_t MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8_t bandwidth);
+
+// GYRO_CONFIG register
+uint8_t MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8_t range);
+
+// ACCEL_CONFIG register
+bool MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+bool MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+bool MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8_t MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8_t range);
+uint8_t MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8_t mode);
+
+// FF_THR register
+uint8_t MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8_t threshold);
+
+// FF_DUR register
+uint8_t MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8_t duration);
+
+// MOT_THR register
+uint8_t MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8_t threshold);
+
+// MOT_DUR register
+uint8_t MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8_t duration);
+
+// ZRMOT_THR register
+uint8_t MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8_t threshold);
+
+// ZRMOT_DUR register
+uint8_t MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8_t duration);
+
+// FIFO_EN register
+bool MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+bool MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+bool MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+bool MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+bool MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+bool MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+bool MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+bool MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+bool MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+bool MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+bool MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+bool MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8_t MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8_t speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8_t MPU6050_getSlaveAddress(uint8_t num);
+void MPU6050_setSlaveAddress(uint8_t num, uint8_t address);
+uint8_t MPU6050_getSlaveRegister(uint8_t num);
+void MPU6050_setSlaveRegister(uint8_t num, uint8_t reg);
+bool MPU6050_getSlaveEnabled(uint8_t num);
+void MPU6050_setSlaveEnabled(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWordByteSwap(uint8_t num);
+void MPU6050_setSlaveWordByteSwap(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWriteMode(uint8_t num);
+void MPU6050_setSlaveWriteMode(uint8_t num, bool mode);
+bool MPU6050_getSlaveWordGroupOffset(uint8_t num);
+void MPU6050_setSlaveWordGroupOffset(uint8_t num, bool enabled);
+uint8_t MPU6050_getSlaveDataLength(uint8_t num);
+void MPU6050_setSlaveDataLength(uint8_t num, uint8_t length);
+
+// I2C_SLV* registers (Slave 4)
+uint8_t MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8_t address);
+uint8_t MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8_t reg);
+void MPU6050_setSlave4OutputByte(uint8_t data);
+bool MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+bool MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+bool MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8_t MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8_t delay);
+uint8_t MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+bool MPU6050_getPassthroughStatus();
+bool MPU6050_getSlave4IsDone();
+bool MPU6050_getLostArbitration();
+bool MPU6050_getSlave4Nack();
+bool MPU6050_getSlave3Nack();
+bool MPU6050_getSlave2Nack();
+bool MPU6050_getSlave1Nack();
+bool MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+bool MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+bool MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+bool MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+bool MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+bool MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+bool MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+bool MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+bool MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8_t MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8_t enabled);
+bool MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+bool MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+bool MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+bool MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+bool MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+bool MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8_t MPU6050_getIntStatus();
+bool MPU6050_getIntFreefallStatus();
+bool MPU6050_getIntMotionStatus();
+bool MPU6050_getIntZeroMotionStatus();
+bool MPU6050_getIntFIFOBufferOverflowStatus();
+bool MPU6050_getIntI2CMasterStatus();
+bool MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getAccelerationX();
+int16_t MPU6050_getAccelerationY();
+int16_t MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16_t MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getRotationX();
+int16_t MPU6050_getRotationY();
+int16_t MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8_t MPU6050_getExternalSensorByte(int position);
+uint16_t MPU6050_getExternalSensorWord(int position);
+uint32_t MPU6050_getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+uint8_t MPU6050_getMotionStatus();
+bool MPU6050_getXNegMotionDetected();
+bool MPU6050_getXPosMotionDetected();
+bool MPU6050_getYNegMotionDetected();
+bool MPU6050_getYPosMotionDetected();
+bool MPU6050_getZNegMotionDetected();
+bool MPU6050_getZPosMotionDetected();
+bool MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8_t num, uint8_t data);
+
+// I2C_MST_DELAY_CTRL register
+bool MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+bool MPU6050_getSlaveDelayEnabled(uint8_t num);
+void MPU6050_setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8_t MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8_t delay);
+uint8_t MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8_t decrement);
+uint8_t MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8_t decrement);
+
+// USER_CTRL register
+bool MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+bool MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+bool MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+bool MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+bool MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8_t MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8_t source);
+
+// PWR_MGMT_2 register
+uint8_t MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8_t frequency);
+bool MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+bool MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+bool MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+bool MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+bool MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+bool MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16_t MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8_t MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8_t data);
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length);
+
+// WHO_AM_I register
+uint8_t MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8_t id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8_t MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8_t MPU6050_getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8_t offset);
+
+// YG_OFFS_TC register
+int8_t MPU6050_getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8_t offset);
+
+// ZG_OFFS_TC register
+int8_t MPU6050_getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8_t offset);
+
+// X_FINE_GAIN register
+int8_t MPU6050_getXFineGain();
+void MPU6050_setXFineGain(int8_t gain);
+
+// Y_FINE_GAIN register
+int8_t MPU6050_getYFineGain();
+void MPU6050_setYFineGain(int8_t gain);
+
+// Z_FINE_GAIN register
+int8_t MPU6050_getZFineGain();
+void MPU6050_setZFineGain(int8_t gain);
+
+// XA_OFFS_* registers
+int16_t MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16_t offset);
+
+// YA_OFFS_* register
+int16_t MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16_t offset);
+
+// ZA_OFFS_* register
+int16_t MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16_t offset);
+
+// XG_OFFS_USR* registers
+int16_t MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16_t offset);
+
+// YG_OFFS_USR* register
+int16_t MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16_t offset);
+
+// ZG_OFFS_USR* register
+int16_t MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16_t offset);
+
+// INT_ENABLE register (DMP functions)
+bool MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+bool MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+bool MPU6050_getDMPInt5Status();
+bool MPU6050_getDMPInt4Status();
+bool MPU6050_getDMPInt3Status();
+bool MPU6050_getDMPInt2Status();
+bool MPU6050_getDMPInt1Status();
+bool MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+bool MPU6050_getIntPLLReadyStatus();
+bool MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+bool MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank);
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8_t address);
+
+// MEM_R_W register
+uint8_t MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8_t data);
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address);
+bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem);
+bool MPU6050_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify);
+
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem);
+bool MPU6050_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
+
+// DMP_CFG_1 register
+uint8_t MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8_t config);
+
+// DMP_CFG_2 register
+uint8_t MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8_t config);
+
+// special methods for MotionApps 2.0 implementation
+#ifdef MPU6050_INCLUDE_DMP_MOTIONAPPS20
+uint8_t *MPU6050_dmpPacketBuffer;
+uint16_t MPU6050_dmpPacketSize;
+
+uint8_t MPU6050_dmpInitialize();
+bool MPU6050_dmpPacketAvailable();
+
+uint8_t MPU6050_dmpSetFIFORate(uint8_t fifoRate);
+uint8_t MPU6050_dmpGetFIFORate();
+uint8_t MPU6050_dmpGetSampleStepSizeMS();
+uint8_t MPU6050_dmpGetSampleFrequency();
+int32_t MPU6050_dmpDecodeTemperature(int8_t tempReg);
+
+// Register callbacks after a packet of FIFO data is processed
+//uint8_t MPU6050_dmpGetRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050_dmpGetUnregisterFIFORateProcess(inv_obj_func func);
+uint8_t MPU6050_dmpRunFIFORateProcesses();
+
+// Setup FIFO for various output
+uint8_t MPU6050_dmpSendQuaternion(uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendPacketNumber(uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+// Get Fixed Point data from FIFO
+uint8_t MPU6050_dmpGetAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetSetLinearAccelFilterCoefficient(float coef);
+uint8_t MPU6050_dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetControlData(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(VectorFloat *v, Quaternion *q);
+uint8_t MPU6050_dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetEIS(int32_t *data, const uint8_t* packet=0);
+
+uint8_t MPU6050_dmpGetEuler(float *data, Quaternion *q);
+uint8_t MPU6050_dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+// Get Floating Point data from FIFO
+uint8_t MPU6050_dmpGetAccelFloat(float *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
+
+uint8_t MPU6050_dmpGetProcessFIFOPacket(const unsigned char *dmpData);
+uint8_t MPU6050_dmpGetReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
+
+uint8_t MPU6050_dmpGetSetFIFOProcessedCallback(void (*func) (void));
+
+uint8_t MPU6050_dmpGetInitFIFOParam();
+uint8_t MPU6050_dmpGetCloseFIFO();
+uint8_t MPU6050_dmpGetSetGyroDataSource(uint8_t source);
+uint8_t MPU6050_dmpGetDecodeQuantizedAccel();
+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 MPU6050_dmpGetInitialize();
+bool dmpPacketAvailable();
+
+uint8_t MPU6050_dmpGetSetFIFORate(uint8_t fifoRate);
+uint8_t MPU6050_dmpGetFIFORate();
+uint8_t MPU6050_dmpGetSampleStepSizeMS();
+uint8_t MPU6050_dmpGetSampleFrequency();
+int32_t dmpDecodeTemperature(int8_t tempReg);
+
+// Register callbacks after a packet of FIFO data is processed
+//uint8_t MPU6050_dmpGetRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
+//uint8_t MPU6050_dmpGetUnregisterFIFORateProcess(inv_obj_func func);
+uint8_t MPU6050_dmpGetRunFIFORateProcesses();
+
+// Setup FIFO for various output
+uint8_t MPU6050_dmpGetSendQuaternion(uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendPacketNumber(uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
+uint8_t MPU6050_dmpGetSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
+
+// Get Fixed Point data from FIFO
+uint8_t MPU6050_dmpGetAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetMag(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetSetLinearAccelFilterCoefficient(float coef);
+uint8_t MPU6050_dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetControlData(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetTemperature(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetGravity(VectorFloat *v, Quaternion *q);
+uint8_t MPU6050_dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetEIS(int32_t *data, const uint8_t* packet=0);
+
+uint8_t MPU6050_dmpGetEuler(float *data, Quaternion *q);
+uint8_t MPU6050_dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity);
+
+// Get Floating Point data from FIFO
+uint8_t MPU6050_dmpGetAccelFloat(float *data, const uint8_t* packet=0);
+uint8_t MPU6050_dmpGetQuaternionFloat(float *data, const uint8_t* packet=0);
+
+uint8_t MPU6050_dmpGetProcessFIFOPacket(const unsigned char *dmpData);
+uint8_t MPU6050_dmpGetReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL);
+
+uint8_t MPU6050_dmpGetSetFIFOProcessedCallback(void (*func) (void));
+
+uint8_t MPU6050_dmpGetInitFIFOParam();
+uint8_t MPU6050_dmpGetCloseFIFO();
+uint8_t MPU6050_dmpGetSetGyroDataSource(uint8_t source);
+uint8_t MPU6050_dmpGetDecodeQuantizedAccel();
+uint32_t MPU6050_dmpGetGyroSumOfSquare();
+uint32_t MPU6050_dmpGetAccelSumOfSquare();
+void MPU6050_dmpOverrideQuaternion(long *q);
+uint16_t MPU6050_dmpGetFIFOPacketSize();
+#endif
+
+#endif /* _MPU6050_H_ */
diff --git a/STM32HAL/README.md b/STM32HAL/README.md
new file mode 100644
index 00000000..7947e860
--- /dev/null
+++ b/STM32HAL/README.md
@@ -0,0 +1,95 @@
+# I2C Device Library for STM32
+
+## Devices
+Currently MPU6050 (with DMP), HMC5883L and BMP180/BMP085 are ported (GY-87 board). ADXL345 is coming soon.
+Adding more functions and devices should be straighforward after reading the source code of the driver.
+Porting drivers should be as simple, as renaming public methods from *DRIVER::func()* to *DRIVER_func()* and moving private fields and functions to *.c file
+
+## BMP180 Example for STM32F429I-DISCO board
+```C
+#include "stm32f4xx.h"
+#include "stm32f4xx_hal.h"
+#include 
+#include 
+#include 
+#include "I2Cdev.h"
+#include "BMP085.h"
+
+I2C_HandleTypeDef hi2c3;
+
+int main(void)
+{
+    SystemInit();
+    HAL_Init();
+    
+    GPIO_InitTypeDef GPIO_InitStruct;
+
+    /**I2C3 GPIO Configuration
+    PC9     ------> I2C3_SDA
+    PA8     ------> I2C3_SCL
+    */
+
+    __GPIOA_CLK_ENABLE();
+    __GPIOC_CLK_ENABLE();
+
+    GPIO_InitStruct.Pin = GPIO_PIN_9;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FAST;
+    GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_8;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FAST;
+    GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
+    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+    __I2C3_CLK_ENABLE();
+
+    hi2c3.Instance = I2C3;
+    hi2c3.Init.ClockSpeed = 400000;
+    hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2;
+    hi2c3.Init.OwnAddress1 = 0x10;
+    hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+    hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLED;
+    hi2c3.Init.OwnAddress2 = 0x11;
+    hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLED;
+    hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
+
+    HAL_I2C_Init(&hi2c3);
+
+    I2Cdev_init(&hi2c3); // init of i2cdevlib.  
+    // You can select other i2c device anytime and 
+    // call the same driver functions on other sensors
+
+    while(!BMP085_testConnection()) ;
+
+    BMP085_initialize();
+    char buf[128];
+    while (1)
+    {
+        BMP085_setControl(BMP085_MODE_TEMPERATURE);
+        HAL_Delay(BMP085_getMeasureDelayMilliseconds(BMP085_MODE_TEMPERATURE));
+        float t = BMP085_getTemperatureC();
+
+        BMP085_setControl(BMP085_MODE_PRESSURE_3);
+        HAL_Delay(BMP085_getMeasureDelayMilliseconds(BMP085_MODE_PRESSURE_3));
+        float p = BMP085_getPressure();
+
+        float a = BMP085_getAltitude(p, 101325);
+    }
+}
+
+void SysTick_Handler()
+{
+    HAL_IncTick();
+    HAL_SYSTICK_IRQHandler();
+}
+```
+
+## Licence
+I2Cdev device library code is placed under the MIT license.
+
+_Copyright (c) 2011 Jeff Rowberg. Copyright (c) 2015 Andrey Voloshin._
diff --git a/dsPIC30F/I2Cdev/I2Cdev.c b/dsPIC30F/I2Cdev/I2Cdev.c
new file mode 100644
index 00000000..cc968752
--- /dev/null
+++ b/dsPIC30F/I2Cdev/I2Cdev.c
@@ -0,0 +1,420 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2017 Daichou
+ *
+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 "I2Cdev.h"
+
+/*uint16_t config2;
+uint16_t config1 = (I2C_ON & I2C_IDLE_CON & I2C_CLK_HLD &
+             I2C_IPMI_DIS & I2C_7BIT_ADD &
+             I2C_SLW_DIS & I2C_SM_DIS &
+             I2C_GCALL_DIS & I2C_STR_DIS &
+             I2C_NACK & I2C_ACK_DIS & I2C_RCV_DIS &
+             I2C_STOP_DIS & I2C_RESTART_DIS &
+             I2C_START_DIS);
+*/
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
+    IFS0bits.MI2CIF = 0;
+    IEC0bits.MI2CIE = 0;
+    /*master Start*/
+    StartI2C();
+    /* Clear interrupt flag */
+
+    /* Wait till Start sequence is completed */
+    while(I2CCONbits.SEN);
+
+    /*master send AD+W*/
+    /* Write Slave Address (Write)*/
+    MasterWriteI2C(devAddr << 1 | 0x00);
+
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send Ack*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    //while(I2CSTATbits.ACKSTAT);
+    /*Master send RA*/
+    /* Write Register Address */
+    MasterWriteI2C(regAddr);
+
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master Pause*/
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+    
+    /*Master Start*/
+    StartI2C();
+    /* Wait till Start sequence is completed */
+    while(I2CCONbits.SEN);
+
+    
+    
+    /*Master send AD+R*/
+    /* Write Slave Address (Read)*/
+    MasterWriteI2C(devAddr << 1 | 0x01);
+    
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send Ack*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    data[0] = MasterReadI2C();
+    unsigned int i;
+    for (i = 1 ; i < length ; i++ ){
+        AckI2C();
+        while(I2CCONbits.ACKEN == 1);
+        data[i] = MasterReadI2C();
+    }
+    NotAckI2C();
+    while(I2CCONbits.ACKEN == 1);
+    /*Master Pause*/
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+    return length;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
+    return I2Cdev_readBytes(devAddr, regAddr, 1, data);
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
+    unsigned char Onebyte[100];
+    I2Cdev_readBytes(devAddr,regAddr,length*2,Onebyte);
+    unsigned int i;
+    for (int i = 0 ; i < length ; i++ ){
+        data[i] = Onebyte[i*2] << 8 | Onebyte[i*2+1];
+    }
+    return length;
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
+    return I2Cdev_readWords(devAddr, regAddr, 1, data);
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
+    uint8_t b;
+    uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data) {
+    uint16_t b;
+    uint8_t count = I2Cdev_readWord(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = I2Cdev_readByte(devAddr, regAddr, &b)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = I2Cdev_readWord(devAddr, regAddr, &w)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
+    IFS0bits.MI2CIF = 0;
+    IEC0bits.MI2CIE = 0;
+    /*Master Start*/
+    StartI2C();
+    /* Wait util Start sequence is completed */
+    while(I2CCONbits.SEN);
+    /* Clear interrupt flag */
+    IFS0bits.MI2CIF = 0;
+
+    /*Master send AD+W*/
+    /* Write Slave address (Write)*/
+    MasterWriteI2C(devAddr << 1 | 0x00);
+    /* Wait till address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master send RA*/
+    /* Write Slave address (Write)*/
+    MasterWriteI2C(regAddr);
+    /* Wait till address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master send data*/
+    /* Transmit string of data */
+    uint8_t i;
+    for (i = 0 ; i < length ; i++){
+        MasterWriteI2C(data[i]);
+        /* Wait till address is transmitted */
+        while(I2CSTATbits.TBF);  // 8 clock cycles
+
+        /*Slave send ACK*/
+        while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+        IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    }
+    
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+    return true;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    return I2Cdev_writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
+    unsigned char OneByte[100];
+    unsigned int i;
+    for (i = 0 ; i < length ; i++){
+        OneByte[i*2] = data[i]>>8;
+        OneByte[i*2+1] = data[i] & 0XFF;
+    }
+    I2Cdev_writeBytes(devAddr,regAddr,length*2,OneByte);
+    return true;
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return I2Cdev_writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    I2Cdev_readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return I2Cdev_writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
+    uint16_t w;
+    I2Cdev_readWord(devAddr, regAddr, &w);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return I2Cdev_writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (I2Cdev_readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+		return I2Cdev_writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (I2Cdev_readWord(devAddr, regAddr, &w) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return I2Cdev_writeWord(devAddr, regAddr, w);
+    } else {
+        return false;
+    }
+}
diff --git a/dsPIC30F/I2Cdev/I2Cdev.h b/dsPIC30F/I2Cdev/I2Cdev.h
new file mode 100644
index 00000000..3f8eae11
--- /dev/null
+++ b/dsPIC30F/I2Cdev/I2Cdev.h
@@ -0,0 +1,68 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2017 Daichou
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#ifndef __XC16
+    #error Use XC16 for compiling
+#endif
+
+#include 
+#include 
+#include 
+#include 
+
+#define I2C_DATA_WAIT 152 //(20*I2C_DATA_WAIT*20 - 1) cycle
+
+
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+#endif /* _I2CDEV_H_ */
diff --git a/dsPIC30F/I2Cdev/I2CdevdsPic30F.X/nbproject/project.xml b/dsPIC30F/I2Cdev/I2CdevdsPic30F.X/nbproject/project.xml
new file mode 100644
index 00000000..d3be9e5f
--- /dev/null
+++ b/dsPIC30F/I2Cdev/I2CdevdsPic30F.X/nbproject/project.xml
@@ -0,0 +1,17 @@
+
+
+    com.microchip.mplab.nbide.embedded.makeproject
+    
+        
+            I2CdevdsPic30F
+            231209f4-6ed3-4604-8ec8-3340e931ead8
+            0
+            c
+            
+            h
+            
+            ISO-8859-1
+            
+        
+    
+
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.c b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.c
new file mode 100644
index 00000000..29bd9eb3
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.c
@@ -0,0 +1,436 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2017 Daichou
+ *
+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 "I2Cdev.h"
+
+
+/*uint16_t config2;
+uint16_t config1 = (I2C_ON & I2C_IDLE_CON & I2C_CLK_HLD &
+             I2C_IPMI_DIS & I2C_7BIT_ADD &
+             I2C_SLW_DIS & I2C_SM_DIS &
+             I2C_GCALL_DIS & I2C_STR_DIS &
+             I2C_NACK & I2C_ACK_DIS & I2C_RCV_DIS &
+             I2C_STOP_DIS & I2C_RESTART_DIS &
+             I2C_START_DIS);
+*/
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @return Number of bytes read (-1 indicates failure)
+ */
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
+    //int8_t count = 0;
+    IFS0bits.MI2CIF = 0;
+    IEC0bits.MI2CIE = 0;
+    //IdleI2C();
+    /*master Start*/
+    StartI2C();
+    /* Clear interrupt flag */
+
+    /* Wait till Start sequence is completed */
+    while(I2CCONbits.SEN);
+
+    /*master send AD+W*/
+    /* Write Slave Address (Write)*/
+    MasterWriteI2C(devAddr << 1 | 0x00);
+
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send Ack*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    //while(I2CSTATbits.ACKSTAT);
+    /*Master send RA*/
+    /* Write Register Address */
+    MasterWriteI2C(regAddr);
+
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master Pause*/
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+
+    /*Master Start*/
+    StartI2C();
+    /* Wait till Start sequence is completed */
+    while(I2CCONbits.SEN);
+
+    /*Master send AD+R*/
+    /* Write Slave Address (Read)*/
+    MasterWriteI2C(devAddr << 1 | 0x01);
+    /* Wait until address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send Ack*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Slave send DATA*/
+    //uint16_t flag = MastergetsI2C(length,data,I2C_DATA_WAIT);
+
+    /*Slave send NACK*/
+    //MastergetsI2C(length,data,100);
+    //NotAckI2C();
+
+    data[0] = MasterReadI2C();
+    unsigned int i;
+    for (i = 1 ; i < length ; i++ ){
+        AckI2C();
+        while(I2CCONbits.ACKEN == 1);
+        data[i] = MasterReadI2C();
+    }
+    NotAckI2C();
+    while(I2CCONbits.ACKEN == 1);
+    /*Master Pause*/
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+    //CloseI2C();
+    //IdleI2C();
+    return length;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
+    return I2Cdev_readBytes(devAddr, regAddr, 1, data);
+}
+
+/** Read multiple words from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of words to read
+ * @param data Buffer to store read data in
+ * @return Number of words read (-1 indicates failure)
+ */
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data) {
+    unsigned char Onebyte[100];
+    I2Cdev_readBytes(devAddr,regAddr,length*2,Onebyte);
+    unsigned int i;
+    for (int i = 0 ; i < length ; i++ ){
+        data[i] = Onebyte[i*2] << 8 | Onebyte[i*2+1];
+    }
+    return length;
+}
+
+/** Read single word from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for word value read from device
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data) {
+    return I2Cdev_readWords(devAddr, regAddr, 1, data);
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data) {
+    uint8_t b;
+    uint8_t count = I2Cdev_readByte(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read a single bit from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-15)
+ * @param data Container for single bit value
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data) {
+    uint16_t b;
+    uint8_t count = I2Cdev_readWord(devAddr, regAddr, &b);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = I2Cdev_readByte(devAddr, regAddr, &b)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read multiple bits from a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-15)
+ * @param length Number of bits to read (not more than 16)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
+ */
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data) {
+    // 1101011001101001 read byte
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    //    010           masked
+    //           -> 010 shifted
+    uint8_t count;
+    uint16_t w;
+    if ((count = I2Cdev_readWord(devAddr, regAddr, &w)) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        w &= mask;
+        w >>= (bitStart - length + 1);
+        *data = w;
+    }
+    return count;
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
+    //if (data[length-1] != '\0' && length != 1)return false;
+	//OpenI2C(config1,config2);
+    IFS0bits.MI2CIF = 0;
+    IEC0bits.MI2CIE = 0;
+    //IdleI2C();
+    /*Master Start*/
+    StartI2C();
+    /* Wait util Start sequence is completed */
+    while(I2CCONbits.SEN);
+    /* Clear interrupt flag */
+    IFS0bits.MI2CIF = 0;
+
+    /*Master send AD+W*/
+    /* Write Slave address (Write)*/
+    MasterWriteI2C(devAddr << 1 | 0x00);
+    /* Wait till address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master send RA*/
+    /* Write Slave address (Write)*/
+    MasterWriteI2C(regAddr);
+    /* Wait till address is transmitted */
+    while(I2CSTATbits.TBF);  // 8 clock cycles
+
+    /*Slave send ACK*/
+    while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    while(I2CSTATbits.ACKSTAT);
+
+    /*Master send data*/
+    /* Transmit string of data */
+    //MasterputsI2C(data);
+    uint8_t i;
+    for (i = 0 ; i < length ; i++){
+        MasterWriteI2C(data[i]);
+        /* Wait till address is transmitted */
+        while(I2CSTATbits.TBF);  // 8 clock cycles
+
+        /*Slave send ACK*/
+        while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+        IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    }
+    //while(!IFS0bits.MI2CIF); // Wait for 9th clock cycle
+    //IFS0bits.MI2CIF = 0;     // Clear interrupt flag
+    StopI2C();
+    /* Wait till stop sequence is completed */
+    while(I2CCONbits.PEN);
+    //CloseI2C();
+    //IdleI2C();
+    return true;
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    return I2Cdev_writeBytes(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
+    unsigned char OneByte[100];
+    unsigned int i;
+    for (i = 0 ; i < length ; i++){
+        OneByte[i*2] = data[i]>>8;
+        OneByte[i*2+1] = data[i] & 0XFF;
+    }
+    I2Cdev_writeBytes(devAddr,regAddr,length*2,OneByte);
+    return true;
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return I2Cdev_writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    I2Cdev_readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return I2Cdev_writeByte(devAddr, regAddr, b);
+}
+
+/** write a single bit in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-15)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
+    uint16_t w;
+    I2Cdev_readWord(devAddr, regAddr, &w);
+    w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
+    return I2Cdev_writeWord(devAddr, regAddr, w);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (I2Cdev_readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+		return I2Cdev_writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write multiple bits in a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-15)
+ * @param length Number of bits to write (not more than 16)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
+    //              010 value to write
+    // fedcba9876543210 bit numbers
+    //    xxx           args: bitStart=12, length=3
+    // 0001110000000000 mask word
+    // 1010111110010110 original value (sample)
+    // 1010001110010110 original & ~mask
+    // 1010101110010110 masked | value
+    uint16_t w;
+    if (I2Cdev_readWord(devAddr, regAddr, &w) != 0) {
+        uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        w &= ~(mask); // zero all important bits in existing word
+        w |= data; // combine data with existing word
+        return I2Cdev_writeWord(devAddr, regAddr, w);
+    } else {
+        return false;
+    }
+}
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.h b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.h
new file mode 100644
index 00000000..3f8eae11
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/I2Cdev.h
@@ -0,0 +1,68 @@
+// I2Cdev library collection - Main I2C device class header file
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// 6/9/2012 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Changelog:
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2013 Jeff Rowberg
+Copyright (c) 2014 Marton Sebok
+Copyright (c) 2017 Daichou
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+
+#ifndef __XC16
+    #error Use XC16 for compiling
+#endif
+
+#include 
+#include 
+#include 
+#include 
+
+#define I2C_DATA_WAIT 152 //(20*I2C_DATA_WAIT*20 - 1) cycle
+
+
+int8_t I2Cdev_readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data);
+int8_t I2Cdev_readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data);
+int8_t I2Cdev_readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data);
+int8_t I2Cdev_readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data);
+int8_t I2Cdev_readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data);
+int8_t I2Cdev_readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+int8_t I2Cdev_readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+bool I2Cdev_writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+bool I2Cdev_writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+bool I2Cdev_writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+bool I2Cdev_writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+bool I2Cdev_writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+bool I2Cdev_writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+bool I2Cdev_writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+bool I2Cdev_writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+#endif /* _I2CDEV_H_ */
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.c b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.c
new file mode 100644
index 00000000..a976419e
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.c
@@ -0,0 +1,3588 @@
+// 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 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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"
+
+MPU6050_t mpu6050;
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+void MPU6050(uint8_t address)
+{
+	mpu6050.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()
+{
+    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    MPU6050_setSleepEnabled(false);
+}
+
+/** 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 MPU6050_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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_SMPLRT_DIV, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050_getExternalFrameSync()
+{
+	I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_CONFIG,
+	                MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT,
+	                MPU6050_CFG_DLPF_CFG_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG,
+	                MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG,
+	                 MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// 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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG,
+	               MPU6050_ACONFIG_XA_ST_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG,
+	               MPU6050_ACONFIG_YA_ST_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG,
+	               MPU6050_ACONFIG_ZA_ST_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG,
+	                MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050_getDHPFMode()
+{
+	I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG,
+	                MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_THR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_DUR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_THR, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+/** Set free-fall 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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DUR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_THR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_DUR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getTempFIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getXGyroFIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getYGyroFIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getZGyroFIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getAccelFIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave2FIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave1FIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave0FIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL,
+	               MPU6050_MULT_MST_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL,
+	               MPU6050_WAIT_FOR_ES_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+bool MPU6050_getSlave3FIFOEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL,
+	               MPU6050_SLV_3_FIFO_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL,
+	               MPU6050_I2C_MST_P_NSR_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050_getMasterClockSpeed()
+{
+	I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL,
+	                MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_ADDR + num * 3,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_REG + num * 3,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+	               MPU6050_I2C_SLV_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+	               MPU6050_I2C_SLV_BYTE_SW_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+	               MPU6050_I2C_SLV_REG_DIS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+	               MPU6050_I2C_SLV_GRP_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num * 3,
+	                MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_ADDR, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_REG, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+	               MPU6050_I2C_SLV4_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+	               MPU6050_I2C_SLV4_INT_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+	               MPU6050_I2C_SLV4_REG_DIS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL,
+	                MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_DI, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_PASS_THROUGH_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV4_DONE_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_LOST_ARB_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV4_NACK_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV3_NACK_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV2_NACK_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV1_NACK_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS,
+	               MPU6050_MST_I2C_SLV0_NACK_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_INT_LEVEL_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_INT_OPEN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_LATCH_INT_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_INT_RD_CLEAR_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_FSYNC_INT_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_I2C_BYPASS_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG,
+	               MPU6050_INTCFG_CLKOUT_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+	return mpu6050.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)
+{
+	MPU6050_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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, mpu6050.buffer);
+	*ax = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+	*ay = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+	*az = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
+	*gx = (((int16_t)mpu6050.buffer[8]) << 8) | mpu6050.buffer[9];
+	*gy = (((int16_t)mpu6050.buffer[10]) << 8) | mpu6050.buffer[11];
+	*gz = (((int16_t)mpu6050.buffer[12]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, mpu6050.buffer);
+	*x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+	*y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+	*z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_TEMP_OUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 6, mpu6050.buffer);
+	*x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+	*y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+	*z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_YOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2,
+	                 mpu6050.buffer);
+	return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4,
+	                 mpu6050.buffer);
+	return (((uint32_t)mpu6050.buffer[0]) << 24) | (((uint32_t)mpu6050.buffer[1]) <<
+	        16) | (((uint16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+}
+
+// MOT_DETECT_STATUS register
+
+/** 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_XNEG_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_XPOS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_YNEG_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_YPOS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_ZNEG_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_ZPOS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS,
+	               MPU6050_MOTION_MOT_ZRMOT_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL,
+	               MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+	                MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+	                MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL,
+	                MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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 mpu6050.buffer is disabled. The FIFO mpu6050.buffer
+ * cannot be written to or read from while disabled. The FIFO mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL,
+	               MPU6050_USERCTRL_FIFO_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL,
+	               MPU6050_USERCTRL_I2C_MST_EN_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL,
+	                MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1,
+	               MPU6050_PWR1_TEMP_DIS_BIT, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT,
+	                MPU6050_PWR1_CLKSEL_LENGTH, mpu6050.buffer);
+	return mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2,
+	                MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH,
+	                mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT,
+	               mpu6050.buffer);
+	return mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2,
+	                MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO mpu6050.buffer size.
+ * This value indicates the number of bytes stored in the FIFO mpu6050.buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO mpu6050.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 mpu6050.buffer size
+ */
+uint16_t MPU6050_getFIFOCount()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_COUNTH, 2, mpu6050.buffer);
+	return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO mpu6050.buffer.
+ * This register is used to read and write data from the FIFO mpu6050.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 mpu6050.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 mpu6050.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 mpu6050.buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO mpu6050.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 mpu6050.buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO mpu6050.buffer
+ */
+uint8_t MPU6050_getFIFOByte()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length)
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO mpu6050.buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_setFIFOByte(uint8_t data)
+{
+	I2Cdev_writeByte(mpu6050.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(mpu6050.devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT,
+	                MPU6050_WHO_AM_I_LENGTH, mpu6050.buffer);
+	return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC,
+	               MPU6050_TC_OTP_BNK_VLD_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setOTPBankValid(bool enabled)
+{
+	I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC,
+	                MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050_getXGyroOffsetTC()
+{
+	I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+	                MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setXGyroOffsetTC(int8_t offset)
+{
+	I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+	                MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setYGyroOffsetTC(int8_t offset)
+{
+	I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT,
+	                MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setZGyroOffsetTC(int8_t offset)
+{
+	I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setXFineGain(int8_t gain)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050_getYFineGain()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setYFineGain(int8_t gain)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050_getZFineGain()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setZFineGain(int8_t gain)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050_getXAccelOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXAccelOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050_getYAccelOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYAccelOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050_getZAccelOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZAccelOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050_getXGyroOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXGyroOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050_getYGyroOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYGyroOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050_getZGyroOffset()
+{
+	I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, mpu6050.buffer);
+	return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZGyroOffset(int16_t offset)
+{
+	I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050_getIntPLLReadyEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setIntPLLReadyEnabled(bool enabled)
+{
+	I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	                MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050_getIntDMPEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	               MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setIntDMPEnabled(bool enabled)
+{
+	I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE,
+	                MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050_getDMPInt5Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt4Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt3Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt2Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt1Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt0Status()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT,
+	               mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050_getIntPLLReadyStatus()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+bool MPU6050_getIntDMPStatus()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS,
+	               MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_getDMPEnabled()
+{
+	I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL,
+	               MPU6050_USERCTRL_DMP_EN_BIT, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setDMPEnabled(bool enabled)
+{
+	I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL,
+	                MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_resetDMP()
+{
+	I2Cdev_writeBit(mpu6050.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(mpu6050.devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_setMemoryStartAddress(uint8_t address)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050_readMemoryByte()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_writeMemoryByte(uint8_t data)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank,
+                             uint8_t address)
+{
+	MPU6050_setMemoryBank(bank, false, false);
+	MPU6050_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(mpu6050.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++;
+			MPU6050_setMemoryBank(bank, false, false);
+			MPU6050_setMemoryStartAddress(address);
+		}
+	}
+}
+/*bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer;
+    uint8_t *progBuffer;
+    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(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+            I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                //Serial.print("Block write verification error, bank ");
+                //Serial.print(bank, DEC);
+                //Serial.print(", address ");
+                //Serial.print(address, DEC);
+                //Serial.print("!\nExpected:");
+                //for (j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (progBuffer[j] < 16) Serial.print("0");
+                //    Serial.print(progBuffer[j], HEX);
+                //}
+                //Serial.print("\nReceived:");
+                //for (uint8_t j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                //    Serial.print(verifyBuffer[i + j], HEX);
+                //}
+                Serial.print("\n");
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_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 MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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 = MPU6050_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(mpu6050.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 MPU6050_writeDMPConfigurationSet(data, dataSize, true);
+}*/
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_getDMPConfig1()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig1(uint8_t config)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050_getDMPConfig2()
+{
+	I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, mpu6050.buffer);
+	return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8_t config)
+{
+	I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.h b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.h
new file mode 100644
index 00000000..830760fc
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050.h
@@ -0,0 +1,786 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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 "I2Cdev.h"
+
+#if ((defined MPU6050_INCLUDE_DMP_MOTIONAPPS20) || (defined MPU6050_INCLUDE_DMP_MOTIONAPPS41))
+    #error DMP is not supported yet
+#endif
+
+#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_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_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
+
+typedef struct MPU6050_t {
+    uint8_t devAddr;
+    uint8_t buffer[14];
+} MPU6050_t;
+
+void MPU6050(uint8_t address);
+
+void MPU6050_initialize();
+bool MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8_t MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8_t level);
+
+// SMPLRT_DIV register
+uint8_t MPU6050_getRate();
+void MPU6050_setRate(uint8_t rate);
+
+// CONFIG register
+uint8_t MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8_t sync);
+uint8_t MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8_t bandwidth);
+
+// GYRO_CONFIG register
+uint8_t MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8_t range);
+
+// ACCEL_CONFIG register
+bool MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+bool MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+bool MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8_t MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8_t range);
+uint8_t MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8_t mode);
+
+// FF_THR register
+uint8_t MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8_t threshold);
+
+// FF_DUR register
+uint8_t MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8_t duration);
+
+// MOT_THR register
+uint8_t MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8_t threshold);
+
+// MOT_DUR register
+uint8_t MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8_t duration);
+
+// ZRMOT_THR register
+uint8_t MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8_t threshold);
+
+// ZRMOT_DUR register
+uint8_t MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8_t duration);
+
+// FIFO_EN register
+bool MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+bool MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+bool MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+bool MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+bool MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+bool MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+bool MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+bool MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+bool MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+bool MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+bool MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+bool MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8_t MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8_t speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8_t MPU6050_getSlaveAddress(uint8_t num);
+void MPU6050_setSlaveAddress(uint8_t num, uint8_t address);
+uint8_t MPU6050_getSlaveRegister(uint8_t num);
+void MPU6050_setSlaveRegister(uint8_t num, uint8_t reg);
+bool MPU6050_getSlaveEnabled(uint8_t num);
+void MPU6050_setSlaveEnabled(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWordByteSwap(uint8_t num);
+void MPU6050_setSlaveWordByteSwap(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWriteMode(uint8_t num);
+void MPU6050_setSlaveWriteMode(uint8_t num, bool mode);
+bool MPU6050_getSlaveWordGroupOffset(uint8_t num);
+void MPU6050_setSlaveWordGroupOffset(uint8_t num, bool enabled);
+uint8_t MPU6050_getSlaveDataLength(uint8_t num);
+void MPU6050_setSlaveDataLength(uint8_t num, uint8_t length);
+
+// I2C_SLV* registers (Slave 4)
+uint8_t MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8_t address);
+uint8_t MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8_t reg);
+void MPU6050_setSlave4OutputByte(uint8_t data);
+bool MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+bool MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+bool MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8_t MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8_t delay);
+uint8_t MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+bool MPU6050_getPassthroughStatus();
+bool MPU6050_getSlave4IsDone();
+bool MPU6050_getLostArbitration();
+bool MPU6050_getSlave4Nack();
+bool MPU6050_getSlave3Nack();
+bool MPU6050_getSlave2Nack();
+bool MPU6050_getSlave1Nack();
+bool MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+bool MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+bool MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+bool MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+bool MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+bool MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+bool MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+bool MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+bool MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8_t MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8_t enabled);
+bool MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+bool MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+bool MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+bool MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+bool MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+bool MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8_t MPU6050_getIntStatus();
+bool MPU6050_getIntFreefallStatus();
+bool MPU6050_getIntMotionStatus();
+bool MPU6050_getIntZeroMotionStatus();
+bool MPU6050_getIntFIFOBufferOverflowStatus();
+bool MPU6050_getIntI2CMasterStatus();
+bool MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getAccelerationX();
+int16_t MPU6050_getAccelerationY();
+int16_t MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16_t MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getRotationX();
+int16_t MPU6050_getRotationY();
+int16_t MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8_t MPU6050_getExternalSensorByte(int position);
+uint16_t MPU6050_getExternalSensorWord(int position);
+uint32_t getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+bool MPU6050_getXNegMotionDetected();
+bool MPU6050_getXPosMotionDetected();
+bool MPU6050_getYNegMotionDetected();
+bool MPU6050_getYPosMotionDetected();
+bool MPU6050_getZNegMotionDetected();
+bool MPU6050_getZPosMotionDetected();
+bool MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8_t num, uint8_t data);
+
+// I2C_MST_DELAY_CTRL register
+bool MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+bool MPU6050_getSlaveDelayEnabled(uint8_t num);
+void MPU6050_setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8_t MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8_t delay);
+uint8_t MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8_t decrement);
+uint8_t MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8_t decrement);
+
+// USER_CTRL register
+bool MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+bool MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+bool MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+bool MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+bool MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8_t MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8_t source);
+
+// PWR_MGMT_2 register
+uint8_t MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8_t frequency);
+bool MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+bool MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+bool MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+bool MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+bool MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+bool MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16_t MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8_t MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8_t data);
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length);
+
+// WHO_AM_I register
+uint8_t MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8_t id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8_t MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8_t getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8_t offset);
+
+// YG_OFFS_TC register
+int8_t getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8_t offset);
+
+// ZG_OFFS_TC register
+int8_t getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8_t offset);
+
+// X_FINE_GAIN register
+int8_t getXFineGain();
+void MPU6050_setXFineGain(int8_t gain);
+
+// Y_FINE_GAIN register
+int8_t getYFineGain();
+void MPU6050_setYFineGain(int8_t gain);
+
+// Z_FINE_GAIN register
+int8_t getZFineGain();
+void MPU6050_setZFineGain(int8_t gain);
+
+// XA_OFFS_* registers
+int16_t MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16_t offset);
+
+// YA_OFFS_* register
+int16_t MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16_t offset);
+
+// ZA_OFFS_* register
+int16_t MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16_t offset);
+
+// XG_OFFS_USR* registers
+int16_t MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16_t offset);
+
+// YG_OFFS_USR* register
+int16_t MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16_t offset);
+
+// ZG_OFFS_USR* register
+int16_t MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16_t offset);
+
+// INT_ENABLE register (DMP functions)
+bool MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+bool MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+bool MPU6050_getDMPInt5Status();
+bool MPU6050_getDMPInt4Status();
+bool MPU6050_getDMPInt3Status();
+bool MPU6050_getDMPInt2Status();
+bool MPU6050_getDMPInt1Status();
+bool MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+bool MPU6050_getIntPLLReadyStatus();
+bool MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+bool MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank);
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8_t address);
+
+// MEM_R_W register
+uint8_t MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8_t data);
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address);
+//bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem);
+//bool MPU6050_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify);
+
+//bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem);
+//bool MPU6050_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
+
+// DMP_CFG_1 register
+uint8_t MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8_t config);
+
+// DMP_CFG_2 register
+uint8_t MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8_t config);
+
+#endif /* _MPU6050_H_ */
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050_example_main.c b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050_example_main.c
new file mode 100644
index 00000000..10605f04
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/MPU6050_example_main.c
@@ -0,0 +1,250 @@
+/*
+ * File:   LEDMain.c
+ * Author: tommycc
+ *
+ * Created on March 1, 2017, 3:49 PM
+ */
+
+
+
+// DSPIC30F4013 Configuration Bit Settings
+
+// 'C' source line config statements
+
+// FOSC
+#pragma config FOSFPR = ECIO       // Oscillator (ECIO w/PLL 8x)
+#pragma config FCKSMEN = CSW_FSCM_OFF   // Clock Switching and Monitor (Sw Disabled, Mon Disabled)
+
+// FWDT
+#pragma config FWPSB = WDTPSB_16        // WDT Prescaler B (1:16)
+#pragma config FWPSA = WDTPSA_512       // WDT Prescaler A (1:512)
+#pragma config WDT = WDT_OFF            // Watchdog Timer (Disabled)
+
+// FBORPOR
+#pragma config FPWRT = PWRT_OFF         // POR Timer Value (Timer Disabled)
+#pragma config BODENV = BORV20          // Brown Out Voltage (Reserved)
+#pragma config BOREN = PBOR_OFF         // PBOR Enable (Disabled)
+#pragma config MCLRE = MCLR_EN          // Master Clear Enable (Enabled)
+
+// FGS
+#pragma config GWRP = GWRP_OFF          // General Code Segment Write Protect (Disabled)
+#pragma config GCP = CODE_PROT_OFF      // General Segment Code Protection (Disabled)
+
+// FICD
+#pragma config ICS = ICS_PGD            // Comm Channel Select (Use PGC/EMUC and PGD/EMUD)
+
+// #pragma config statements should precede project file includes.
+// Use project enums instead of #define for ON and OFF.
+
+#include 
+
+
+
+#include
+#include
+#include
+#include"MPU6050.h"
+#include"I2Cdev.h"
+/* Received data is stored in array Buf  */
+char Buf[80];
+char* Receivedddata = Buf;
+int16_t ax, ay, az, gx, gy, gz;
+/* This is UART1 transmit ISR */
+void __attribute__((__interrupt__)) _U2TXInterrupt(void)
+{  
+   IFS1bits.U2TXIF = 0;
+}
+
+/*******************************************************************************/
+//  delay us using for-loop
+/*******************************************************************************/
+void delay_us( unsigned int usec )
+{
+	unsigned int i;
+	//?   ?   ?   ?   ?   ?   ?   ?   ?   ?              //40 MIPS ,
+	for ( i = 0 ; i < usec * 2;
+	        i++ ) {                //for-loop 8Tcy -> 1us -> add two NOP()
+		asm("NOP");
+		asm("NOP");
+	}
+}
+
+/*******************************************************************************/
+// delay ms using Timer 1 interrupt
+/*******************************************************************************/
+void delay_ms( unsigned int msec )
+{
+	int i = 0;
+	for (i = 0; i < msec; i++)
+		delay_us(1000);
+}
+int main(void)
+{
+    TRISCbits.TRISC14 = 0;
+    LATCbits.LATC14 = 1;
+    while(1){
+        /* Data to be transmitted using UART communication module */
+        TRISFbits.TRISF3 = 0;
+        TRISFbits.TRISF2 = 0;
+        char Buffer[80];
+        /* Holds the value of baud register   */
+        unsigned int baudvalue;
+        /* Holds the value of uart config reg */
+        unsigned int U2MODEvalue;
+        /* Holds the information regarding uart
+        TX & RX interrupt modes */
+        unsigned int U2STAvalue;
+        CloseI2C();
+        /* Turn off UART1module */
+        CloseUART2();
+        /* Configure uart1 transmit interrupt */
+        ConfigIntUART2(  UART_TX_INT_DIS & UART_TX_INT_PR2);
+        /* Configure UART1 module to transmit 8 bit data with one stopbit. Also Enable loopback mode  */
+        baudvalue = 15;//129;  //9600
+        U2MODEvalue = UART_EN & UART_IDLE_CON &
+                      UART_DIS_WAKE & UART_DIS_LOOPBACK  &
+                      UART_EN_ABAUD & UART_NO_PAR_8BIT  &
+                      UART_1STOPBIT;
+        U2STAvalue  = UART_INT_TX_BUF_EMPTY  &
+                      UART_TX_PIN_NORMAL &
+                      UART_TX_ENABLE & UART_INT_RX_3_4_FUL &
+                      UART_ADR_DETECT_DIS &
+                      UART_RX_OVERRUN_CLEAR;
+        unsigned int I2C_config1,I2C_config2;
+        I2C_config1 = (I2C_ON & I2C_IDLE_CON & I2C_CLK_HLD &
+                 I2C_IPMI_DIS & I2C_7BIT_ADD &
+                 I2C_SLW_DIS & I2C_SM_DIS &
+                 I2C_GCALL_DIS & I2C_STR_EN &
+                 I2C_ACK & I2C_ACK_EN & I2C_RCV_EN &
+                 I2C_STOP_DIS & I2C_RESTART_EN &
+                 I2C_START_DIS);
+        I2C_config2 = 381;
+        ConfigIntI2C(MI2C_INT_OFF & SI2C_INT_OFF);
+        OpenI2C(I2C_config1,I2C_config2);
+        OpenUART2(U2MODEvalue, U2STAvalue, baudvalue);
+
+        MPU6050(MPU6050_ADDRESS_AD0_LOW);
+        sprintf(Buf,"Initialize MPU6050\n\0");
+        /* Load transmit buffer and transmit the same till null character is encountered */
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+        MPU6050_initialize();
+
+        unsigned char MPU6050_ID = MPU6050_getDeviceID();
+
+        sprintf(Buf,"Testing device connections...\n\0");
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"MPU6050_ID = 0x%X\n",MPU6050_ID);
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+
+        sprintf(Buf,MPU6050_testConnection() ? "MPU6050 connection successful\r\n" :
+            "MPU6050 connection failed\r\n");
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        if (!MPU6050_testConnection())
+            continue;
+
+        sprintf(Buf,"Reading offset\n");
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Xaccel= %d\n",MPU6050_getXAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Yaccel= %d\n",MPU6050_getYAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Zaccel= %d\n",MPU6050_getZAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Xgyro= %d\n",MPU6050_getXGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Ygyro= %d\n",MPU6050_getYGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Zgyro= %d\n",MPU6050_getZGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        MPU6050_setXGyroOffset(220);
+        MPU6050_setYGyroOffset(76);
+        MPU6050_setZGyroOffset(-85);
+
+        sprintf(Buf,"Reading Updated offset\n");
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Xaccel= %d\n",MPU6050_getXAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Yaccel= %d\n",MPU6050_getYAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Zaccel= %d\n",MPU6050_getZAccelOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Xgyro= %d\n",MPU6050_getXGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Ygyro= %d\n",MPU6050_getYGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        sprintf(Buf,"Zgyro= %d\n",MPU6050_getZGyroOffset());
+        putsUART2 ((unsigned int *)Buf);
+        /* Wait for  transmission to complete */
+        while(BusyUART2());
+
+        MPU6050_setSleepEnabled(false);
+        while (true) {
+           // Read raw accel/gyro measurements from device
+            MPU6050_getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
+
+            // Display tab-separated accel/gyro x/y/z values
+            sprintf(Buf,"a/g:\t%d\t%d\t%d\t%d\t%d\t%d\r\n", ax, ay, az, gx, gy, gz);
+            putsUART2 ((unsigned int *)Buf);
+            /* Wait for  transmission to complete */
+            while(BusyUART2());
+            // Blink LED to indicate activity
+            LATCbits.LATC14 ^= 1;
+
+            delay_ms(25);
+            delay_ms(25);
+        }
+        //CloseUART2();
+
+    }
+    return 0;
+}
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/Makefile b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/Makefile
new file mode 100644
index 00000000..fca8e2cc
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/Makefile
@@ -0,0 +1,113 @@
+#
+#  There exist several targets which are by default empty and which can be 
+#  used for execution of your targets. These targets are usually executed 
+#  before and after some main targets. They are: 
+#
+#     .build-pre:              called before 'build' target
+#     .build-post:             called after 'build' target
+#     .clean-pre:              called before 'clean' target
+#     .clean-post:             called after 'clean' target
+#     .clobber-pre:            called before 'clobber' target
+#     .clobber-post:           called after 'clobber' target
+#     .all-pre:                called before 'all' target
+#     .all-post:               called after 'all' target
+#     .help-pre:               called before 'help' target
+#     .help-post:              called after 'help' target
+#
+#  Targets beginning with '.' are not intended to be called on their own.
+#
+#  Main targets can be executed directly, and they are:
+#  
+#     build                    build a specific configuration
+#     clean                    remove built files from a configuration
+#     clobber                  remove all built files
+#     all                      build all configurations
+#     help                     print help mesage
+#  
+#  Targets .build-impl, .clean-impl, .clobber-impl, .all-impl, and
+#  .help-impl are implemented in nbproject/makefile-impl.mk.
+#
+#  Available make variables:
+#
+#     CND_BASEDIR                base directory for relative paths
+#     CND_DISTDIR                default top distribution directory (build artifacts)
+#     CND_BUILDDIR               default top build directory (object files, ...)
+#     CONF                       name of current configuration
+#     CND_ARTIFACT_DIR_${CONF}   directory of build artifact (current configuration)
+#     CND_ARTIFACT_NAME_${CONF}  name of build artifact (current configuration)
+#     CND_ARTIFACT_PATH_${CONF}  path to build artifact (current configuration)
+#     CND_PACKAGE_DIR_${CONF}    directory of package (current configuration)
+#     CND_PACKAGE_NAME_${CONF}   name of package (current configuration)
+#     CND_PACKAGE_PATH_${CONF}   path to package (current configuration)
+#
+# NOCDDL
+
+
+# Environment 
+MKDIR=mkdir
+CP=cp
+CCADMIN=CCadmin
+RANLIB=ranlib
+
+
+# build
+build: .build-post
+
+.build-pre:
+# Add your pre 'build' code here...
+
+.build-post: .build-impl
+# Add your post 'build' code here...
+
+
+# clean
+clean: .clean-post
+
+.clean-pre:
+# Add your pre 'clean' code here...
+# WARNING: the IDE does not call this target since it takes a long time to
+# simply run make. Instead, the IDE removes the configuration directories
+# under build and dist directly without calling make.
+# This target is left here so people can do a clean when running a clean
+# outside the IDE.
+
+.clean-post: .clean-impl
+# Add your post 'clean' code here...
+
+
+# clobber
+clobber: .clobber-post
+
+.clobber-pre:
+# Add your pre 'clobber' code here...
+
+.clobber-post: .clobber-impl
+# Add your post 'clobber' code here...
+
+
+# all
+all: .all-post
+
+.all-pre:
+# Add your pre 'all' code here...
+
+.all-post: .all-impl
+# Add your post 'all' code here...
+
+
+# help
+help: .help-post
+
+.help-pre:
+# Add your pre 'help' code here...
+
+.help-post: .help-impl
+# Add your post 'help' code here...
+
+
+
+# include project implementation makefile
+include nbproject/Makefile-impl.mk
+
+# include project make variables
+include nbproject/Makefile-variables.mk
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-default.mk b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-default.mk
new file mode 100644
index 00000000..11a69249
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-default.mk
@@ -0,0 +1,188 @@
+#
+# Generated Makefile - do not edit!
+#
+# Edit the Makefile in the project folder instead (../Makefile). Each target
+# has a -pre and a -post target defined where you can add customized code.
+#
+# This makefile implements configuration specific macros and targets.
+
+
+# Include project Makefile
+ifeq "${IGNORE_LOCAL}" "TRUE"
+# do not include local makefile. User is passing all local related variables already
+else
+include Makefile
+# Include makefile containing local settings
+ifeq "$(wildcard nbproject/Makefile-local-default.mk)" "nbproject/Makefile-local-default.mk"
+include nbproject/Makefile-local-default.mk
+endif
+endif
+
+# Environment
+MKDIR=mkdir -p
+RM=rm -f 
+MV=mv 
+CP=cp 
+
+# Macros
+CND_CONF=default
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+IMAGE_TYPE=debug
+OUTPUT_SUFFIX=elf
+DEBUGGABLE_SUFFIX=elf
+FINAL_IMAGE=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+else
+IMAGE_TYPE=production
+OUTPUT_SUFFIX=hex
+DEBUGGABLE_SUFFIX=elf
+FINAL_IMAGE=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+endif
+
+ifeq ($(COMPARE_BUILD), true)
+COMPARISON_BUILD=-mafrlcsj
+else
+COMPARISON_BUILD=
+endif
+
+ifdef SUB_IMAGE_ADDRESS
+SUB_IMAGE_ADDRESS_COMMAND=--image-address $(SUB_IMAGE_ADDRESS)
+else
+SUB_IMAGE_ADDRESS_COMMAND=
+endif
+
+# Object Directory
+OBJECTDIR=build/${CND_CONF}/${IMAGE_TYPE}
+
+# Distribution Directory
+DISTDIR=dist/${CND_CONF}/${IMAGE_TYPE}
+
+# Source Files Quoted if spaced
+SOURCEFILES_QUOTED_IF_SPACED=MPU6050_example_main.c I2Cdev.c MPU6050.c
+
+# Object Files Quoted if spaced
+OBJECTFILES_QUOTED_IF_SPACED=${OBJECTDIR}/MPU6050_example_main.o ${OBJECTDIR}/I2Cdev.o ${OBJECTDIR}/MPU6050.o
+POSSIBLE_DEPFILES=${OBJECTDIR}/MPU6050_example_main.o.d ${OBJECTDIR}/I2Cdev.o.d ${OBJECTDIR}/MPU6050.o.d
+
+# Object Files
+OBJECTFILES=${OBJECTDIR}/MPU6050_example_main.o ${OBJECTDIR}/I2Cdev.o ${OBJECTDIR}/MPU6050.o
+
+# Source Files
+SOURCEFILES=MPU6050_example_main.c I2Cdev.c MPU6050.c
+
+
+CFLAGS=
+ASFLAGS=
+LDLIBSOPTIONS=
+
+############# Tool locations ##########################################
+# If you copy a project from one host to another, the path where the  #
+# compiler is installed may be different.                             #
+# If you open this project with MPLAB X in the new host, this         #
+# makefile will be regenerated and the paths will be corrected.       #
+#######################################################################
+# fixDeps replaces a bunch of sed/cat/printf statements that slow down the build
+FIXDEPS=fixDeps
+
+.build-conf:  ${BUILD_SUBPROJECTS}
+ifneq ($(INFORMATION_MESSAGE), )
+	@echo $(INFORMATION_MESSAGE)
+endif
+	${MAKE}  -f nbproject/Makefile-default.mk dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+
+MP_PROCESSOR_OPTION=30F4013
+MP_LINKER_FILE_OPTION=,--script=p30F4013.gld
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: compile
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+${OBJECTDIR}/MPU6050_example_main.o: MPU6050_example_main.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/MPU6050_example_main.o.d 
+	@${RM} ${OBJECTDIR}/MPU6050_example_main.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  MPU6050_example_main.c  -o ${OBJECTDIR}/MPU6050_example_main.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/MPU6050_example_main.o.d"      -g -D__DEBUG -D__MPLAB_DEBUGGER_PK3=1    -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/MPU6050_example_main.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+${OBJECTDIR}/I2Cdev.o: I2Cdev.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/I2Cdev.o.d 
+	@${RM} ${OBJECTDIR}/I2Cdev.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  I2Cdev.c  -o ${OBJECTDIR}/I2Cdev.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/I2Cdev.o.d"      -g -D__DEBUG -D__MPLAB_DEBUGGER_PK3=1    -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/I2Cdev.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+${OBJECTDIR}/MPU6050.o: MPU6050.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/MPU6050.o.d 
+	@${RM} ${OBJECTDIR}/MPU6050.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  MPU6050.c  -o ${OBJECTDIR}/MPU6050.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/MPU6050.o.d"      -g -D__DEBUG -D__MPLAB_DEBUGGER_PK3=1    -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/MPU6050.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+else
+${OBJECTDIR}/MPU6050_example_main.o: MPU6050_example_main.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/MPU6050_example_main.o.d 
+	@${RM} ${OBJECTDIR}/MPU6050_example_main.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  MPU6050_example_main.c  -o ${OBJECTDIR}/MPU6050_example_main.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/MPU6050_example_main.o.d"        -g -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/MPU6050_example_main.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+${OBJECTDIR}/I2Cdev.o: I2Cdev.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/I2Cdev.o.d 
+	@${RM} ${OBJECTDIR}/I2Cdev.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  I2Cdev.c  -o ${OBJECTDIR}/I2Cdev.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/I2Cdev.o.d"        -g -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/I2Cdev.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+${OBJECTDIR}/MPU6050.o: MPU6050.c  nbproject/Makefile-${CND_CONF}.mk
+	@${MKDIR} "${OBJECTDIR}" 
+	@${RM} ${OBJECTDIR}/MPU6050.o.d 
+	@${RM} ${OBJECTDIR}/MPU6050.o 
+	${MP_CC} $(MP_EXTRA_CC_PRE)  MPU6050.c  -o ${OBJECTDIR}/MPU6050.o  -c -mcpu=$(MP_PROCESSOR_OPTION)  -MMD -MF "${OBJECTDIR}/MPU6050.o.d"        -g -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -O0 -msmart-io=1 -Wall -msfr-warn=off  
+	@${FIXDEPS} "${OBJECTDIR}/MPU6050.o.d" $(SILENT)  -rsi ${MP_CC_DIR}../ 
+	
+endif
+
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: assemble
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+else
+endif
+
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: assemblePreproc
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+else
+endif
+
+# ------------------------------------------------------------------------------------
+# Rules for buildStep: link
+ifeq ($(TYPE_IMAGE), DEBUG_RUN)
+dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}: ${OBJECTFILES}  nbproject/Makefile-${CND_CONF}.mk    
+	@${MKDIR} dist/${CND_CONF}/${IMAGE_TYPE} 
+	${MP_CC} $(MP_EXTRA_LD_PRE)  -o dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}  ${OBJECTFILES_QUOTED_IF_SPACED}      -mcpu=$(MP_PROCESSOR_OPTION)        -D__DEBUG -D__MPLAB_DEBUGGER_PK3=1  -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99  -mreserve=data@0x800:0x81F -mreserve=data@0x820:0x821 -mreserve=data@0x822:0x823 -mreserve=data@0x824:0x84F   -Wl,,,--defsym=__MPLAB_BUILD=1,--defsym=__MPLAB_DEBUG=1,--defsym=__DEBUG=1,--defsym=__MPLAB_DEBUGGER_PK3=1,$(MP_LINKER_FILE_OPTION),--stack=16,--check-sections,--data-init,--pack-data,--handles,--isr,--no-gc-sections,--fill-upper=0,--stackguard=16,--no-force-link,--smart-io,-Map="${DISTDIR}/${PROJECTNAME}.${IMAGE_TYPE}.map",--report-mem,--memorysummary,dist/${CND_CONF}/${IMAGE_TYPE}/memoryfile.xml$(MP_EXTRA_LD_POST) 
+	
+else
+dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}: ${OBJECTFILES}  nbproject/Makefile-${CND_CONF}.mk   
+	@${MKDIR} dist/${CND_CONF}/${IMAGE_TYPE} 
+	${MP_CC} $(MP_EXTRA_LD_PRE)  -o dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${DEBUGGABLE_SUFFIX}  ${OBJECTFILES_QUOTED_IF_SPACED}      -mcpu=$(MP_PROCESSOR_OPTION)        -omf=elf -DXPRJ_default=$(CND_CONF)  -legacy-libc  $(COMPARISON_BUILD)  --std=gnu99 -Wl,,,--defsym=__MPLAB_BUILD=1,$(MP_LINKER_FILE_OPTION),--stack=16,--check-sections,--data-init,--pack-data,--handles,--isr,--no-gc-sections,--fill-upper=0,--stackguard=16,--no-force-link,--smart-io,-Map="${DISTDIR}/${PROJECTNAME}.${IMAGE_TYPE}.map",--report-mem,--memorysummary,dist/${CND_CONF}/${IMAGE_TYPE}/memoryfile.xml$(MP_EXTRA_LD_POST) 
+	${MP_CC_DIR}/xc16-bin2hex dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${DEBUGGABLE_SUFFIX} -a  -omf=elf  
+	
+endif
+
+
+# Subprojects
+.build-subprojects:
+
+
+# Subprojects
+.clean-subprojects:
+
+# Clean Targets
+.clean-conf: ${CLEAN_SUBPROJECTS}
+	${RM} -r build/default
+	${RM} -r dist/default
+
+# Enable dependency checking
+.dep.inc: .depcheck-impl
+
+DEPFILES=$(shell "${PATH_TO_IDE_BIN}"mplabwildcard ${POSSIBLE_DEPFILES})
+ifneq (${DEPFILES},)
+include ${DEPFILES}
+endif
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-genesis.properties b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-genesis.properties
new file mode 100644
index 00000000..b83100c2
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-genesis.properties
@@ -0,0 +1,9 @@
+#
+#Wed Apr 26 15:57:55 CST 2017
+default.com-microchip-mplab-nbide-toolchainXC16-XC16LanguageToolchain.md5=10deee1b358b9eb8b4aaaeb9dce00420
+default.languagetoolchain.dir=/opt/microchip/xc16/v1.31/bin
+configurations-xml=fd1da04a6257bc2861900a03a85b49c1
+com-microchip-mplab-nbide-embedded-makeproject-MakeProject.md5=68c66d83550b716fa3e84319671e802c
+default.languagetoolchain.version=1.31
+host.platform=linux
+conf.ids=default
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-impl.mk b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-impl.mk
new file mode 100644
index 00000000..5b2593af
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-impl.mk
@@ -0,0 +1,69 @@
+#
+# Generated Makefile - do not edit!
+#
+# Edit the Makefile in the project folder instead (../Makefile). Each target
+# has a pre- and a post- target defined where you can add customization code.
+#
+# This makefile implements macros and targets common to all configurations.
+#
+# NOCDDL
+
+
+# Building and Cleaning subprojects are done by default, but can be controlled with the SUB
+# macro. If SUB=no, subprojects will not be built or cleaned. The following macro
+# statements set BUILD_SUB-CONF and CLEAN_SUB-CONF to .build-reqprojects-conf
+# and .clean-reqprojects-conf unless SUB has the value 'no'
+SUB_no=NO
+SUBPROJECTS=${SUB_${SUB}}
+BUILD_SUBPROJECTS_=.build-subprojects
+BUILD_SUBPROJECTS_NO=
+BUILD_SUBPROJECTS=${BUILD_SUBPROJECTS_${SUBPROJECTS}}
+CLEAN_SUBPROJECTS_=.clean-subprojects
+CLEAN_SUBPROJECTS_NO=
+CLEAN_SUBPROJECTS=${CLEAN_SUBPROJECTS_${SUBPROJECTS}}
+
+
+# Project Name
+PROJECTNAME=MPU6050_example.X
+
+# Active Configuration
+DEFAULTCONF=default
+CONF=${DEFAULTCONF}
+
+# All Configurations
+ALLCONFS=default 
+
+
+# build
+.build-impl: .build-pre
+	${MAKE} -f nbproject/Makefile-${CONF}.mk SUBPROJECTS=${SUBPROJECTS} .build-conf
+
+
+# clean
+.clean-impl: .clean-pre
+	${MAKE} -f nbproject/Makefile-${CONF}.mk SUBPROJECTS=${SUBPROJECTS} .clean-conf
+
+# clobber
+.clobber-impl: .clobber-pre .depcheck-impl
+	    ${MAKE} SUBPROJECTS=${SUBPROJECTS} CONF=default clean
+
+
+
+# all
+.all-impl: .all-pre .depcheck-impl
+	    ${MAKE} SUBPROJECTS=${SUBPROJECTS} CONF=default build
+
+
+
+# dependency checking support
+.depcheck-impl:
+#	@echo "# This code depends on make tool being used" >.dep.inc
+#	@if [ -n "${MAKE_VERSION}" ]; then \
+#	    echo "DEPFILES=\$$(wildcard \$$(addsuffix .d, \$${OBJECTFILES}))" >>.dep.inc; \
+#	    echo "ifneq (\$${DEPFILES},)" >>.dep.inc; \
+#	    echo "include \$${DEPFILES}" >>.dep.inc; \
+#	    echo "endif" >>.dep.inc; \
+#	else \
+#	    echo ".KEEP_STATE:" >>.dep.inc; \
+#	    echo ".KEEP_STATE_FILE:.make.state.\$${CONF}" >>.dep.inc; \
+#	fi
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-local-default.mk b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-local-default.mk
new file mode 100644
index 00000000..867c8d36
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-local-default.mk
@@ -0,0 +1,36 @@
+#
+# Generated Makefile - do not edit!
+#
+#
+# This file contains information about the location of compilers and other tools.
+# If you commmit this file into your revision control server, you will be able to 
+# to checkout the project and build it from the command line with make. However,
+# if more than one person works on the same project, then this file might show
+# conflicts since different users are bound to have compilers in different places.
+# In that case you might choose to not commit this file and let MPLAB X recreate this file
+# for each user. The disadvantage of not commiting this file is that you must run MPLAB X at
+# least once so the file gets created and the project can be built. Finally, you can also
+# avoid using this file at all if you are only building from the command line with make.
+# You can invoke make with the values of the macros:
+# $ makeMP_CC="/opt/microchip/mplabc30/v3.30c/bin/pic30-gcc" ...  
+#
+PATH_TO_IDE_BIN=/opt/microchip/mplabx/v3.60/mplab_ide/platform/../mplab_ide/modules/../../bin/
+# Adding MPLAB X bin directory to path.
+PATH:=/opt/microchip/mplabx/v3.60/mplab_ide/platform/../mplab_ide/modules/../../bin/:$(PATH)
+# Path to java used to run MPLAB X when this makefile was created
+MP_JAVA_PATH="/opt/microchip/mplabx/v3.60/sys/java/jre1.8.0_121/bin/"
+OS_CURRENT="$(shell uname -s)"
+MP_CC="/opt/microchip/xc16/v1.31/bin/xc16-gcc"
+# MP_CPPC is not defined
+# MP_BC is not defined
+MP_AS="/opt/microchip/xc16/v1.31/bin/xc16-as"
+MP_LD="/opt/microchip/xc16/v1.31/bin/xc16-ld"
+MP_AR="/opt/microchip/xc16/v1.31/bin/xc16-ar"
+DEP_GEN=${MP_JAVA_PATH}java -jar "/opt/microchip/mplabx/v3.60/mplab_ide/platform/../mplab_ide/modules/../../bin/extractobjectdependencies.jar"
+MP_CC_DIR="/opt/microchip/xc16/v1.31/bin"
+# MP_CPPC_DIR is not defined
+# MP_BC_DIR is not defined
+MP_AS_DIR="/opt/microchip/xc16/v1.31/bin"
+MP_LD_DIR="/opt/microchip/xc16/v1.31/bin"
+MP_AR_DIR="/opt/microchip/xc16/v1.31/bin"
+# MP_BC_DIR is not defined
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-variables.mk b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-variables.mk
new file mode 100644
index 00000000..a8ce072f
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Makefile-variables.mk
@@ -0,0 +1,13 @@
+#
+# Generated - do not edit!
+#
+# NOCDDL
+#
+CND_BASEDIR=`pwd`
+# default configuration
+CND_ARTIFACT_DIR_default=dist/default/production
+CND_ARTIFACT_NAME_default=MPU6050_example.X.production.hex
+CND_ARTIFACT_PATH_default=dist/default/production/MPU6050_example.X.production.hex
+CND_PACKAGE_DIR_default=${CND_DISTDIR}/default/package
+CND_PACKAGE_NAME_default=mpu6050example.x.tar
+CND_PACKAGE_PATH_default=${CND_DISTDIR}/default/package/mpu6050example.x.tar
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Package-default.bash b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Package-default.bash
new file mode 100644
index 00000000..552f66ff
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/Package-default.bash
@@ -0,0 +1,73 @@
+#!/bin/bash -x
+
+#
+# Generated - do not edit!
+#
+
+# Macros
+TOP=`pwd`
+CND_CONF=default
+CND_DISTDIR=dist
+TMPDIR=build/${CND_CONF}/${IMAGE_TYPE}/tmp-packaging
+TMPDIRNAME=tmp-packaging
+OUTPUT_PATH=dist/${CND_CONF}/${IMAGE_TYPE}/MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+OUTPUT_BASENAME=MPU6050_example.X.${IMAGE_TYPE}.${OUTPUT_SUFFIX}
+PACKAGE_TOP_DIR=mpu6050example.x/
+
+# Functions
+function checkReturnCode
+{
+    rc=$?
+    if [ $rc != 0 ]
+    then
+        exit $rc
+    fi
+}
+function makeDirectory
+# $1 directory path
+# $2 permission (optional)
+{
+    mkdir -p "$1"
+    checkReturnCode
+    if [ "$2" != "" ]
+    then
+      chmod $2 "$1"
+      checkReturnCode
+    fi
+}
+function copyFileToTmpDir
+# $1 from-file path
+# $2 to-file path
+# $3 permission
+{
+    cp "$1" "$2"
+    checkReturnCode
+    if [ "$3" != "" ]
+    then
+        chmod $3 "$2"
+        checkReturnCode
+    fi
+}
+
+# Setup
+cd "${TOP}"
+mkdir -p ${CND_DISTDIR}/${CND_CONF}/package
+rm -rf ${TMPDIR}
+mkdir -p ${TMPDIR}
+
+# Copy files and create directories and links
+cd "${TOP}"
+makeDirectory ${TMPDIR}/mpu6050example.x/bin
+copyFileToTmpDir "${OUTPUT_PATH}" "${TMPDIR}/${PACKAGE_TOP_DIR}bin/${OUTPUT_BASENAME}" 0755
+
+
+# Generate tar file
+cd "${TOP}"
+rm -f ${CND_DISTDIR}/${CND_CONF}/package/mpu6050example.x.tar
+cd ${TMPDIR}
+tar -vcf ../../../../${CND_DISTDIR}/${CND_CONF}/package/mpu6050example.x.tar *
+checkReturnCode
+
+# Cleanup
+cd "${TOP}"
+rm -rf ${TMPDIR}
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/configurations.xml b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/configurations.xml
new file mode 100644
index 00000000..6fd8b07f
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/configurations.xml
@@ -0,0 +1,549 @@
+
+
+  
+    
+      I2Cdev.h
+      MPU6050.h
+    
+    
+    
+    
+      MPU6050_example_main.c
+      I2Cdev.c
+      MPU6050.c
+    
+    
+      Makefile
+    
+  
+  Makefile
+  
+    
+      
+        localhost
+        dsPIC30F4013
+        
+        
+        PICkit3PlatformTool
+        XC16
+        1.31
+        2
+      
+      
+        
+          
+          
+        
+        
+        
+        
+          false
+          false
+          
+        
+        
+        
+      
+      
+        false
+        
+        false
+        
+        false
+        false
+        false
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+      
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+        
+      
+    
+  
+
diff --git a/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/project.xml b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/project.xml
new file mode 100644
index 00000000..3230bc50
--- /dev/null
+++ b/dsPIC30F/MPU6050/Examples/MPU6050_example.X/nbproject/project.xml
@@ -0,0 +1,17 @@
+
+
+    com.microchip.mplab.nbide.embedded.makeproject
+    
+        
+            MPU6050_example
+            04e5589e-1a03-434e-ace3-9ef3c9c9d958
+            0
+            c
+            
+            h
+            
+            ISO-8859-1
+            
+        
+    
+
diff --git a/dsPIC30F/MPU6050/MPU6050.c b/dsPIC30F/MPU6050/MPU6050.c
new file mode 100644
index 00000000..2683529a
--- /dev/null
+++ b/dsPIC30F/MPU6050/MPU6050.c
@@ -0,0 +1,3141 @@
+// 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 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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"
+
+MPU6050_t mpu6050;
+
+/** Specific address constructor.
+ * @param address I2C address
+ * @see MPU6050_DEFAULT_ADDRESS
+ * @see MPU6050_ADDRESS_AD0_LOW
+ * @see MPU6050_ADDRESS_AD0_HIGH
+ */
+void MPU6050(uint8_t address) {
+    mpu6050.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() {
+    MPU6050_setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+    MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_250);
+    MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
+    MPU6050_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 MPU6050_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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_PWR_MODE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_SMPLRT_DIV, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 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]
+ * 

+ *
+ * @return FSYNC configuration value
+ */
+uint8_t MPU6050_getExternalFrameSync() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ *          |   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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
+}
+
+// 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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_XA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_YA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ZA_ST_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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.
+ *
+ * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ *    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.
+ * 

+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current high-pass filter configuration
+ * @see MPU6050_DHPF_RESET
+ * @see MPU6050_RA_ACCEL_CONFIG
+ */
+uint8_t MPU6050_getDHPFMode() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_ACCEL_HPF_BIT, MPU6050_ACONFIG_ACCEL_HPF_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_FF_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_THR, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+/** Set free-fall 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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_THR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZRMOT_DUR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current temperature FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getTempFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_TEMP_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope X-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getXGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Y-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getYGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current gyroscope Z-axis FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getZGyroFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current accelerometer FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getAccelFIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 2 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave2FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV2_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 1 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave1FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV1_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 0 FIFO enabled value
+ * @see MPU6050_RA_FIFO_EN
+ */
+bool MPU6050_getSlave0FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_FIFO_EN, MPU6050_SLV0_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_MULT_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_WAIT_FOR_ES_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer.
+ * @return Current Slave 3 FIFO enabled value
+ * @see MPU6050_RA_MST_CTRL
+ */
+bool MPU6050_getSlave3FIFOEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_SLV_3_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_P_NSR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @return Current I2C master clock speed
+ * @see MPU6050_RA_I2C_MST_CTRL
+ */
+uint8_t MPU6050_getMasterClockSpeed() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_I2C_MST_CTRL, MPU6050_I2C_MST_CLK_BIT, MPU6050_I2C_MST_CLK_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_ADDR + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_REG + num*3, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_BYTE_SW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_GRP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV0_CTRL + num*3, MPU6050_I2C_SLV_LEN_BIT, MPU6050_I2C_SLV_LEN_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_ADDR, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_REG, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_REG_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_CTRL, MPU6050_I2C_SLV4_MST_DLY_BIT, MPU6050_I2C_SLV4_MST_DLY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_SLV4_DI, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_PASS_THROUGH_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_DONE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_LOST_ARB_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV4_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV3_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV2_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV1_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_STATUS, MPU6050_MST_I2C_SLV0_NACK_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_OPEN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_LATCH_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_INT_RD_CLEAR_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_FSYNC_INT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_I2C_BYPASS_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_PIN_CFG, MPU6050_INTCFG_CLKOUT_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FF_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_MOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_ZMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_I2C_MST_INT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DATA_RDY_BIT, mpu6050.buffer);
+    return mpu6050.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) {
+    MPU6050_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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 14, mpu6050.buffer);
+    *ax = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *ay = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *az = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.buffer[5];
+    *gx = (((int16_t)mpu6050.buffer[8]) << 8) | mpu6050.buffer[9];
+    *gy = (((int16_t)mpu6050.buffer[10]) << 8) | mpu6050.buffer[11];
+    *gz = (((int16_t)mpu6050.buffer[12]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_ACCEL_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_TEMP_OUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 6, mpu6050.buffer);
+    *x = (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+    *y = (((int16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+    *z = (((int16_t)mpu6050.buffer[4]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_XOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_YOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_GYRO_ZOUT_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.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(mpu6050.devAddr, MPU6050_RA_EXT_SENS_DATA_00 + position, 4, mpu6050.buffer);
+    return (((uint32_t)mpu6050.buffer[0]) << 24) | (((uint32_t)mpu6050.buffer[1]) << 16) | (((uint16_t)mpu6050.buffer[2]) << 8) | mpu6050.buffer[3];
+}
+
+// MOT_DETECT_STATUS register
+
+/** 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_XPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_YPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZNEG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZPOS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_STATUS, MPU6050_MOTION_MOT_ZRMOT_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_I2C_MST_DELAY_CTRL, num, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_ACCEL_ON_DELAY_BIT, MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * FF_COUNT | Counter Decrement
+ * ---------+------------------
+ * 0        | Reset
+ * 1        | 1
+ * 2        | 2
+ * 3        | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_FF_COUNT_BIT, MPU6050_DETECT_FF_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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:
+ *
+ * 
+ * MOT_COUNT | Counter Decrement
+ * ----------+------------------
+ * 0         | Reset
+ * 1         | 1
+ * 2         | 2
+ * 3         | 4
+ * 

+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_MOT_DETECT_CTRL, MPU6050_DETECT_MOT_COUNT_BIT, MPU6050_DETECT_MOT_COUNT_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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 mpu6050.buffer is disabled. The FIFO mpu6050.buffer
+ * cannot be written to or read from while disabled. The FIFO mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_IF_DIS_BIT, enabled);
+}
+/** Reset the FIFO.
+ * This bit resets the FIFO mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CYCLE_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_TEMP_DIS_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, mpu6050.buffer);
+    return mpu6050.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:
+ *
+ * 
+ * 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
+ * 

+ *
+ * @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(mpu6050.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:
+ *
+ * 
+ * LP_WAKE_CTRL | Wake-up Frequency
+ * -------------+------------------
+ * 0            | 1.25 Hz
+ * 1            | 2.5 Hz
+ * 2            | 5 Hz
+ * 3            | 10 Hz
+ * 
+ *
+ * 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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_LP_WAKE_CTRL_BIT, MPU6050_PWR2_LP_WAKE_CTRL_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZA_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_XG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_YG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, mpu6050.buffer);
+    return mpu6050.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(mpu6050.devAddr, MPU6050_RA_PWR_MGMT_2, MPU6050_PWR2_STBY_ZG_BIT, enabled);
+}
+
+// FIFO_COUNT* registers
+
+/** Get current FIFO mpu6050.buffer size.
+ * This value indicates the number of bytes stored in the FIFO mpu6050.buffer. This
+ * number is in turn the number of bytes that can be read from the FIFO mpu6050.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 mpu6050.buffer size
+ */
+uint16_t MPU6050_getFIFOCount() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_COUNTH, 2, mpu6050.buffer);
+    return (((uint16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+
+// FIFO_R_W register
+
+/** Get byte from FIFO mpu6050.buffer.
+ * This register is used to read and write data from the FIFO mpu6050.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 mpu6050.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 mpu6050.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 mpu6050.buffer has overflowed, the oldest data will be lost and new
+ * data will be written to the FIFO.
+ *
+ * If the FIFO mpu6050.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 mpu6050.buffer is not read when
+ * empty.
+ *
+ * @return Byte from FIFO mpu6050.buffer
+ */
+uint8_t MPU6050_getFIFOByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length) {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_FIFO_R_W, length, data);
+}
+/** Write byte to FIFO mpu6050.buffer.
+ * @see getFIFOByte()
+ * @see MPU6050_RA_FIFO_R_W
+ */
+void MPU6050_setFIFOByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.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(mpu6050.devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, mpu6050.buffer);
+    return mpu6050.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(mpu6050.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(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setOTPBankValid(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, enabled);
+}
+int8_t MPU6050_getXGyroOffsetTC() {
+    I2Cdev_readBits(mpu6050.devAddr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZGyroOffsetTC(int8_t offset) {
+    I2Cdev_writeBits(mpu6050.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(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setXFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_X_FINE_GAIN, gain);
+}
+
+// Y_FINE_GAIN register
+
+int8_t MPU6050_getYFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setYFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Y_FINE_GAIN, gain);
+}
+
+// Z_FINE_GAIN register
+
+int8_t MPU6050_getZFineGain() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setZFineGain(int8_t gain) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_Z_FINE_GAIN, gain);
+}
+
+// XA_OFFS_* registers
+
+int16_t MPU6050_getXAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XA_OFFS_H, offset);
+}
+
+// YA_OFFS_* register
+
+int16_t MPU6050_getYAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YA_OFFS_H, offset);
+}
+
+// ZA_OFFS_* register
+
+int16_t MPU6050_getZAccelOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZAccelOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZA_OFFS_H, offset);
+}
+
+// XG_OFFS_USR* registers
+
+int16_t MPU6050_getXGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setXGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_XG_OFFS_USRH, offset);
+}
+
+// YG_OFFS_USR* register
+
+int16_t MPU6050_getYGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setYGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_YG_OFFS_USRH, offset);
+}
+
+// ZG_OFFS_USR* register
+
+int16_t MPU6050_getZGyroOffset() {
+    I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, 2, mpu6050.buffer);
+    return (((int16_t)mpu6050.buffer[0]) << 8) | mpu6050.buffer[1];
+}
+void MPU6050_setZGyroOffset(int16_t offset) {
+    I2Cdev_writeWord(mpu6050.devAddr, MPU6050_RA_ZG_OFFS_USRH, offset);
+}
+
+// INT_ENABLE register (DMP functions)
+
+bool MPU6050_getIntPLLReadyEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntPLLReadyEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled);
+}
+bool MPU6050_getIntDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setIntDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled);
+}
+
+// DMP_INT_STATUS
+
+bool MPU6050_getDMPInt5Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_5_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt4Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt3Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_3_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt2Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt1Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_1_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getDMPInt0Status() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_0_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// INT_STATUS register (DMP functions)
+
+bool MPU6050_getIntPLLReadyStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+bool MPU6050_getIntDMPStatus() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_DMP_INT_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+
+// USER_CTRL register (DMP functions)
+
+bool MPU6050_getDMPEnabled() {
+    I2Cdev_readBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPEnabled(bool enabled) {
+    I2Cdev_writeBit(mpu6050.devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled);
+}
+void MPU6050_resetDMP() {
+    I2Cdev_writeBit(mpu6050.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(mpu6050.devAddr, MPU6050_RA_BANK_SEL, bank);
+}
+
+// MEM_START_ADDR register
+
+void MPU6050_setMemoryStartAddress(uint8_t address) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_START_ADDR, address);
+}
+
+// MEM_R_W register
+
+uint8_t MPU6050_readMemoryByte() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_writeMemoryByte(uint8_t data) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_MEM_R_W, data);
+}
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_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(mpu6050.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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+        }
+    }
+}
+/*bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
+    MPU6050_setMemoryBank(bank, false, false);
+    MPU6050_setMemoryStartAddress(address);
+    uint8_t chunkSize;
+    uint8_t *verifyBuffer;
+    uint8_t *progBuffer;
+    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(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+
+        // verify data if needed
+        if (verify && verifyBuffer) {
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_setMemoryStartAddress(address);
+            I2Cdev_readBytes(mpu6050.devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
+            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
+                //Serial.print("Block write verification error, bank ");
+                //Serial.print(bank, DEC);
+                //Serial.print(", address ");
+                //Serial.print(address, DEC);
+                //Serial.print("!\nExpected:");
+                //for (j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (progBuffer[j] < 16) Serial.print("0");
+                //    Serial.print(progBuffer[j], HEX);
+                //}
+                //Serial.print("\nReceived:");
+                //for (uint8_t j = 0; j < chunkSize; j++) {
+                //    Serial.print(" 0x");
+                //    if (verifyBuffer[i + j] < 16) Serial.print("0");
+                //    Serial.print(verifyBuffer[i + j], HEX);
+                //}
+                Serial.print("\n");
+                free(verifyBuffer);
+                if (useProgMem) free(progBuffer);
+                return false; // uh oh.
+            }
+        }
+
+        // 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++;
+            MPU6050_setMemoryBank(bank, false, false);
+            MPU6050_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 MPU6050_writeMemoryBlock(data, dataSize, bank, address, verify, true);
+}
+bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem) {
+    uint8_t *progBuffer, 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 = MPU6050_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(mpu6050.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 MPU6050_writeDMPConfigurationSet(data, dataSize, true);
+}*/
+
+// DMP_CFG_1 register
+
+uint8_t MPU6050_getDMPConfig1() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig1(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_1, config);
+}
+
+// DMP_CFG_2 register
+
+uint8_t MPU6050_getDMPConfig2() {
+    I2Cdev_readByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, mpu6050.buffer);
+    return mpu6050.buffer[0];
+}
+void MPU6050_setDMPConfig2(uint8_t config) {
+    I2Cdev_writeByte(mpu6050.devAddr, MPU6050_RA_DMP_CFG_2, config);
+}
\ No newline at end of file
diff --git a/dsPIC30F/MPU6050/MPU6050.h b/dsPIC30F/MPU6050/MPU6050.h
new file mode 100644
index 00000000..4f35e336
--- /dev/null
+++ b/dsPIC30F/MPU6050/MPU6050.h
@@ -0,0 +1,786 @@
+// 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)
+// 10/3/2011 by Jeff Rowberg 
+// 11/28/2014 by Marton Sebok 
+//
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+//     ...        - ongoing debug release
+//     2014-11-28 - ported to PIC18 peripheral library from Arduino code
+
+// 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
+Copyright (c) 2014 Marton Sebok
+
+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 "../I2Cdev/I2Cdev.h"
+
+#if ((defined MPU6050_INCLUDE_DMP_MOTIONAPPS20) || (defined MPU6050_INCLUDE_DMP_MOTIONAPPS41))
+    #error DMP is not supported yet
+#endif
+
+#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_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_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
+
+typedef struct MPU6050_t {
+    uint8_t devAddr;
+    uint8_t buffer[14];
+} MPU6050_t;
+
+void MPU6050(uint8_t address);
+
+void MPU6050_initialize();
+bool MPU6050_testConnection();
+
+// AUX_VDDIO register
+uint8_t MPU6050_getAuxVDDIOLevel();
+void MPU6050_setAuxVDDIOLevel(uint8_t level);
+
+// SMPLRT_DIV register
+uint8_t MPU6050_getRate();
+void MPU6050_setRate(uint8_t rate);
+
+// CONFIG register
+uint8_t MPU6050_getExternalFrameSync();
+void MPU6050_setExternalFrameSync(uint8_t sync);
+uint8_t MPU6050_getDLPFMode();
+void MPU6050_setDLPFMode(uint8_t bandwidth);
+
+// GYRO_CONFIG register
+uint8_t MPU6050_getFullScaleGyroRange();
+void MPU6050_setFullScaleGyroRange(uint8_t range);
+
+// ACCEL_CONFIG register
+bool MPU6050_getAccelXSelfTest();
+void MPU6050_setAccelXSelfTest(bool enabled);
+bool MPU6050_getAccelYSelfTest();
+void MPU6050_setAccelYSelfTest(bool enabled);
+bool MPU6050_getAccelZSelfTest();
+void MPU6050_setAccelZSelfTest(bool enabled);
+uint8_t MPU6050_getFullScaleAccelRange();
+void MPU6050_setFullScaleAccelRange(uint8_t range);
+uint8_t MPU6050_getDHPFMode();
+void MPU6050_setDHPFMode(uint8_t mode);
+
+// FF_THR register
+uint8_t MPU6050_getFreefallDetectionThreshold();
+void MPU6050_setFreefallDetectionThreshold(uint8_t threshold);
+
+// FF_DUR register
+uint8_t MPU6050_getFreefallDetectionDuration();
+void MPU6050_setFreefallDetectionDuration(uint8_t duration);
+
+// MOT_THR register
+uint8_t MPU6050_getMotionDetectionThreshold();
+void MPU6050_setMotionDetectionThreshold(uint8_t threshold);
+
+// MOT_DUR register
+uint8_t MPU6050_getMotionDetectionDuration();
+void MPU6050_setMotionDetectionDuration(uint8_t duration);
+
+// ZRMOT_THR register
+uint8_t MPU6050_getZeroMotionDetectionThreshold();
+void MPU6050_setZeroMotionDetectionThreshold(uint8_t threshold);
+
+// ZRMOT_DUR register
+uint8_t MPU6050_getZeroMotionDetectionDuration();
+void MPU6050_setZeroMotionDetectionDuration(uint8_t duration);
+
+// FIFO_EN register
+bool MPU6050_getTempFIFOEnabled();
+void MPU6050_setTempFIFOEnabled(bool enabled);
+bool MPU6050_getXGyroFIFOEnabled();
+void MPU6050_setXGyroFIFOEnabled(bool enabled);
+bool MPU6050_getYGyroFIFOEnabled();
+void MPU6050_setYGyroFIFOEnabled(bool enabled);
+bool MPU6050_getZGyroFIFOEnabled();
+void MPU6050_setZGyroFIFOEnabled(bool enabled);
+bool MPU6050_getAccelFIFOEnabled();
+void MPU6050_setAccelFIFOEnabled(bool enabled);
+bool MPU6050_getSlave2FIFOEnabled();
+void MPU6050_setSlave2FIFOEnabled(bool enabled);
+bool MPU6050_getSlave1FIFOEnabled();
+void MPU6050_setSlave1FIFOEnabled(bool enabled);
+bool MPU6050_getSlave0FIFOEnabled();
+void MPU6050_setSlave0FIFOEnabled(bool enabled);
+
+// I2C_MST_CTRL register
+bool MPU6050_getMultiMasterEnabled();
+void MPU6050_setMultiMasterEnabled(bool enabled);
+bool MPU6050_getWaitForExternalSensorEnabled();
+void MPU6050_setWaitForExternalSensorEnabled(bool enabled);
+bool MPU6050_getSlave3FIFOEnabled();
+void MPU6050_setSlave3FIFOEnabled(bool enabled);
+bool MPU6050_getSlaveReadWriteTransitionEnabled();
+void MPU6050_setSlaveReadWriteTransitionEnabled(bool enabled);
+uint8_t MPU6050_getMasterClockSpeed();
+void MPU6050_setMasterClockSpeed(uint8_t speed);
+
+// I2C_SLV* registers (Slave 0-3)
+uint8_t MPU6050_getSlaveAddress(uint8_t num);
+void MPU6050_setSlaveAddress(uint8_t num, uint8_t address);
+uint8_t MPU6050_getSlaveRegister(uint8_t num);
+void MPU6050_setSlaveRegister(uint8_t num, uint8_t reg);
+bool MPU6050_getSlaveEnabled(uint8_t num);
+void MPU6050_setSlaveEnabled(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWordByteSwap(uint8_t num);
+void MPU6050_setSlaveWordByteSwap(uint8_t num, bool enabled);
+bool MPU6050_getSlaveWriteMode(uint8_t num);
+void MPU6050_setSlaveWriteMode(uint8_t num, bool mode);
+bool MPU6050_getSlaveWordGroupOffset(uint8_t num);
+void MPU6050_setSlaveWordGroupOffset(uint8_t num, bool enabled);
+uint8_t MPU6050_getSlaveDataLength(uint8_t num);
+void MPU6050_setSlaveDataLength(uint8_t num, uint8_t length);
+
+// I2C_SLV* registers (Slave 4)
+uint8_t MPU6050_getSlave4Address();
+void MPU6050_setSlave4Address(uint8_t address);
+uint8_t MPU6050_getSlave4Register();
+void MPU6050_setSlave4Register(uint8_t reg);
+void MPU6050_setSlave4OutputByte(uint8_t data);
+bool MPU6050_getSlave4Enabled();
+void MPU6050_setSlave4Enabled(bool enabled);
+bool MPU6050_getSlave4InterruptEnabled();
+void MPU6050_setSlave4InterruptEnabled(bool enabled);
+bool MPU6050_getSlave4WriteMode();
+void MPU6050_setSlave4WriteMode(bool mode);
+uint8_t MPU6050_getSlave4MasterDelay();
+void MPU6050_setSlave4MasterDelay(uint8_t delay);
+uint8_t MPU6050_getSlate4InputByte();
+
+// I2C_MST_STATUS register
+bool MPU6050_getPassthroughStatus();
+bool MPU6050_getSlave4IsDone();
+bool MPU6050_getLostArbitration();
+bool MPU6050_getSlave4Nack();
+bool MPU6050_getSlave3Nack();
+bool MPU6050_getSlave2Nack();
+bool MPU6050_getSlave1Nack();
+bool MPU6050_getSlave0Nack();
+
+// INT_PIN_CFG register
+bool MPU6050_getInterruptMode();
+void MPU6050_setInterruptMode(bool mode);
+bool MPU6050_getInterruptDrive();
+void MPU6050_setInterruptDrive(bool drive);
+bool MPU6050_getInterruptLatch();
+void MPU6050_setInterruptLatch(bool latch);
+bool MPU6050_getInterruptLatchClear();
+void MPU6050_setInterruptLatchClear(bool clear);
+bool MPU6050_getFSyncInterruptLevel();
+void MPU6050_setFSyncInterruptLevel(bool level);
+bool MPU6050_getFSyncInterruptEnabled();
+void MPU6050_setFSyncInterruptEnabled(bool enabled);
+bool MPU6050_getI2CBypassEnabled();
+void MPU6050_setI2CBypassEnabled(bool enabled);
+bool MPU6050_getClockOutputEnabled();
+void MPU6050_setClockOutputEnabled(bool enabled);
+
+// INT_ENABLE register
+uint8_t MPU6050_getIntEnabled();
+void MPU6050_setIntEnabled(uint8_t enabled);
+bool MPU6050_getIntFreefallEnabled();
+void MPU6050_setIntFreefallEnabled(bool enabled);
+bool MPU6050_getIntMotionEnabled();
+void MPU6050_setIntMotionEnabled(bool enabled);
+bool MPU6050_getIntZeroMotionEnabled();
+void MPU6050_setIntZeroMotionEnabled(bool enabled);
+bool MPU6050_getIntFIFOBufferOverflowEnabled();
+void MPU6050_setIntFIFOBufferOverflowEnabled(bool enabled);
+bool MPU6050_getIntI2CMasterEnabled();
+void MPU6050_setIntI2CMasterEnabled(bool enabled);
+bool MPU6050_getIntDataReadyEnabled();
+void MPU6050_setIntDataReadyEnabled(bool enabled);
+
+// INT_STATUS register
+uint8_t MPU6050_getIntStatus();
+bool MPU6050_getIntFreefallStatus();
+bool MPU6050_getIntMotionStatus();
+bool MPU6050_getIntZeroMotionStatus();
+bool MPU6050_getIntFIFOBufferOverflowStatus();
+bool MPU6050_getIntI2CMasterStatus();
+bool MPU6050_getIntDataReadyStatus();
+
+// ACCEL_*OUT_* registers
+void MPU6050_getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz);
+void MPU6050_getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz);
+void MPU6050_getAcceleration(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getAccelerationX();
+int16_t MPU6050_getAccelerationY();
+int16_t MPU6050_getAccelerationZ();
+
+// TEMP_OUT_* registers
+int16_t MPU6050_getTemperature();
+
+// GYRO_*OUT_* registers
+void MPU6050_getRotation(int16_t* x, int16_t* y, int16_t* z);
+int16_t MPU6050_getRotationX();
+int16_t MPU6050_getRotationY();
+int16_t MPU6050_getRotationZ();
+
+// EXT_SENS_DATA_* registers
+uint8_t MPU6050_getExternalSensorByte(int position);
+uint16_t MPU6050_getExternalSensorWord(int position);
+uint32_t getExternalSensorDWord(int position);
+
+// MOT_DETECT_STATUS register
+bool MPU6050_getXNegMotionDetected();
+bool MPU6050_getXPosMotionDetected();
+bool MPU6050_getYNegMotionDetected();
+bool MPU6050_getYPosMotionDetected();
+bool MPU6050_getZNegMotionDetected();
+bool MPU6050_getZPosMotionDetected();
+bool MPU6050_getZeroMotionDetected();
+
+// I2C_SLV*_DO register
+void MPU6050_setSlaveOutputByte(uint8_t num, uint8_t data);
+
+// I2C_MST_DELAY_CTRL register
+bool MPU6050_getExternalShadowDelayEnabled();
+void MPU6050_setExternalShadowDelayEnabled(bool enabled);
+bool MPU6050_getSlaveDelayEnabled(uint8_t num);
+void MPU6050_setSlaveDelayEnabled(uint8_t num, bool enabled);
+
+// SIGNAL_PATH_RESET register
+void MPU6050_resetGyroscopePath();
+void MPU6050_resetAccelerometerPath();
+void MPU6050_resetTemperaturePath();
+
+// MOT_DETECT_CTRL register
+uint8_t MPU6050_getAccelerometerPowerOnDelay();
+void MPU6050_setAccelerometerPowerOnDelay(uint8_t delay);
+uint8_t MPU6050_getFreefallDetectionCounterDecrement();
+void MPU6050_setFreefallDetectionCounterDecrement(uint8_t decrement);
+uint8_t MPU6050_getMotionDetectionCounterDecrement();
+void MPU6050_setMotionDetectionCounterDecrement(uint8_t decrement);
+
+// USER_CTRL register
+bool MPU6050_getFIFOEnabled();
+void MPU6050_setFIFOEnabled(bool enabled);
+bool MPU6050_getI2CMasterModeEnabled();
+void MPU6050_setI2CMasterModeEnabled(bool enabled);
+void MPU6050_switchSPIEnabled(bool enabled);
+void MPU6050_resetFIFO();
+void MPU6050_resetI2CMaster();
+void MPU6050_resetSensors();
+
+// PWR_MGMT_1 register
+void MPU6050_reset();
+bool MPU6050_getSleepEnabled();
+void MPU6050_setSleepEnabled(bool enabled);
+bool MPU6050_getWakeCycleEnabled();
+void MPU6050_setWakeCycleEnabled(bool enabled);
+bool MPU6050_getTempSensorEnabled();
+void MPU6050_setTempSensorEnabled(bool enabled);
+uint8_t MPU6050_getClockSource();
+void MPU6050_setClockSource(uint8_t source);
+
+// PWR_MGMT_2 register
+uint8_t MPU6050_getWakeFrequency();
+void MPU6050_setWakeFrequency(uint8_t frequency);
+bool MPU6050_getStandbyXAccelEnabled();
+void MPU6050_setStandbyXAccelEnabled(bool enabled);
+bool MPU6050_getStandbyYAccelEnabled();
+void MPU6050_setStandbyYAccelEnabled(bool enabled);
+bool MPU6050_getStandbyZAccelEnabled();
+void MPU6050_setStandbyZAccelEnabled(bool enabled);
+bool MPU6050_getStandbyXGyroEnabled();
+void MPU6050_setStandbyXGyroEnabled(bool enabled);
+bool MPU6050_getStandbyYGyroEnabled();
+void MPU6050_setStandbyYGyroEnabled(bool enabled);
+bool MPU6050_getStandbyZGyroEnabled();
+void MPU6050_setStandbyZGyroEnabled(bool enabled);
+
+// FIFO_COUNT_* registers
+uint16_t MPU6050_getFIFOCount();
+
+// FIFO_R_W register
+uint8_t MPU6050_getFIFOByte();
+void MPU6050_setFIFOByte(uint8_t data);
+void MPU6050_getFIFOBytes(uint8_t *data, uint8_t length);
+
+// WHO_AM_I register
+uint8_t MPU6050_getDeviceID();
+void MPU6050_setDeviceID(uint8_t id);
+
+// ======== UNDOCUMENTED/DMP REGISTERS/METHODS ========
+
+// XG_OFFS_TC register
+uint8_t MPU6050_getOTPBankValid();
+void MPU6050_setOTPBankValid(bool enabled);
+int8_t getXGyroOffsetTC();
+void MPU6050_setXGyroOffsetTC(int8_t offset);
+
+// YG_OFFS_TC register
+int8_t getYGyroOffsetTC();
+void MPU6050_setYGyroOffsetTC(int8_t offset);
+
+// ZG_OFFS_TC register
+int8_t getZGyroOffsetTC();
+void MPU6050_setZGyroOffsetTC(int8_t offset);
+
+// X_FINE_GAIN register
+int8_t getXFineGain();
+void MPU6050_setXFineGain(int8_t gain);
+
+// Y_FINE_GAIN register
+int8_t getYFineGain();
+void MPU6050_setYFineGain(int8_t gain);
+
+// Z_FINE_GAIN register
+int8_t getZFineGain();
+void MPU6050_setZFineGain(int8_t gain);
+
+// XA_OFFS_* registers
+int16_t MPU6050_getXAccelOffset();
+void MPU6050_setXAccelOffset(int16_t offset);
+
+// YA_OFFS_* register
+int16_t MPU6050_getYAccelOffset();
+void MPU6050_setYAccelOffset(int16_t offset);
+
+// ZA_OFFS_* register
+int16_t MPU6050_getZAccelOffset();
+void MPU6050_setZAccelOffset(int16_t offset);
+
+// XG_OFFS_USR* registers
+int16_t MPU6050_getXGyroOffset();
+void MPU6050_setXGyroOffset(int16_t offset);
+
+// YG_OFFS_USR* register
+int16_t MPU6050_getYGyroOffset();
+void MPU6050_setYGyroOffset(int16_t offset);
+
+// ZG_OFFS_USR* register
+int16_t MPU6050_getZGyroOffset();
+void MPU6050_setZGyroOffset(int16_t offset);
+
+// INT_ENABLE register (DMP functions)
+bool MPU6050_getIntPLLReadyEnabled();
+void MPU6050_setIntPLLReadyEnabled(bool enabled);
+bool MPU6050_getIntDMPEnabled();
+void MPU6050_setIntDMPEnabled(bool enabled);
+
+// DMP_INT_STATUS
+bool MPU6050_getDMPInt5Status();
+bool MPU6050_getDMPInt4Status();
+bool MPU6050_getDMPInt3Status();
+bool MPU6050_getDMPInt2Status();
+bool MPU6050_getDMPInt1Status();
+bool MPU6050_getDMPInt0Status();
+
+// INT_STATUS register (DMP functions)
+bool MPU6050_getIntPLLReadyStatus();
+bool MPU6050_getIntDMPStatus();
+
+// USER_CTRL register (DMP functions)
+bool MPU6050_getDMPEnabled();
+void MPU6050_setDMPEnabled(bool enabled);
+void MPU6050_resetDMP();
+
+// BANK_SEL register
+void MPU6050_setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank);
+
+// MEM_START_ADDR register
+void MPU6050_setMemoryStartAddress(uint8_t address);
+
+// MEM_R_W register
+uint8_t MPU6050_readMemoryByte();
+void MPU6050_writeMemoryByte(uint8_t data);
+void MPU6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address);
+//bool MPU6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem);
+//bool MPU6050_writeProgMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify);
+
+//bool MPU6050_writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem);
+//bool MPU6050_writeProgDMPConfigurationSet(const uint8_t *data, uint16_t dataSize);
+
+// DMP_CFG_1 register
+uint8_t MPU6050_getDMPConfig1();
+void MPU6050_setDMPConfig1(uint8_t config);
+
+// DMP_CFG_2 register
+uint8_t MPU6050_getDMPConfig2();
+void MPU6050_setDMPConfig2(uint8_t config);
+
+#endif /* _MPU6050_H_ */
diff --git a/dsPIC30F/README.md b/dsPIC30F/README.md
new file mode 100644
index 00000000..d63bbbe1
--- /dev/null
+++ b/dsPIC30F/README.md
@@ -0,0 +1,14 @@
+# I2C Device Library for dsPIC30F(under construction)
+
+## I2Cdev
+We use XC16's peripheral libraries (_plib_), read timeout is not implemented.
+
+## Devices
+Currently only MPU6050 is supported without DMP functions. There's an example MPLABX project showing how to read raw data from the MPU.
+
+Adding more functions and devices should be straighforward after reading the source code.
+
+## Licence
+I2Cdev device library code is placed under the MIT license.
+
+_Copyright (c) 2011 Jeff Rowberg. Copyright (c) 2014 Marton Sebok._ Copyright (c) 2017 Daichou.
diff --git a/nRF51/I2CDev/I2Cdev.cpp b/nRF51/I2CDev/I2Cdev.cpp
new file mode 100644
index 00000000..c278c017
--- /dev/null
+++ b/nRF51/I2CDev/I2Cdev.cpp
@@ -0,0 +1,255 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+//
+// Nordic Semiconductors nrf51 code by August Bering , tested with MPU6050 only
+// readTimeout not implemented.
+
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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.
+===============================================
+*/
+extern "C"{
+#include "nrf_drv_twi.h"
+#include "app_util_platform.h"
+#include  // For memcpy
+}
+#include "I2Cdev.h"
+static nrf_drv_twi_t m_twi=NRF_DRV_TWI_INSTANCE(0);
+/** Default constructor.
+ */
+I2Cdev::I2Cdev() {
+}
+
+#define SCL_PIN 7
+#define SDA_PIN 30
+
+uint16_t I2Cdev::readTimeout=I2CDEV_DEFAULT_READ_TIMEOUT;
+/** Initialize I2C0
+ */
+void I2Cdev::initialize() {
+	 ret_code_t err_code;
+
+	    const nrf_drv_twi_config_t config = {
+	       .scl                = SCL_PIN,
+	       .sda                = SDA_PIN,
+	       .frequency          = NRF_TWI_FREQ_400K,
+	       .interrupt_priority = APP_IRQ_PRIORITY_HIGH
+	    };
+
+
+	    err_code = nrf_drv_twi_init(&m_twi, &config,NULL, NULL);
+	    APP_ERROR_CHECK(err_code);
+}
+
+/** Enable or disable I2C
+ * @param isEnabled true = enable, false = disable
+ */
+void I2Cdev::enable(bool isEnabled) {
+  
+	if (isEnabled)
+		nrf_drv_twi_enable(&m_twi);
+	else
+		nrf_drv_twi_disable(&m_twi);
+
+}
+
+/** Read a single bit from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitNum Bit position to read (0-7)
+ * @param data Container for single bit value
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
+    uint8_t b;
+    uint8_t count = readByte(devAddr, regAddr, &b, timeout);
+    *data = b & (1 << bitNum);
+    return count;
+}
+
+/** Read multiple bits from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param bitStart First bit position to read (0-7)
+ * @param length Number of bits to read (not more than 8)
+ * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
+    // 01101001 read byte
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    //    010   masked
+    //   -> 010 shifted
+    uint8_t count, b;
+    if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        b &= mask;
+        b >>= (bitStart - length + 1);
+        *data = b;
+    }
+    return count;
+}
+
+/** Read single byte from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to read from
+ * @param data Container for byte value read from device
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return Status of read operation (true = success)
+ */
+int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
+    return readBytes(devAddr, regAddr, 1, data, timeout);
+}
+
+/** Read multiple bytes from an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register regAddr to read from
+ * @param length Number of bytes to read
+ * @param data Buffer to store read data in
+ * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
+ * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
+ */
+int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
+
+	//bool transfer_succeeded;
+	    nrf_drv_twi_tx(&m_twi,devAddr,®Addr,1,true);
+	//    transfer_succeeded  = twi_master_transfer(m_device_address, ®ister_address, 1, TWI_DONT_ISSUE_STOP);
+
+	    ret_code_t r= nrf_drv_twi_rx(&m_twi,devAddr,data,length,false);
+
+	    //transfer_succeeded &= twi_master_transfer(m_device_address|TWI_READ_BIT, destination, number_of_bytes, TWI_ISSUE_STOP);
+	    return r==NRF_SUCCESS;
+  
+}
+
+/** write a single bit in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitNum Bit position to write (0-7)
+ * @param value New bit value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
+    uint8_t b;
+    readByte(devAddr, regAddr, &b);
+    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
+    return writeByte(devAddr, regAddr, b);
+}
+
+/** Write multiple bits in an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register regAddr to write to
+ * @param bitStart First bit position to write (0-7)
+ * @param length Number of bits to write (not more than 8)
+ * @param data Right-aligned value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
+    //      010 value to write
+    // 76543210 bit numbers
+    //    xxx   args: bitStart=4, length=3
+    // 00011100 mask byte
+    // 10101111 original value (sample)
+    // 10100011 original & ~mask
+    // 10101011 masked | value
+    uint8_t b;
+    if (readByte(devAddr, regAddr, &b) != 0) {
+        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
+        data <<= (bitStart - length + 1); // shift data into correct position
+        data &= mask; // zero all non-important bits in data
+        b &= ~(mask); // zero all important bits in existing byte
+        b |= data; // combine data with existing byte
+        return writeByte(devAddr, regAddr, b);
+    } else {
+        return false;
+    }
+}
+
+/** Write single byte to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New byte value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
+    uint8_t w2_data[2];
+
+    w2_data[0] = regAddr;
+    w2_data[1] = data;
+    return NRF_SUCCESS==nrf_drv_twi_tx(&m_twi,devAddr,w2_data,2,false);
+}
+
+/** Write multiple bytes to an 8-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of bytes to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
+    const uint8_t buf_len = length+1; // Register address + number of bytes
+    uint8_t tx_buf[buf_len];
+
+    tx_buf[0] = regAddr;
+    memcpy(tx_buf+1, data, length);
+
+    return NRF_SUCCESS == nrf_drv_twi_tx(&m_twi, devAddr, tx_buf, buf_len, true);
+}
+
+/** Write single word to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr Register address to write to
+ * @param data New word value to write
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
+    return writeWords(devAddr, regAddr, 1, &data);
+}
+
+/** Write multiple words to a 16-bit device register.
+ * @param devAddr I2C slave device address
+ * @param regAddr First register address to write to
+ * @param length Number of words to write
+ * @param data Buffer to copy new data from
+ * @return Status of operation (true = success)
+ */
+bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data){
+	if (NRF_SUCCESS!=nrf_drv_twi_tx(&m_twi,devAddr,®Addr,1,true))
+		return false;
+
+	for (int i=0;i>8;
+		d[1]=data[i]&0xff;
+	bool pending = i < length - 1;
+	if (NRF_SUCCESS != nrf_drv_twi_tx(&m_twi, devAddr, d, 2, pending))
+			return false;
+	}
+	return true;
+}
diff --git a/nRF51/I2CDev/I2Cdev.h b/nRF51/I2CDev/I2Cdev.h
new file mode 100644
index 00000000..b4e56f77
--- /dev/null
+++ b/nRF51/I2CDev/I2Cdev.h
@@ -0,0 +1,85 @@
+// I2Cdev library collection - Main I2C device class
+// Abstracts bit and byte I2C R/W functions into a convenient class
+// EFM32 stub port by Nicolas Baldeck 
+// Based on Arduino's I2Cdev by Jeff Rowberg 
+//
+// Changelog:
+//      2015-01-02 - Initial release
+
+
+/* ============================================
+I2Cdev device library code is placed under the MIT license
+Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
+
+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 _I2CDEV_H_
+#define _I2CDEV_H_
+#include 
+#include "compiler_abstraction.h"
+#include "nrf.h"
+#ifdef SOFTDEVICE_PRESENT
+#include "nrf_soc.h"
+#include "app_error.h"
+#endif
+
+
+#define I2C_SDA_PORT gpioPortA
+#define I2C_SDA_PIN 0
+#define I2C_SDA_MODE gpioModeWiredAnd
+#define I2C_SDA_DOUT 1
+
+#define I2C_SCL_PORT gpioPortA
+#define I2C_SCL_PIN 1
+#define I2C_SCL_MODE gpioModeWiredAnd
+#define I2C_SCL_DOUT 1
+
+#define I2CDEV_DEFAULT_READ_TIMEOUT 0
+
+class I2Cdev {
+    public:
+        I2Cdev();
+
+        static void initialize();
+        static void enable(bool isEnabled);
+
+        static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
+        //TODO static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
+
+        static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
+        //TODO static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
+        static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
+        //TODO static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
+        static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
+        static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
+        static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
+        static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
+
+        static uint16_t readTimeout;
+};
+
+#endif /* _I2CDEV_H_ */