diff --git a/pio/lib/I2Cdev/I2Cdev.cpp b/pio/lib/I2Cdev/I2Cdev.cpp new file mode 100644 index 00000000..892274ba --- /dev/null +++ b/pio/lib/I2Cdev/I2Cdev.cpp @@ -0,0 +1,1488 @@ +// 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" + +#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.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.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.6.5+ with I2CDEV_BUILTIN_FASTWIRE implementation is recommended. + #warning This I2Cdev implementation does not support: + #warning - Timeout detection (some Wire requests block forever)*/ + #endif + #endif + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + + //#error The I2CDEV_BUILTIN_FASTWIRE implementation is known to be broken right now. Patience, Iago! + +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + + #ifdef I2CDEV_IMPLEMENTATION_WARNINGS + #warning Using I2CDEV_BUILTIN_NBWIRE implementation may adversely affect interrupt detection. + #warning This I2Cdev implementation does not support: + #warning - Repeated starts conditions + #endif + + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + TwoWire Wire; + +#endif + +/** 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, uint16_t timeout, void *wireObj) { + uint8_t b; + uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj); + *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, void *wireObj) { + uint16_t b; + uint8_t count = readWord(devAddr, regAddr, &b, timeout, wireObj); + *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, 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, wireObj)) != 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, void *wireObj) { + // 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, wireObj)) != 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, void *wireObj) { + return readBytes(devAddr, regAddr, 1, data, timeout, wireObj); +} + +/** 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, void *wireObj) { + return readWords(devAddr, regAddr, 1, data, timeout, wireObj); +} + +/** 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, void *wireObj) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" bytes from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + + #if (I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + + for (; 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 + } + + useWire->endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + + useWire->endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than I2CDEVLIB_WIRE_BUFFER_LENGTH bytes, we have to do it in + // smaller chunks instead of all at once + for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH)); + + for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) { + data[count] = useWire->read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + } + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + uint8_t status = Fastwire::readBuf(devAddr << 1, regAddr, data, length); + if (status == 0) { + count = length; // success + } else { + count = -1; // error + } + + #endif + + // check for timeout + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + 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, uint16_t timeout, void *wireObj) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") reading "); + Serial.print(length, DEC); + Serial.print(" words from 0x"); + Serial.print(regAddr, HEX); + Serial.print("..."); + #endif + + int8_t count = 0; + uint32_t t1 = millis(); + +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE + TwoWire *useWire = &Wire; + if (wireObj) useWire = (TwoWire *)wireObj; + + #if (ARDUINO < 100) + // Arduino v00xx (before v1.0), Wire library + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than 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, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->send(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = useWire->receive() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= useWire->receive(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + useWire->endTransmission(); + } + #elif (ARDUINO == 100) + // Arduino v1.0.0, Wire library + // Adds standardized write() and read() stream methods instead of send() and receive() + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than 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, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = useWire->read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= useWire->read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + useWire->endTransmission(); + } + #elif (ARDUINO > 100) + // Arduino v1.0.1+, Wire library + // Adds official support for repeated start condition, yay! + + // I2C/TWI subsystem uses internal buffer that breaks with large data requests + // so if user requests more than 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, I2CDEVLIB_WIRE_BUFFER_LENGTH)) { + useWire->beginTransmission(devAddr); + useWire->write(regAddr); + useWire->endTransmission(); + useWire->beginTransmission(devAddr); + useWire->requestFrom(devAddr, (uint8_t)(length * 2)); // length=words, this wants bytes + + bool msb = true; // starts with MSB, then LSB + for (; useWire->available() && count < length && (timeout == 0 || millis() - t1 < timeout);) { + if (msb) { + // first byte is bits 15-8 (MSb=15) + data[count] = useWire->read() << 8; + } else { + // second byte is bits 7-0 (LSb=0) + data[count] |= useWire->read(); + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[count], HEX); + if (count + 1 < length) Serial.print(" "); + #endif + count++; + } + msb = !msb; + } + + useWire->endTransmission(); + } + #endif + + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + + // Fastwire library + // no loop required for fastwire + 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++) { + data[i] = (intermediate[2*i] << 8) | intermediate[2*i + 1]; + } + } else { + count = -1; // error + } + + #endif + + if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout + + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(". Done ("); + Serial.print(count, DEC); + Serial.println(" read)."); + #endif + + return count; +} + +/** 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, void *wireObj) { + uint8_t b; + readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj); + b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); + return writeByte(devAddr, regAddr, b, wireObj); +} + +/** 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, void *wireObj) { + uint16_t w; + readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj); + w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); + return writeWord(devAddr, regAddr, w, wireObj); +} + +/** 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, void *wireObj) { + // 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, 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, wireObj); + } 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, void *wireObj) { + // 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, 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, wireObj); + } 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, void *wireObj) { + return writeBytes(devAddr, regAddr, 1, &data, wireObj); +} + +/** 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, void *wireObj) { + return writeWords(devAddr, regAddr, 1, &data, wireObj); +} + +/** 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, void *wireObj) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" bytes to 0x"); + Serial.print(regAddr, HEX); + 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) + useWire->beginTransmission(devAddr); + useWire->send((uint8_t) regAddr); // send address + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->beginTransmission(devAddr); + useWire->write((uint8_t) regAddr); // send address + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::beginTransmission(devAddr); + Fastwire::write(regAddr); + #endif + for (uint8_t i = 0; i < length; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->send((uint8_t) data[i]); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t) data[i]); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t) data[i]); + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** 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, void *wireObj) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print("I2C (0x"); + Serial.print(devAddr, HEX); + Serial.print(") writing "); + Serial.print(length, DEC); + Serial.print(" words to 0x"); + Serial.print(regAddr, HEX); + 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) + 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; i++) { + #ifdef I2CDEV_SERIAL_DEBUG + Serial.print(data[i], HEX); + if (i + 1 < length) Serial.print(" "); + #endif + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->send((uint8_t)(data[i] >> 8)); // send MSB + useWire->send((uint8_t)data[i]); // send LSB + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + useWire->write((uint8_t)(data[i] >> 8)); // send MSB + useWire->write((uint8_t)data[i]); // send LSB + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::write((uint8_t)(data[i] >> 8)); // send MSB + status = Fastwire::write((uint8_t)data[i]); // send LSB + if (status != 0) break; + #endif + } + #if ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO < 100) || I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE) + useWire->endTransmission(); + #elif ((I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE && ARDUINO >= 100) \ + || (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_SBWIRE && ARDUINO >= 100) \ + || I2CDEV_IMPLEMENTATION == I2CDEV_TEENSY_3X_WIRE) + status = useWire->endTransmission(); + #elif (I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE) + Fastwire::stop(); + //status = Fastwire::endTransmission(); + #endif + #ifdef I2CDEV_SERIAL_DEBUG + Serial.println(". Done."); + #endif + return status == 0; +} + +/** Default timeout value for read operations. + * Set this to 0 to disable timeout detection. + */ +uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + // I2C library + ////////////////////// + // Copyright(C) 2012 + // Francesco Ferrara + // ferrara[at]libero[point]it + ////////////////////// + + /* + FastWire + - 0.24 added stop + - 0.23 added reset + + This is a library to help faster programs to read I2C devices. + Copyright(C) 2012 Francesco Ferrara + occhiobello at gmail dot com + [used by Jeff Rowberg for I2Cdevlib with permission] + */ + + boolean Fastwire::waitInt() { + int l = 250; + while (!(TWCR & (1 << TWINT)) && l-- > 0); + return l > 0; + } + + void Fastwire::setup(int khz, boolean pullup) { + TWCR = 0; + #if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__) + // activate internal pull-ups for twi (PORTC bits 4 & 5) + // as per note from atmega8 manual pg167 + if (pullup) PORTC |= ((1 << 4) | (1 << 5)); + else PORTC &= ~((1 << 4) | (1 << 5)); + #elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__) + // activate internal pull-ups for twi (PORTC bits 0 & 1) + if (pullup) PORTC |= ((1 << 0) | (1 << 1)); + else PORTC &= ~((1 << 0) | (1 << 1)); + #else + // activate internal pull-ups for twi (PORTD bits 0 & 1) + // as per note from atmega128 manual pg204 + if (pullup) PORTD |= ((1 << 0) | (1 << 1)); + else PORTD &= ~((1 << 0) | (1 << 1)); + #endif + + TWSR = 0; // no prescaler => prescaler = 1 + TWBR = F_CPU / 2000 / khz - 8; // change the I2C clock rate + TWCR = 1 << TWEN; // enable twi module, no interrupt + } + + // added by Jeff Rowberg 2013-05-07: + // Arduino Wire-style "beginTransmission" function + // (takes 7-bit device address like the Wire method, NOT 8-bit: 0x68, not 0xD0/0xD1) + byte Fastwire::beginTransmission(byte device) { + byte twst, retry; + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device << 1; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + return 0; + } + + byte Fastwire::writeBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 2; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xFE; // send device address without read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 3; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 4; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 5; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 6; + + for (byte i = 0; i < num; i++) { + //Serial.print(data[i], HEX); + //Serial.print(" "); + TWDR = data[i]; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 7; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 8; + } + //Serial.print("\n"); + + return 0; + } + + byte Fastwire::write(byte value) { + byte twst; + //Serial.println(value, HEX); + TWDR = value; // send data + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 1; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 2; + return 0; + } + + byte Fastwire::readBuf(byte device, byte address, byte *data, byte num) { + byte twst, retry; + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 16; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 17; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device & 0xfe; // send device address to write + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 18; + twst = TWSR & 0xF8; + } while (twst == TW_MT_SLA_NACK && retry-- > 0); + if (twst != TW_MT_SLA_ACK) return 19; + + //Serial.print(address, HEX); + //Serial.print(" "); + TWDR = address; // send data to the previously addressed device + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 20; + twst = TWSR & 0xF8; + if (twst != TW_MT_DATA_ACK) return 21; + + /***/ + + retry = 2; + do { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO) | (1 << TWSTA); + if (!waitInt()) return 22; + twst = TWSR & 0xF8; + if (twst != TW_START && twst != TW_REP_START) return 23; + + //Serial.print(device, HEX); + //Serial.print(" "); + TWDR = device | 0x01; // send device address with the read bit (1) + TWCR = (1 << TWINT) | (1 << TWEN); + if (!waitInt()) return 24; + twst = TWSR & 0xF8; + } while (twst == TW_MR_SLA_NACK && retry-- > 0); + if (twst != TW_MR_SLA_ACK) return 25; + + for (uint8_t i = 0; i < num; i++) { + if (i == num - 1) + TWCR = (1 << TWINT) | (1 << TWEN); + else + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA); + if (!waitInt()) return 26; + twst = TWSR & 0xF8; + if (twst != TW_MR_DATA_ACK && twst != TW_MR_DATA_NACK) return twst; + data[i] = TWDR; + //Serial.print(data[i], HEX); + //Serial.print(" "); + } + //Serial.print("\n"); + stop(); + + return 0; + } + + void Fastwire::reset() { + TWCR = 0; + } + + byte Fastwire::stop() { + TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO); + if (!waitInt()) return 1; + return 0; + } +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html + + /* + call this version 1.0 + + Offhand, the only funky part that I can think of is in nbrequestFrom, where the buffer + length and index are set *before* the data is actually read. The problem is that these + are variables local to the TwoWire object, and by the time we actually have read the + data, and know what the length actually is, we have no simple access to the object's + variables. The actual bytes read *is* given to the callback function, though. + + The ISR code for a slave receiver is commented out. I don't have that setup, and can't + verify it at this time. Save it for 2.0! + + The handling of the read and write processes here is much like in the demo sketch code: + the process is broken down into sequential functions, where each registers the next as a + callback, essentially. + + For example, for the Read process, twi_read00 just returns if TWI is not yet in a + ready state. When there's another interrupt, and the interface *is* ready, then it + sets up the read, starts it, and registers twi_read01 as the function to call after + the *next* interrupt. twi_read01, then, just returns if the interface is still in a + "reading" state. When the reading is done, it copies the information to the buffer, + cleans up, and calls the user-requested callback function with the actual number of + bytes read. + + The writing is similar. + + Questions, comments and problems can go to Gene@Telobot.com. + + Thumbs Up! + Gene Knight + + */ + + uint8_t TwoWire::rxBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::rxBufferIndex = 0; + uint8_t TwoWire::rxBufferLength = 0; + + uint8_t TwoWire::txAddress = 0; + uint8_t TwoWire::txBuffer[NBWIRE_BUFFER_LENGTH]; + uint8_t TwoWire::txBufferIndex = 0; + uint8_t TwoWire::txBufferLength = 0; + + //uint8_t TwoWire::transmitting = 0; + void (*TwoWire::user_onRequest)(void); + void (*TwoWire::user_onReceive)(int); + + static volatile uint8_t twi_transmitting; + static volatile uint8_t twi_state; + static uint8_t twi_slarw; + static volatile uint8_t twi_error; + static uint8_t twi_masterBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_masterBufferIndex; + static uint8_t twi_masterBufferLength; + static uint8_t twi_rxBuffer[TWI_BUFFER_LENGTH]; + static volatile uint8_t twi_rxBufferIndex; + //static volatile uint8_t twi_Interrupt_Continue_Command; + static volatile uint8_t twi_Return_Value; + static volatile uint8_t twi_Done; + void (*twi_cbendTransmissionDone)(int); + void (*twi_cbreadFromDone)(int); + + void twi_init() { + // initialize state + twi_state = TWI_READY; + + // activate internal pull-ups for twi + // as per note from atmega8 manual pg167 + sbi(PORTC, 4); + sbi(PORTC, 5); + + // initialize twi prescaler and bit rate + cbi(TWSR, TWPS0); // TWI Status Register - Prescaler bits + cbi(TWSR, TWPS1); + + /* twi bit rate formula from atmega128 manual pg 204 + SCL Frequency = CPU Clock Frequency / (16 + (2 * TWBR)) + note: TWBR should be 10 or higher for master mode + It is 72 for a 16mhz Wiring board with 100kHz TWI */ + + TWBR = ((CPU_FREQ / TWI_FREQ) - 16) / 2; // bitrate register + // enable twi module, acks, and twi interrupt + + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA); + + /* TWEN - TWI Enable Bit + TWIE - TWI Interrupt Enable + TWEA - TWI Enable Acknowledge Bit + TWINT - TWI Interrupt Flag + TWSTA - TWI Start Condition + */ + } + + typedef struct { + uint8_t address; + uint8_t* data; + uint8_t length; + uint8_t wait; + uint8_t i; + } twi_Write_Vars; + + twi_Write_Vars *ptwv = 0; + static void (*fNextInterruptFunction)(void) = 0; + + void twi_Finish(byte bRetVal) { + if (ptwv) { + free(ptwv); + ptwv = 0; + } + twi_Done = 0xFF; + twi_Return_Value = bRetVal; + fNextInterruptFunction = 0; + } + + uint8_t twii_WaitForDone(uint16_t timeout) { + uint32_t endMillis = millis() + timeout; + while (!twi_Done && (timeout == 0 || millis() < endMillis)) continue; + return twi_Return_Value; + } + + void twii_SetState(uint8_t ucState) { + twi_state = ucState; + } + + void twii_SetError(uint8_t ucError) { + twi_error = ucError ; + } + + void twii_InitBuffer(uint8_t ucPos, uint8_t ucLength) { + twi_masterBufferIndex = 0; + twi_masterBufferLength = ucLength; + } + + void twii_CopyToBuf(uint8_t* pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + twi_masterBuffer[i] = pData[i]; + } + } + + void twii_CopyFromBuf(uint8_t *pData, uint8_t ucLength) { + uint8_t i; + for (i = 0; i < ucLength; ++i) { + pData[i] = twi_masterBuffer[i]; + } + } + + void twii_SetSlaRW(uint8_t ucSlaRW) { + twi_slarw = ucSlaRW; + } + + void twii_SetStart() { + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT) | (1 << TWSTA); + } + + void twi_write01() { + if (TWI_MTX == twi_state) return; // blocking test + twi_transmitting = 0 ; + if (twi_error == 0xFF) + twi_Finish (0); // success + else if (twi_error == TW_MT_SLA_NACK) + twi_Finish (2); // error: address send, nack received + else if (twi_error == TW_MT_DATA_NACK) + twi_Finish (3); // error: data send, nack received + else + twi_Finish (4); // other twi error + if (twi_cbendTransmissionDone) return twi_cbendTransmissionDone(twi_Return_Value); + return; + } + + + void twi_write00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) { + twi_Finish(1); // end write with error 1 + return; + } + twi_Done = 0x00; // show as working + twii_SetState(TWI_MTX); // to transmitting + twii_SetError(0xFF); // to No Error + twii_InitBuffer(0, ptwv -> length); // pointer and length + twii_CopyToBuf(ptwv -> data, ptwv -> length); // get the data + twii_SetSlaRW((ptwv -> address << 1) | TW_WRITE); // write command + twii_SetStart(); // start the cycle + fNextInterruptFunction = twi_write01; // next routine + return twi_write01(); + } + + void twi_writeTo(uint8_t address, uint8_t* data, uint8_t length, uint8_t wait) { + uint8_t i; + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + ptwv -> wait = wait; + fNextInterruptFunction = twi_write00; + return twi_write00(); + } + + void twi_read01() { + if (TWI_MRX == twi_state) return; // blocking test + if (twi_masterBufferIndex < ptwv -> length) ptwv -> length = twi_masterBufferIndex; + twii_CopyFromBuf(ptwv -> data, ptwv -> length); + twi_Finish(ptwv -> length); + if (twi_cbreadFromDone) return twi_cbreadFromDone(twi_Return_Value); + return; + } + + void twi_read00() { + if (TWI_READY != twi_state) return; // blocking test + if (TWI_BUFFER_LENGTH < ptwv -> length) twi_Finish(0); // error return + twi_Done = 0x00; // show as working + twii_SetState(TWI_MRX); // reading + twii_SetError(0xFF); // reset error + twii_InitBuffer(0, ptwv -> length - 1); // init to one less than length + twii_SetSlaRW((ptwv -> address << 1) | TW_READ); // read command + twii_SetStart(); // start cycle + fNextInterruptFunction = twi_read01; + return twi_read01(); + } + + void twi_readFrom(uint8_t address, uint8_t* data, uint8_t length) { + uint8_t i; + + ptwv = (twi_Write_Vars *)malloc(sizeof(twi_Write_Vars)); + ptwv -> address = address; + ptwv -> data = data; + ptwv -> length = length; + fNextInterruptFunction = twi_read00; + return twi_read00(); + } + + void twi_reply(uint8_t ack) { + // transmit master read ready signal, with or without ack + if (ack){ + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT) | (1 << TWEA); + } else { + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWINT); + } + } + + void twi_stop(void) { + // send stop condition + 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 & (1 << TWSTO)) { + continue; + } + + // update twi state + twi_state = TWI_READY; + } + + void twi_releaseBus(void) { + // release bus + TWCR = (1 << TWEN) | (1 << TWIE) | (1 << TWEA) | (1 << TWINT); + + // update twi state + twi_state = TWI_READY; + } + + SIGNAL(TWI_vect) { + switch (TW_STATUS) { + // All Master + case TW_START: // sent start condition + case TW_REP_START: // sent repeated start condition + // copy device address and r/w bit to output register and ack + TWDR = twi_slarw; + twi_reply(1); + break; + + // Master Transmitter + case TW_MT_SLA_ACK: // slave receiver acked address + case TW_MT_DATA_ACK: // slave receiver acked data + // if there is data to send, send it, otherwise stop + if (twi_masterBufferIndex < twi_masterBufferLength) { + // copy data to output register and ack + TWDR = twi_masterBuffer[twi_masterBufferIndex++]; + twi_reply(1); + } else { + twi_stop(); + } + break; + + case TW_MT_SLA_NACK: // address sent, nack received + twi_error = TW_MT_SLA_NACK; + twi_stop(); + break; + + case TW_MT_DATA_NACK: // data sent, nack received + twi_error = TW_MT_DATA_NACK; + twi_stop(); + break; + + case TW_MT_ARB_LOST: // lost bus arbitration + twi_error = TW_MT_ARB_LOST; + twi_releaseBus(); + break; + + // Master Receiver + case TW_MR_DATA_ACK: // data received, ack sent + // put byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_ACK: // address sent, ack received + // ack if more bytes are expected, otherwise nack + if (twi_masterBufferIndex < twi_masterBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_MR_DATA_NACK: // data received, nack sent + // put final byte into buffer + twi_masterBuffer[twi_masterBufferIndex++] = TWDR; + + case TW_MR_SLA_NACK: // address sent, nack received + twi_stop(); + break; + + // TW_MR_ARB_LOST handled by TW_MT_ARB_LOST case + + // Slave Receiver (NOT IMPLEMENTED YET) + /* + case TW_SR_SLA_ACK: // addressed, returned ack + case TW_SR_GCALL_ACK: // addressed generally, returned ack + case TW_SR_ARB_LOST_SLA_ACK: // lost arbitration, returned ack + case TW_SR_ARB_LOST_GCALL_ACK: // lost arbitration, returned ack + // enter slave receiver mode + twi_state = TWI_SRX; + + // indicate that rx buffer can be overwritten and ack + twi_rxBufferIndex = 0; + twi_reply(1); + break; + + case TW_SR_DATA_ACK: // data received, returned ack + case TW_SR_GCALL_DATA_ACK: // data received generally, returned ack + // if there is still room in the rx buffer + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + // put byte in buffer and ack + twi_rxBuffer[twi_rxBufferIndex++] = TWDR; + twi_reply(1); + } else { + // otherwise nack + twi_reply(0); + } + break; + + case TW_SR_STOP: // stop or repeated start condition received + // put a null char after data if there's room + if (twi_rxBufferIndex < TWI_BUFFER_LENGTH) { + twi_rxBuffer[twi_rxBufferIndex] = 0; + } + + // sends ack and stops interface for clock stretching + twi_stop(); + + // callback to user defined callback + twi_onSlaveReceive(twi_rxBuffer, twi_rxBufferIndex); + + // since we submit rx buffer to "wire" library, we can reset it + twi_rxBufferIndex = 0; + + // ack future responses and leave slave receiver state + twi_releaseBus(); + break; + + case TW_SR_DATA_NACK: // data received, returned nack + case TW_SR_GCALL_DATA_NACK: // data received generally, returned nack + // nack back at master + twi_reply(0); + break; + + // Slave Transmitter + case TW_ST_SLA_ACK: // addressed, returned ack + case TW_ST_ARB_LOST_SLA_ACK: // arbitration lost, returned ack + // enter slave transmitter mode + twi_state = TWI_STX; + + // ready the tx buffer index for iteration + twi_txBufferIndex = 0; + + // set tx buffer length to be zero, to verify if user changes it + twi_txBufferLength = 0; + + // request for txBuffer to be filled and length to be set + // note: user must call twi_transmit(bytes, length) to do this + twi_onSlaveTransmit(); + + // if they didn't change buffer & length, initialize it + if (0 == twi_txBufferLength) { + twi_txBufferLength = 1; + twi_txBuffer[0] = 0x00; + } + + // transmit first byte from buffer, fall through + + case TW_ST_DATA_ACK: // byte sent, ack returned + // copy data to output register + TWDR = twi_txBuffer[twi_txBufferIndex++]; + + // if there is more to send, ack, otherwise nack + if (twi_txBufferIndex < twi_txBufferLength) { + twi_reply(1); + } else { + twi_reply(0); + } + break; + + case TW_ST_DATA_NACK: // received nack, we are done + case TW_ST_LAST_DATA: // received ack, but we are done already! + // ack future responses + twi_reply(1); + // leave slave receiver state + twi_state = TWI_READY; + break; + */ + + // all + case TW_NO_INFO: // no state information + break; + + case TW_BUS_ERROR: // bus error, illegal stop/start + twi_error = TW_BUS_ERROR; + twi_stop(); + break; + } + + if (fNextInterruptFunction) return fNextInterruptFunction(); + } + + TwoWire::TwoWire() { } + + void TwoWire::begin(void) { + rxBufferIndex = 0; + rxBufferLength = 0; + + txBufferIndex = 0; + txBufferLength = 0; + + twi_init(); + } + + void TwoWire::beginTransmission(uint8_t address) { + //beginTransmission((uint8_t)address); + + // indicate that we are transmitting + twi_transmitting = 1; + + // set address of targeted slave + txAddress = address; + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + } + + uint8_t TwoWire::endTransmission(uint16_t timeout) { + // transmit buffer (blocking) + //int8_t ret = + twi_cbendTransmissionDone = NULL; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + int8_t ret = twii_WaitForDone(timeout); + + // reset tx buffer iterator vars + txBufferIndex = 0; + txBufferLength = 0; + + // indicate that we are done transmitting + // twi_transmitting = 0; + return ret; + } + + void TwoWire::nbendTransmission(void (*function)(int)) { + twi_cbendTransmissionDone = function; + twi_writeTo(txAddress, txBuffer, txBufferLength, 1); + return; + } + + void TwoWire::send(uint8_t data) { + if (twi_transmitting) { + // in master transmitter mode + // don't bother if buffer is full + if (txBufferLength >= NBWIRE_BUFFER_LENGTH) { + return; + } + + // put byte in tx buffer + txBuffer[txBufferIndex] = data; + ++txBufferIndex; + + // update amount in buffer + txBufferLength = txBufferIndex; + } else { + // in slave send mode + // reply to master + //twi_transmit(&data, 1); + } + } + + uint8_t TwoWire::receive(void) { + // default to returning null char + // for people using with char strings + uint8_t value = 0; + + // get each successive byte on each call + if (rxBufferIndex < rxBufferLength) { + value = rxBuffer[rxBufferIndex]; + ++rxBufferIndex; + } + + return value; + } + + uint8_t TwoWire::requestFrom(uint8_t address, int quantity, uint16_t timeout) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = NULL; + twi_readFrom(address, rxBuffer, quantity); + uint8_t read = twii_WaitForDone(timeout); + + // set rx buffer iterator vars + rxBufferIndex = 0; + rxBufferLength = read; + + return read; + } + + void TwoWire::nbrequestFrom(uint8_t address, int quantity, void (*function)(int)) { + // clamp to buffer length + if (quantity > NBWIRE_BUFFER_LENGTH) { + quantity = NBWIRE_BUFFER_LENGTH; + } + + // perform blocking read into buffer + twi_cbreadFromDone = function; + twi_readFrom(address, rxBuffer, quantity); + //uint8_t read = twii_WaitForDone(); + + // set rx buffer iterator vars + //rxBufferIndex = 0; + //rxBufferLength = read; + + rxBufferIndex = 0; + rxBufferLength = quantity; // this is a hack + + return; //read; + } + + uint8_t TwoWire::available(void) { + return rxBufferLength - rxBufferIndex; + } + +#endif diff --git a/pio/lib/I2Cdev/I2Cdev.h b/pio/lib/I2Cdev/I2Cdev.h new file mode 100644 index 00000000..5b59c56f --- /dev/null +++ b/pio/lib/I2Cdev/I2Cdev.h @@ -0,0 +1,311 @@ +// 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_ + +// ----------------------------------------------------------------------------- +// 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 +#define I2CDEV_IMPLEMENTATION_WARNINGS + +// ----------------------------------------------------------------------------- +// I2C interface implementation options +// ----------------------------------------------------------------------------- +#define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino +#define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project + // ^^^ NBWire implementation is still buggy w/some interrupts! +#define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project +#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) +// ----------------------------------------------------------------------------- +//#define I2CDEV_SERIAL_DEBUG + +#ifdef ARDUINO + #if ARDUINO < 100 + #include "WProgram.h" + #else + #include "Arduino.h" + #endif + #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];") +#define I2CDEV_DEFAULT_READ_TIMEOUT 1000 + +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, void *wireObj=0); + static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0); + + static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0); + static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0); + static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0); + static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0); + static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0); + static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0); + static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0); + static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0); + + static uint16_t readTimeout; +}; + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + ////////////////////// + // FastWire 0.24 + // This is a library to help faster programs to read I2C devices. + // Copyright(C) 2012 + // Francesco Ferrara + ////////////////////// + + /* Master */ + #define TW_START 0x08 + #define TW_REP_START 0x10 + + /* Master Transmitter */ + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + + /* Master Receiver */ + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + + #define TW_OK 0 + #define TW_ERROR 1 + + class Fastwire { + private: + static boolean waitInt(); + + public: + static void setup(int khz, boolean pullup); + static byte beginTransmission(byte device); + static byte write(byte value); + static byte writeBuf(byte device, byte address, byte *data, byte num); + static byte readBuf(byte device, byte address, byte *data, byte num); + static void reset(); + static byte stop(); + }; +#endif + +#if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + // NBWire implementation based heavily on code by Gene Knight + // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html + // 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(); + void begin(uint8_t); + void begin(int); + void beginTransmission(uint8_t); + //void beginTransmission(int); + uint8_t endTransmission(uint16_t timeout=0); + void nbendTransmission(void (*function)(int)) ; + uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); + //uint8_t requestFrom(int, int); + void nbrequestFrom(uint8_t, int, void (*function)(int)); + void send(uint8_t); + void send(uint8_t*, uint8_t); + //void send(int); + void send(char*); + uint8_t available(void); + uint8_t receive(void); + 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 ((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 + #define TW_MT_SLA_ACK 0x18 + #define TW_MT_SLA_NACK 0x20 + #define TW_MT_DATA_ACK 0x28 + #define TW_MT_DATA_NACK 0x30 + #define TW_MT_ARB_LOST 0x38 + #define TW_MR_ARB_LOST 0x38 + #define TW_MR_SLA_ACK 0x40 + #define TW_MR_SLA_NACK 0x48 + #define TW_MR_DATA_ACK 0x50 + #define TW_MR_DATA_NACK 0x58 + #define TW_ST_SLA_ACK 0xA8 + #define TW_ST_ARB_LOST_SLA_ACK 0xB0 + #define TW_ST_DATA_ACK 0xB8 + #define TW_ST_DATA_NACK 0xC0 + #define TW_ST_LAST_DATA 0xC8 + #define TW_SR_SLA_ACK 0x60 + #define TW_SR_ARB_LOST_SLA_ACK 0x68 + #define TW_SR_GCALL_ACK 0x70 + #define TW_SR_ARB_LOST_GCALL_ACK 0x78 + #define TW_SR_DATA_ACK 0x80 + #define TW_SR_DATA_NACK 0x88 + #define TW_SR_GCALL_DATA_ACK 0x90 + #define TW_SR_GCALL_DATA_NACK 0x98 + #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) |= (1 << bit)) + #endif // sbi + + #ifndef cbi // clear bit + #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~(1 << bit)) + #endif // cbi + + extern TwoWire Wire; + +#endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE + +#endif /* _I2CDEV_H_ */ diff --git a/pio/lib/I2Cdev/keywords.txt b/pio/lib/I2Cdev/keywords.txt new file mode 100644 index 00000000..4132a06c --- /dev/null +++ b/pio/lib/I2Cdev/keywords.txt @@ -0,0 +1,38 @@ +####################################### +# Syntax Coloring Map For I2Cdev +####################################### + +####################################### +# Datatypes (KEYWORD1) +####################################### +I2Cdev KEYWORD1 + +####################################### +# Methods and Functions (KEYWORD2) +####################################### + +readBit KEYWORD2 +readBitW KEYWORD2 +readBits KEYWORD2 +readBitsW KEYWORD2 +readByte KEYWORD2 +readBytes KEYWORD2 +readWord KEYWORD2 +readWords KEYWORD2 +writeBit KEYWORD2 +writeBitW KEYWORD2 +writeBits KEYWORD2 +writeBitsW KEYWORD2 +writeByte KEYWORD2 +writeBytes KEYWORD2 +writeWord KEYWORD2 +writeWords KEYWORD2 + +####################################### +# Instances (KEYWORD2) +####################################### + +####################################### +# Constants (LITERAL1) +####################################### + diff --git a/pio/lib/I2Cdev/library.json b/pio/lib/I2Cdev/library.json new file mode 100644 index 00000000..5c5eab8f --- /dev/null +++ b/pio/lib/I2Cdev/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-Core", + "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": [ + { + "name": "Wire" + } + ] +} diff --git a/pio/lib/MPU6050/MPU6050.cpp b/pio/lib/MPU6050/MPU6050.cpp new file mode 100644 index 00000000..ecdfe04d --- /dev/null +++ b/pio/lib/MPU6050/MPU6050.cpp @@ -0,0 +1,3395 @@ +// 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" + +/** 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, void *wireObj):devAddr(address), wireObj(wireObj) { +} + +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, 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 + +/** 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, 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_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_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_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_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_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 + * 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, 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 + * 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, wireObj); + 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, 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 + * 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, wireObj); + 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, 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 + * 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, 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 + * 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, 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 + * 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, wireObj); + 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, 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, + * 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, wireObj); + 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, 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 + * 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, wireObj); + 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, 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. + * 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, wireObj); + 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, 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 + * 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, 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 + * 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, 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]; + *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, 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]; +} +/** 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, 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]; +} +/** 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, 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_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. + * @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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); +} +/** 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, wireObj); +} +/** 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, 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_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 + * 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, wireObj); +} +/** 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, wireObj); +} +/** 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, wireObj); +} + +// 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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_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_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. + * 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} +/** 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); + 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, 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_Base::setFIFOByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_FIFO_R_W, data, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// ======== 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, wireObj); + return buffer[0]; +} +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_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_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_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_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_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_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_Base::getXFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_X_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +void MPU6050_Base::setXFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_X_FINE_GAIN, gain, wireObj); +} + +// Y_FINE_GAIN register + +int8_t MPU6050_Base::getYFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Y_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +void MPU6050_Base::setYFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Y_FINE_GAIN, gain, wireObj); +} + +// Z_FINE_GAIN register + +int8_t MPU6050_Base::getZFineGain() { + I2Cdev::readByte(devAddr, MPU6050_RA_Z_FINE_GAIN, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +void MPU6050_Base::setZFineGain(int8_t gain) { + I2Cdev::writeByte(devAddr, MPU6050_RA_Z_FINE_GAIN, gain, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// 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, wireObj); + 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, wireObj); +} + +// XG_OFFS_USR* registers + +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_Base::setXGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_XG_OFFS_USRH, offset, wireObj); +} + +// YG_OFFS_USR* register + +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_Base::setYGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_YG_OFFS_USRH, offset, wireObj); +} + +// ZG_OFFS_USR* register + +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_Base::setZGyroOffset(int16_t offset) { + I2Cdev::writeWord(devAddr, MPU6050_RA_ZG_OFFS_USRH, offset, wireObj); +} + +// 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, wireObj); + return buffer[0]; +} +void MPU6050_Base::setIntPLLReadyEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, enabled, wireObj); +} +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_Base::setIntDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, enabled, wireObj); +} + +// DMP_INT_STATUS + +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_Base::getDMPInt4Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_4_BIT, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +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_Base::getDMPInt2Status() { + I2Cdev::readBit(devAddr, MPU6050_RA_DMP_INT_STATUS, MPU6050_DMPINT_2_BIT, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +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_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_Base::getIntPLLReadyStatus() { + I2Cdev::readBit(devAddr, MPU6050_RA_INT_STATUS, MPU6050_INTERRUPT_PLL_RDY_INT_BIT, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +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_Base::getDMPEnabled() { + I2Cdev::readBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +void MPU6050_Base::setDMPEnabled(bool enabled) { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, enabled, wireObj); +} +void MPU6050_Base::resetDMP() { + I2Cdev::writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true, wireObj); +} + +// 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, wireObj); +} + +// MEM_START_ADDR register + +void MPU6050_Base::setMemoryStartAddress(uint8_t address) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_START_ADDR, address, wireObj); +} + +// MEM_R_W register + +uint8_t MPU6050_Base::readMemoryByte() { + I2Cdev::readByte(devAddr, MPU6050_RA_MEM_R_W, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +void MPU6050_Base::writeMemoryByte(uint8_t data) { + I2Cdev::writeByte(devAddr, MPU6050_RA_MEM_R_W, data, wireObj); +} +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, wireObj); + + // 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] = 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, wireObj); + + // verify data if needed + if (verify && verifyBuffer) { + setMemoryBank(bank); + setMemoryStartAddress(address); + 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); + 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 = 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, wireObj); // 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, wireObj); + return buffer[0]; +} +void MPU6050_Base::setDMPConfig1(uint8_t config) { + I2Cdev::writeByte(devAddr, MPU6050_RA_DMP_CFG_1, config, wireObj); +} + +// DMP_CFG_2 register + +uint8_t MPU6050_Base::getDMPConfig2() { + I2Cdev::readByte(devAddr, MPU6050_RA_DMP_CFG_2, buffer, I2Cdev::readTimeout, wireObj); + return buffer[0]; +} +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(); +} + +void MPU6050_Base::PrintActiveOffsets() { + uint8_t AOffsetRegister = (getDeviceID() < 0x38 )? MPU6050_RA_XA_OFFS_H:0x77; + int16_t Data[3]; + //Serial.print(F("Offset Register 0x")); + //Serial.print(AOffsetRegister>>4,HEX);Serial.print(AOffsetRegister&0x0F,HEX); + Serial.print(F("\n// X Accel Y Accel Z Accel X Gyro Y Gyro Z Gyro\n// OFFSETS ")); + if(AOffsetRegister == 0x06) I2Cdev::readWords(devAddr, AOffsetRegister, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); + else { + I2Cdev::readWords(devAddr, AOffsetRegister, 1, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+3, 1, (uint16_t *)Data+1, I2Cdev::readTimeout, wireObj); + I2Cdev::readWords(devAddr, AOffsetRegister+6, 1, (uint16_t *)Data+2, I2Cdev::readTimeout, wireObj); + } + // A_OFFSET_H_READ_A_OFFS(Data); + Serial.print((float)Data[0], 5); Serial.print(", "); + Serial.print((float)Data[1], 5); Serial.print(", "); + Serial.print((float)Data[2], 5); Serial.print(", "); + I2Cdev::readWords(devAddr, 0x13, 3, (uint16_t *)Data, I2Cdev::readTimeout, wireObj); + // XG_OFFSET_H_READ_OFFS_USR(Data); + Serial.print((float)Data[0], 5); Serial.print(", "); + Serial.print((float)Data[1], 5); Serial.print(", "); + Serial.print((float)Data[2], 5); Serial.print("\n"); +} diff --git a/pio/lib/MPU6050/MPU6050.h b/pio/lib/MPU6050/MPU6050.h new file mode 100644 index 00000000..888d4db9 --- /dev/null +++ b/pio/lib/MPU6050/MPU6050.h @@ -0,0 +1,848 @@ +// 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" + +// 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 +#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 *wireObj=0); + + 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 + + protected: + uint8_t devAddr; + void *wireObj; + uint8_t buffer[14]; + uint32_t fifoTimeout = MPU6050_FIFO_DEFAULT_TIMEOUT; +}; + +#ifndef I2CDEVLIB_MPU6050_TYPEDEF +#define I2CDEVLIB_MPU6050_TYPEDEF +typedef MPU6050_Base MPU6050; +#endif + +#endif /* _MPU6050_H_ */ diff --git a/pio/lib/MPU6050/MPU6050_6Axis_MotionApps20.cpp b/pio/lib/MPU6050/MPU6050_6Axis_MotionApps20.cpp new file mode 100644 index 00000000..3a896745 --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/MPU6050_6Axis_MotionApps20.h b/pio/lib/MPU6050/MPU6050_6Axis_MotionApps20.h new file mode 100644 index 00000000..8bbb9d23 --- /dev/null +++ b/pio/lib/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, 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/pio/lib/MPU6050/MPU6050_6Axis_MotionApps612.cpp b/pio/lib/MPU6050/MPU6050_6Axis_MotionApps612.cpp new file mode 100644 index 00000000..3fb53e4d --- /dev/null +++ b/pio/lib/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, +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 = +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]) / (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/pio/lib/MPU6050/MPU6050_6Axis_MotionApps612.h b/pio/lib/MPU6050/MPU6050_6Axis_MotionApps612.h new file mode 100644 index 00000000..56e3bb35 --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/MPU6050_9Axis_MotionApps41.cpp b/pio/lib/MPU6050/MPU6050_9Axis_MotionApps41.cpp new file mode 100644 index 00000000..d85d74a5 --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/MPU6050_9Axis_MotionApps41.h b/pio/lib/MPU6050/MPU6050_9Axis_MotionApps41.h new file mode 100644 index 00000000..6d3576e9 --- /dev/null +++ b/pio/lib/MPU6050/MPU6050_9Axis_MotionApps41.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_MOTIONAPPS41_H_ +#define _MPU6050_6AXIS_MOTIONAPPS41_H_ + +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF + +#include "MPU6050.h" + +class MPU6050_9Axis_MotionApps41 : public MPU6050_Base { + public: + MPU6050_9Axis_MotionApps41(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 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(); + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; +}; + +typedef MPU6050_9Axis_MotionApps41 MPU6050; + +#endif /* _MPU6050_6AXIS_MOTIONAPPS41_H_ */ diff --git a/pio/lib/MPU6050/examples/IMU_Zero/IMU_Zero.ino b/pio/lib/MPU6050/examples/IMU_Zero/IMU_Zero.ino new file mode 100644 index 00000000..8e50b991 --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino b/pio/lib/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino new file mode 100644 index 00000000..3ebe2ee6 --- /dev/null +++ b/pio/lib/MPU6050/examples/MPU6050_DMP6/MPU6050_DMP6.ino @@ -0,0 +1,345 @@ +// 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: +// 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 +// 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_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 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() { + // 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(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) { + // 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.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.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/pio/lib/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde b/pio/lib/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde new file mode 100644 index 00000000..130fc4dc --- /dev/null +++ b/pio/lib/MPU6050/examples/MPU6050_DMP6/Processing/MPUTeapot/MPUTeapot.pde @@ -0,0 +1,242 @@ +// 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. +=============================================== +*/ + +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]; + +void setup() { + // 300px square viewport using OpenGL rendering + size(300, 300, OPENGL); + gfx = new ToxiclibsSupport(this); + + // setup lights and antialiasing + lights(); + smooth(); + + // display serial port list for debugging/clarity + println(Serial.list()); + + // get the first available port (use EITHER this OR the specific port code below) + String portName = Serial.list()[0]; + + // get a specific serial port (use EITHER this OR the first-available code above) + //String portName = "COM4"; + + // 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'); +} + +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); + + // 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 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); + */ + } + } + } +} + +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/pio/lib/MPU6050/examples/MPU6050_DMP6_ESPWiFi/MPU6050_DMP6_ESPWiFi.ino b/pio/lib/MPU6050/examples/MPU6050_DMP6_ESPWiFi/MPU6050_DMP6_ESPWiFi.ino new file mode 100644 index 00000000..a080869c --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_ESPWiFi/Processing/MPUOSCTeapot/MPUOSCTeapot.pde b/pio/lib/MPU6050/examples/MPU6050_DMP6_ESPWiFi/Processing/MPUOSCTeapot/MPUOSCTeapot.pde new file mode 100644 index 00000000..dfe65189 --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_Ethernet/MPU6050_DMP6_Ethernet.ino b/pio/lib/MPU6050/examples/MPU6050_DMP6_Ethernet/MPU6050_DMP6_Ethernet.ino new file mode 100644 index 00000000..b2e2081d --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPU6050_DMP6_using_DMP_V6.12.ino b/pio/lib/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/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/MPUplane.pde b/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/MPUplane.pde new file mode 100644 index 00000000..ce4a6fda --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/biplane.obj b/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/biplane.obj new file mode 100644 index 00000000..6867928e --- /dev/null +++ b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/diffuse_512.png b/pio/lib/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/pio/lib/MPU6050/examples/MPU6050_DMP6_using_DMP_V6.12/MPUplane/data/diffuse_512.png differ diff --git a/pio/lib/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino b/pio/lib/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino new file mode 100644 index 00000000..596f0750 --- /dev/null +++ b/pio/lib/MPU6050/examples/MPU6050_raw/MPU6050_raw.ino @@ -0,0 +1,153 @@ +// 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: +// 2013-05-08 - added multiple output formats +// - added seamless Fastwire support +// 2011-10-07 - 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. +=============================================== +*/ + +// 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 +//MPU6050 accelgyro(0x68, &Wire1); // <-- use for AD0 low, but 2nd Wire (TWI/I2C) object + +int16_t ax, ay, az; +int16_t gx, gy, gz; + + + +// uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated +// list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read, +// not so easy to parse, and slow(er) over UART. +#define OUTPUT_READABLE_ACCELGYRO + +// uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit +// binary, one right after the other. This is very fast (as fast as possible +// without compression or data loss), and easy to parse, but impossible to read +// for a human. +//#define OUTPUT_BINARY_ACCELGYRO + + +#define LED_PIN 13 +bool blinkState = false; + +void setup() { + // 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 + + // 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(); + + // verify connection + Serial.println("Testing device connections..."); + Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); + + // use the code below to change accel/gyro offset values + /* + Serial.println("Updating internal sensor offsets..."); + // -76 -2359 1688 0 0 0 + Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 + Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 + Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 + Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 + Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 + Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 + Serial.print("\n"); + accelgyro.setXGyroOffset(220); + accelgyro.setYGyroOffset(76); + accelgyro.setZGyroOffset(-85); + Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 + Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 + Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 + Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 + Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 + Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 + Serial.print("\n"); + */ + + // configure Arduino LED pin for output + pinMode(LED_PIN, OUTPUT); +} + +void loop() { + // read raw accel/gyro measurements from device + accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); + + // these methods (and a few others) are also available + //accelgyro.getAcceleration(&ax, &ay, &az); + //accelgyro.getRotation(&gx, &gy, &gz); + + #ifdef OUTPUT_READABLE_ACCELGYRO + // display tab-separated accel/gyro x/y/z values + Serial.print("a/g:\t"); + Serial.print(ax); Serial.print("\t"); + Serial.print(ay); Serial.print("\t"); + Serial.print(az); Serial.print("\t"); + Serial.print(gx); Serial.print("\t"); + Serial.print(gy); Serial.print("\t"); + Serial.println(gz); + #endif + + #ifdef OUTPUT_BINARY_ACCELGYRO + Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF)); + Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF)); + Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF)); + Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF)); + Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF)); + Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF)); + #endif + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + delay(100); +} diff --git a/pio/lib/MPU6050/helper_3dmath.h b/pio/lib/MPU6050/helper_3dmath.h new file mode 100644 index 00000000..9ed260ec --- /dev/null +++ b/pio/lib/MPU6050/helper_3dmath.h @@ -0,0 +1,216 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper +// 6/5/2012 by Jeff Rowberg +// 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/pio/lib/MPU6050/library.json b/pio/lib/MPU6050/library.json new file mode 100644 index 00000000..e89e10cb --- /dev/null +++ b/pio/lib/MPU6050/library.json @@ -0,0 +1,18 @@ +{ + "name": "I2Cdevlib-MPU6050", + "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": + { + "name": "I2Cdevlib-Core", + "frameworks": "arduino" + }, + "frameworks": "arduino", + "platforms": "*" +} diff --git a/platformio.ini b/platformio.ini index 38a78651..d0a86617 100644 --- a/platformio.ini +++ b/platformio.ini @@ -6,7 +6,7 @@ ; Advanced options: extra scripting ; ; Please visit documentation for the other options and examples -; http://docs.platformio.org/page/projectconf.html +; https://docs.platformio.org/page/projectconf.html [platformio] default_envs = d1_mini_git @@ -14,16 +14,15 @@ lib_dir = pio/lib src_dir = pio/src [common_env_data] -lib_deps = - ArduinoJson@>5 - ESP Async WebServer - I2Cdevlib-MPU6050 - OneWire - DallasTemperature - RunningMedian - PubSubClient - Blynk - ThingSpeak +lib_deps = + ArduinoJson@>5 + ESP Async WebServer + OneWire + DallasTemperature + RunningMedian + PubSubClient + Blynk + ThingSpeak [env:d1_mini_git] platform = espressif8266 @@ -32,8 +31,8 @@ framework = arduino monitor_speed = 115200 upload_speed = 115200 build_flags = !python3 git_rev.py -lib_deps = - ${common_env_data.lib_deps} +lib_deps = + ${common_env_data.lib_deps} [env:d1_mini_standalone] platform = espressif8266 @@ -41,9 +40,9 @@ board = d1_mini framework = arduino monitor_speed = 115200 upload_speed = 115200 -lib_deps = - ${common_env_data.lib_deps} - +lib_deps = + ${common_env_data.lib_deps} + [env:d1_mini_git_stage] platform = https://github.com/platformio/platform-espressif8266.git#feature/stage board = d1_mini @@ -51,5 +50,5 @@ framework = arduino monitor_speed = 115200 upload_speed = 921600 build_flags = !python3 git_rev.py -lib_deps = - ${common_env_data.lib_deps} +lib_deps = + ${common_env_data.lib_deps}