diff --git a/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/I2Cdev.h b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/I2Cdev.h new file mode 100644 index 0000000..5b59c56 --- /dev/null +++ b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/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/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/IMUtests_v0.5__RAGHAV_S_GEESE_.ino b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/IMUtests_v0.5__RAGHAV_S_GEESE_.ino new file mode 100644 index 0000000..540129d --- /dev/null +++ b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/IMUtests_v0.5__RAGHAV_S_GEESE_.ino @@ -0,0 +1,369 @@ +// I2C device class (I2Cdev) demonstration Arduino sketch for moose6050 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: +// 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 moose6050 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 moose(0x68); // <-- use for AD0 high + +/* ========================================================================= + NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch + depends on the moose-6050's INT pin being connected to the Arduino's + external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is + digital I/O pin 2. + * ========================================================================= */ + +/* ========================================================================= + NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error + when using Serial.write(buf, len). The Teapot output uses this method. + The solution requires a modification to the Arduino USBAPI.h file, which + is fortunately simple, but annoying. This will be fixed in the next IDE + release. For more info, see these links: + + http://arduino.cc/forum/index.php/topic,109987.0.html + http://code.google.com/p/arduino/issues/detail?id=958 + * ========================================================================= */ + + + +// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual +// quaternion components in a [w, x, y, z] format (not best for parsing +// on a remote host such as Processing or something though) +#define OUTPUT_READABLE_QUATERNION + +// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles +// (in degrees) calculated from the quaternions coming from the FIFO. +// Note that Euler angles suffer from gimbal lock (for more info, see +// http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_EULER + +// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ +// pitch/roll angles (in degrees) calculated from the quaternions coming +// from the FIFO. Note this also requires gravity vector calculations. +// Also note that yaw/pitch/roll angles suffer from gimbal lock (for +// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) +//#define OUTPUT_READABLE_YAWPITCHROLL + +// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration +// components with gravity removed. This acceleration reference frame is +// not compensated for orientation, so +X is always +X according to the +// sensor, just without the effects of gravity. If you want acceleration +// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. +//#define OUTPUT_READABLE_REALACCEL + +// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration +// components with gravity removed and adjusted for the world frame of +// reference (yaw is relative to initial orientation, since no magnetometer +// is present in this case). Could be quite handy in some cases. +//#define OUTPUT_READABLE_WORLDACCEL + +// uncomment "OUTPUT_TEAPOT" if you want output that matches the +// format used for the InvenSense teapot demo +//#define OUTPUT_TEAPOT + + + +#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) +bool blinkState = false; + +// moose control/status vars +bool dmpReady = false; // set true if DMP init was successful +uint8_t mooseIntStatus; // holds actual interrupt status byte from moose +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 +float quaternion[3]; + +// 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 mooseInterrupt = false; // indicates whether moose interrupt pin has gone high +void dmpDataReady() { + mooseInterrupt = true; +} + + + +// ================================================================ +// === INITIAL SETUP === +// ================================================================ + +void setup() { + // join I2C bus (I2Cdev library doesn't do this automatically) + #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) + #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); + #endif + + // initialize serial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + Serial.begin(115200); + while (!Serial); // wait for Leonardo enumeration, others continue immediately + + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio + // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to + // the baud timing being too misaligned with processor ticks. You must use + // 38400 or slower in these cases, or use some kind of external separate + // crystal solution for the UART timer. + + // initialize device + Serial.println(F("Initializing I2C devices...")); + moose.initialize(); + + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(moose.testConnection() ? F("moose6050 connection successful") : F("moose6050 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 = moose.dmpInitialize(); + + // supply your own gyro offsets here, scaled for min sensitivity + moose.setXGyroOffset(220); + moose.setYGyroOffset(76); + moose.setZGyroOffset(-85); + moose.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...")); + moose.setDMPEnabled(true); + + // enable Arduino interrupt detection + Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); + attachInterrupt(0, dmpDataReady, RISING); + mooseIntStatus = moose.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 = moose.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } + + // configure LED for output + pinMode(LED_PIN, OUTPUT); +} + + + +// ================================================================ +// === MAIN PROGRAM LOOP === +// ================================================================ + +void loop() { + // if programming failed, don't try to do anything + if (!dmpReady) return; + // wait for moose interrupt or extra packet(s) available + while (!mooseInterrupt && fifoCount < packetSize) { + // other program behavior stuff here + // . + // . + // . + // if you are really paranoid you can frequently test in between other + // stuff to see if mooseInterrupt is true, and if so, "break;" from the + // while() loop to immediately process the moose data + // . + // . + // . + } + + // reset interrupt flag and get INT_STATUS byte + mooseInterrupt = false; + mooseIntStatus = moose.getIntStatus(); + + // get current FIFO count + fifoCount = moose.getFIFOCount(); + + // check for overflow (this should never happen unless our code is too inefficient) + if ((mooseIntStatus & 0x10) || fifoCount == 1024) { + // reset so we can continue cleanly + moose.resetFIFO(); + Serial.println(F("FIFO overflow!")); + + // otherwise, check for DMP data ready interrupt (this should happen frequently) + } else if (mooseIntStatus & 0x02) { + // wait for correct available data length, should be a VERY short wait + while (fifoCount < packetSize) fifoCount = moose.getFIFOCount(); + + // read a packet from FIFO + moose.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 + moose.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat\t"); + Serial.print((q.x+0.03)*180); + Serial.print("\t"); + Serial.print((q.y+0.03)*180); + Serial.print("\t"); + Serial.println((q.z+0.52)*180); + #endif + + #ifdef OUTPUT_READABLE_EULER + // display Euler angles in degrees + moose.dmpGetQuaternion(&q, fifoBuffer); + moose.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 + moose.dmpGetQuaternion(&q, fifoBuffer); + moose.dmpGetGravity(&gravity, &q); + moose.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 + moose.dmpGetQuaternion(&q, fifoBuffer); + moose.dmpGetAccel(&aa, fifoBuffer); + moose.dmpGetGravity(&gravity, &q); + moose.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 + moose.dmpGetQuaternion(&q, fifoBuffer); + moose.dmpGetAccel(&aa, fifoBuffer); + moose.dmpGetGravity(&gravity, &q); + moose.dmpGetLinearAccel(&aaReal, &aa, &gravity); + moose.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/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050.h b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050.h new file mode 100644 index 0000000..888d4db --- /dev/null +++ b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/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/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050_6Axis_MotionApps20.h b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/MPU6050_6Axis_MotionApps20.h new file mode 100644 index 0000000..8bbb9d2 --- /dev/null +++ b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/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/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/i2cdevlib-master.zip b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/i2cdevlib-master.zip new file mode 100644 index 0000000..60ce503 Binary files /dev/null and b/communication/IMU/IMUtests_v0.5__RAGHAV_S_GEESE_/i2cdevlib-master.zip differ diff --git a/communication/IMU/MPU6050_DMP6_edit/MPU6050_DMP6_edit.ino b/communication/IMU/MPU6050_DMP6_edit/MPU6050_DMP6_edit.ino new file mode 100644 index 0000000..3ebe2ee --- /dev/null +++ b/communication/IMU/MPU6050_DMP6_edit/MPU6050_DMP6_edit.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/communication/IMU/MPU6050_DMP6_v1.1/MPU6050_DMP6_v1.1.ino b/communication/IMU/MPU6050_DMP6_v1.1/MPU6050_DMP6_v1.1.ino new file mode 100644 index 0000000..a349c6b --- /dev/null +++ b/communication/IMU/MPU6050_DMP6_v1.1/MPU6050_DMP6_v1.1.ino @@ -0,0 +1,389 @@ +// 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 mpu2(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 mpu2IntStatus; // 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 packetSize2; // 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 + +float ypr2[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(); + + mpu2.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")); + + Serial.println(mpu2.testConnection() ? F("MPU6050 #2 connection successful") : F("MPU6050 #2 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(); + + devStatus = mpu2.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 + + mpu2.setXGyroOffset(220); + mpu2.setYGyroOffset(76); + mpu2.setZGyroOffset(-85); + mpu2.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); + + mpu2.CalibrateAccel(6); + mpu2.CalibrateGyro(6); + mpu2.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP on 0x69...")); + mpu2.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(); + mpu2IntStatus = 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(); + + packetSize2 = 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); + mpu2.dmpGetQuaternion(&q, fifoBuffer); + Serial.print("quat2\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); + mpu2.dmpGetQuaternion(&q, fifoBuffer); + mpu2.dmpGetGravity(&gravity, &q); + mpu2.dmpGetYawPitchRoll(ypr2, &q, &gravity); + Serial.print("ypr2\t"); + Serial.print(ypr2[0] * 180/M_PI); + Serial.print("\t"); + Serial.print(ypr2[1] * 180/M_PI); + Serial.print("\t"); + Serial.println(ypr2[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/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.4.ino b/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.4.ino index d4735e4..8000fa3 100644 --- a/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.4.ino +++ b/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.4.ino @@ -1,6 +1,5 @@ #include -//from https://create.arduino.cc/projecthub/MinukaThesathYapa/arduino-mpu6050-accelerometer-f92d8b -//currently trying to tweak why the thing is increasing a lot :( +//https://create.arduino.cc/projecthub/MinukaThesathYapa/arduino-mpu6050-accelerometer-f92d8b const int MPU = 0x68; float AccX, AccY, AccZ; float GyroX, GyroY, GyroZ; @@ -9,14 +8,11 @@ float roll, pitch, yaw; float AccErrorX, AccErrorY, GyroErrorX, GyroErrorY, GyroErrorZ; float elapsedTime, currentTime, previousTime; int c = 0; -float roc_p = 0; -float roc_y = 0; -float roc_r = 0; +int ticker = 1; +float c_r = 0.02666666666666666666666666666666666; +float c_p = 0.00734734734734734734734734734734734; +float c_y = 0.040754754754754754754754754754755; -float p_p = 0; -float p_y = 0; -float p_r = 0; -int ticker = 0; void setup() { Serial.begin(19200); @@ -59,23 +55,14 @@ void loop() yaw = yaw + GyroZ * elapsedTime; roll = 0.96 * gyroAngleX + 0.04 * accAngleX; pitch = 0.96 * gyroAngleY + 0.04 * accAngleY; - roll = roll*2; - pitch = pitch*2; - yaw = yaw*2; - while(roll<0.0){roll+=360.0;} - while(pitch<0.0){pitch+=360.0;} - while(yaw<0.0){yaw+=360.0;} - while(roll>=360.0){roll-=360.0;} - while(pitch>=360.0){pitch-=360.0;} - while(yaw>=360.0){yaw-=360.0;} + Serial.print(roll+c_r*ticker); + Serial.print("/"); + Serial.print(pitch+c_p*ticker); + Serial.print("/"); + Serial.println(yaw+c_y*ticker); + ticker++; + - Serial.print(roll); - Serial.print("/"); - Serial.print(pitch); - Serial.print("/"); - Serial.println(yaw); - if(ticker<100){ticker+=1;} - } diff --git a/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.5/i_copied_this_from_someone_v0.5.ino b/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.5/i_copied_this_from_someone_v0.5.ino new file mode 100644 index 0000000..131be38 --- /dev/null +++ b/communication/IMU/i_copied_this_from_someone_v0.4/i_copied_this_from_someone_v0.5/i_copied_this_from_someone_v0.5.ino @@ -0,0 +1,232 @@ +// +// MPU-6050 Mahony IMU (yaw angle is relative to starting orientation) +// last update 7/9/2020 +// + +#include "Wire.h" + +// AD0 low = 0x68 (default for Sparkfun module) +// AD0 high = 0x69 +int MPU_addr = 0x68; + +// vvvvvvvvvvvvvvvvvv VERY VERY IMPORTANT vvvvvvvvvvvvvvvvvvvvvvvvvvvvv +//These are the previously determined offsets and scale factors for accelerometer and gyro for +// a particular example of an MPU-6050. They are not correct for other examples. +//The IMU code will NOT work well or at all if these are not correct + +float A_cal[6] = {265.0, -80.0, -700.0, 0.994, 1.000, 1.014}; // 0..2 offset xyz, 3..5 scale xyz +float G_off[3] = { -499.5, -17.7, -82.0}; //raw offsets, determined for gyro at rest +#define gscale ((250./32768.0)*(PI/180.0)) //gyro default 250 LSB per d/s -> rad/s + +// ^^^^^^^^^^^^^^^^^^^ VERY VERY IMPORTANT ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +// GLOBALLY DECLARED, required for Mahony filter +// vector to hold quaternion +float q[4] = {1.0, 0.0, 0.0, 0.0}; + +// Free parameters in the Mahony filter and fusion scheme, +// Kp for proportional feedback, Ki for integral +float Kp = 30.0; +float Ki = 0.0; + +// with MPU-9250, angles start oscillating at Kp=40. Ki does not seem to help and is not required. +// with MPU-6050, some instability observed at Kp=100 Now set to 30. + +// char s[60]; //snprintf buffer, if needed + +// globals for AHRS loop timing +unsigned long now_ms, last_ms = 0; //millis() timers + +// print interval +unsigned long print_ms = 200; //print angles every "print_ms" milliseconds +float yaw, pitch, roll; //Euler angle output + +void setup() { + + Wire.begin(); + Serial.begin(9600); + Serial.println("starting"); + + // initialize sensor + // defaults for gyro and accel sensitivity are 250 dps and +/- 2 g + Wire.beginTransmission(MPU_addr); + Wire.write(0x6B); // PWR_MGMT_1 register + Wire.write(0); // set to zero (wakes up the MPU-6050) + Wire.endTransmission(true); + +} + +// AHRS loop + +void loop() +{ + static int i = 0; + static float deltat = 0; //loop time in seconds + static unsigned long now = 0, last = 0; //micros() timers + + //raw data + int16_t ax, ay, az; + int16_t gx, gy, gz; + int16_t Tmp; //temperature + + //scaled data as vector + float Axyz[3]; + float Gxyz[3]; + + + Wire.beginTransmission(MPU_addr); + Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H) + Wire.endTransmission(false); + Wire.requestFrom(MPU_addr, 14, true); // request a total of 14 registers + int t = Wire.read() << 8; + ax = t | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L) + t = Wire.read() << 8; + ay = t | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L) + t = Wire.read() << 8; + az = t | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L) + t = Wire.read() << 8; + Tmp = t | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L) + t = Wire.read() << 8; + gx = t | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L) + t = Wire.read() << 8; + gy = t | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L) + t = Wire.read() << 8; + gz = t | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L) + + Axyz[0] = (float) ax; + Axyz[1] = (float) ay; + Axyz[2] = (float) az; + + //apply offsets and scale factors from Magneto + for (int i = 0; i < 3; i++) Axyz[i] = (Axyz[i] - A_cal[i]) * A_cal[i + 3]; + + Gxyz[0] = ((float) gx - G_off[0]) * gscale; //250 LSB(d/s) default to radians/s + Gxyz[1] = ((float) gy - G_off[1]) * gscale; + Gxyz[2] = ((float) gz - G_off[2]) * gscale; + + // snprintf(s,sizeof(s),"mpu raw %d,%d,%d,%d,%d,%d",ax,ay,az,gx,gy,gz); + // Serial.println(s); + + now = micros(); + deltat = (now - last) * 1.0e-6; //seconds since last update + last = now; + + Mahony_update(Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], deltat); + + // Compute Tait-Bryan angles. + // In this coordinate system, the positive z-axis is down toward Earth. + // Yaw is the angle between Sensor x-axis and Earth magnetic North + // (or true North if corrected for local declination, looking down on the sensor + // positive yaw is counterclockwise, which is not conventional for NED navigation. + // Pitch is angle between sensor x-axis and Earth ground plane, toward the + // Earth is positive, up toward the sky is negative. Roll is angle between + // sensor y-axis and Earth ground plane, y-axis up is positive roll. These + // arise from the definition of the homogeneous rotation matrix constructed + // from quaternions. Tait-Bryan angles as well as Euler angles are + // non-commutative; that is, the get the correct orientation the rotations + // must be applied in the correct order which for this configuration is yaw, + // pitch, and then roll. + // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + // which has additional links. + + roll = atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2])); + pitch = asin(2.0 * (q[0] * q[2] - q[1] * q[3])); + //conventional yaw increases clockwise from North. Not that the MPU-6050 knows where North is. + yaw = -atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3])); + // to degrees + yaw *= 180.0 / PI; + if (yaw < 0) yaw += 360.0; //compass circle + pitch *= 180.0 / PI; + roll *= 180.0 / PI; + + now_ms = millis(); //time to print? + if (now_ms - last_ms >= print_ms) { + last_ms = now_ms; + // print angles for serial plotter... + // Serial.print("ypr "); + Serial.print(yaw, 2); + Serial.print(", "); + Serial.print(pitch, 2); + Serial.print(", "); + Serial.println(roll, 2); + } +} +//-------------------------------------------------------------------------------------------------- +// Mahony scheme uses proportional and integral filtering on +// the error between estimated reference vector (gravity) and measured one. +// Madgwick's implementation of Mayhony's AHRS algorithm. +// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms +// +// Date Author Notes +// 29/09/2011 SOH Madgwick Initial release +// 02/10/2011 SOH Madgwick Optimised for reduced CPU load +// last update 07/09/2020 SJR minor edits +//-------------------------------------------------------------------------------------------------- +// IMU algorithm update + +void Mahony_update(float ax, float ay, float az, float gx, float gy, float gz, float deltat) { + float recipNorm; + float vx, vy, vz; + float ex, ey, ez; //error terms + float qa, qb, qc; + static float ix = 0.0, iy = 0.0, iz = 0.0; //integral feedback terms + float tmp; + + // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) + tmp = ax * ax + ay * ay + az * az; + if (tmp > 0.0) + { + + // Normalise accelerometer (assumed to measure the direction of gravity in body frame) + recipNorm = 1.0 / sqrt(tmp); + ax *= recipNorm; + ay *= recipNorm; + az *= recipNorm; + + // Estimated direction of gravity in the body frame (factor of two divided out) + vx = q[1] * q[3] - q[0] * q[2]; + vy = q[0] * q[1] + q[2] * q[3]; + vz = q[0] * q[0] - 0.5f + q[3] * q[3]; + + // Error is cross product between estimated and measured direction of gravity in body frame + // (half the actual magnitude) + ex = (ay * vz - az * vy); + ey = (az * vx - ax * vz); + ez = (ax * vy - ay * vx); + + // Compute and apply to gyro term the integral feedback, if enabled + if (Ki > 0.0f) { + ix += Ki * ex * deltat; // integral error scaled by Ki + iy += Ki * ey * deltat; + iz += Ki * ez * deltat; + gx += ix; // apply integral feedback + gy += iy; + gz += iz; + } + + // Apply proportional feedback to gyro term + gx += Kp * ex; + gy += Kp * ey; + gz += Kp * ez; + } + + // Integrate rate of change of quaternion, q cross gyro term + deltat = 0.5 * deltat; + gx *= deltat; // pre-multiply common factors + gy *= deltat; + gz *= deltat; + qa = q[0]; + qb = q[1]; + qc = q[2]; + q[0] += (-qb * gx - qc * gy - q[3] * gz); + q[1] += (qa * gx + qc * gz - q[3] * gy); + q[2] += (qa * gy - qb * gz + q[3] * gx); + q[3] += (qa * gz + qb * gy - qc * gx); + + // renormalise quaternion + recipNorm = 1.0 / sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]); + q[0] = q[0] * recipNorm; + q[1] = q[1] * recipNorm; + q[2] = q[2] * recipNorm; + q[3] = q[3] * recipNorm; +}