2 MPU-6050 Problems with data output

Hi! I want to use 2 MPU-6050 (GY-521) at the same time, I found some code on GitHub (GitHub - eadf/MPU6050_DMP6_Multiple: Temporary project) special for my theme. It is working, but with each sec data is growing up without touching any sensors.

Code:

// 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_Wrapper.h"
#include "TogglePin.h"
#include "DeathTimer.h"

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif


// 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
//#define OUTPUT_READABLE_PITCHROLL






#ifdef OUTPUT_TEAPOT
// Teapot demo can only output from one MPU6050
const bool useSecondMpu = false;
MPU6050_Array mpus(1);
#else
const bool useSecondMpu = true;
MPU6050_Array mpus(useSecondMpu ? 2 : 1);
#endif

#define AD0_PIN_0 4  // Connect this pin to the AD0 pin on MPU #0
#define AD0_PIN_1 5  // Connect this pin to the AD0 pin on MPU #1

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)

#define OUTPUT_SERIAL Serial
//#define OUTPUT_SERIAL Serial2
//#define OUTPUT_SERIAL softSerial


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] = { '

Thank you for answering!

P.S: You can find Serial Monitor Output and code file in the attachment.

MPU-6050_2.ino (11.3 KB)

MPU6050_Wrapper.h (6.43 KB)

DeathTimer.h (2.03 KB)

TogglePin.h (1.93 KB), 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

TogglePin activityLed(LED_PIN, 100);
DeathTimer deathTimer(5000L);

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(9600);

while (!Serial)
    ;

// initialize device
  Serial.println(F("Initializing I2C devices..."));
  mpus.add(AD0_PIN_0);
  if (useSecondMpu) mpus.add(AD0_PIN_1);

mpus.initialize();

// configure LED for output
  pinMode(LED_PIN, OUTPUT);

// verify connection
  Serial.println(F("Testing device connections..."));
  if (mpus.testConnection()) {
    Serial.println(F("MPU6050 connection successful"));
  } else {
    mpus.halt(F("MPU6050 connection failed, halting"));
  }

// 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())
    activityLed.update(); // flash led while waiting for data
  while (Serial.available() && Serial.read())
    ; // empty buffer again
  activityLed.setPeriod(500); // slow down led to 2Hz
  delay(5000);
  // load and configure the DMP
  Serial.println(F("Initializing DMP..."));
  mpus.dmpInitialize();

// supply your own gyro offsets here, scaled for min sensitivity
  MPU6050_Wrapper* currentMPU = mpus.select(0);
  currentMPU->_mpu.setXGyroOffset(220);
  currentMPU->_mpu.setYGyroOffset(76);
  currentMPU->_mpu.setZGyroOffset(-85);
  currentMPU->_mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  if (useSecondMpu) {
    currentMPU = mpus.select(1);
    currentMPU->_mpu.setXGyroOffset(220);
    currentMPU->_mpu.setYGyroOffset(76);
    currentMPU->_mpu.setZGyroOffset(-85);
    currentMPU->_mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
  }
  mpus.programDmp(0);
  if (useSecondMpu)
    mpus.programDmp(1);
}

void handleMPUevent(uint8_t mpu) {

MPU6050_Wrapper* currentMPU = mpus.select(mpu);
  // reset interrupt flag and get INT_STATUS byte
  currentMPU->getIntStatus();

// check for overflow (this should never happen unless our code is too inefficient)
  if ((currentMPU->_mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT))
      || currentMPU->_fifoCount >= 1024) {
    // reset so we can continue cleanly
    currentMPU->resetFIFO();
    Serial.println(F("FIFO overflow!"));
    return;
  }
  // otherwise, check for DMP data ready interrupt (this should happen frequently)
  if (currentMPU->_mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) {

// read and dump a packet if the queue contains more than one
    while (currentMPU->_fifoCount >= 2 * currentMPU->_packetSize) {
      // read and dump one sample
      //Serial.print("DUMP"); // this trace will be removed soon
      currentMPU->getFIFOBytes(fifoBuffer);
    }

// read a packet from FIFO
    currentMPU->getFIFOBytes(fifoBuffer);

#if defined(OUTPUT_READABLE_YAWPITCHROLL) or defined(OUTPUT_READABLE_PITCHROLL)
    // display Euler angles in degrees
    currentMPU->_mpu.dmpGetQuaternion(&q, fifoBuffer);
    currentMPU->_mpu.dmpGetGravity(&gravity, &q);
    currentMPU->_mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
#if defined(OUTPUT_READABLE_YAWPITCHROLL)
    OUTPUT_SERIAL.print("y");
#endif
    OUTPUT_SERIAL.print("pr:");
    OUTPUT_SERIAL.print(mpu);
    OUTPUT_SERIAL.print("\t");
#if defined(OUTPUT_READABLE_YAWPITCHROLL)
    OUTPUT_SERIAL.print(ypr[0] * 180 / M_PI);
    OUTPUT_SERIAL.print("\t");

#endif
    OUTPUT_SERIAL.print(ypr[1] * 180 / M_PI);
    OUTPUT_SERIAL.print("\t");
    OUTPUT_SERIAL.println(ypr[2] * 180 / M_PI);

#endif

}
}

void loop() {

static uint8_t mpu = 0;
  static MPU6050_Wrapper* currentMPU = NULL;
  if (useSecondMpu) {
    for (int i = 0; i < 2; i++) {
      mpu = (mpu + 1) % 2; // failed attempt at round robin
      currentMPU = mpus.select(mpu);
      if (currentMPU->isDue()) {
        handleMPUevent(mpu);
      }
    }
  } else {
    mpu = 0;
    currentMPU = mpus.select(mpu);
    if (currentMPU->isDue()) {
      handleMPUevent(mpu);
    }
  }
}


Thank you for answering!

P.S: You can find Serial Monitor Output and code file in the attachment.

![grafik.png|1919x1015](upload://5H3Lhve5eLfw5ggG04Toy5gO4jX.png)

[MPU-6050_2.ino|attachment](upload://rsrNd1j3FI6slcYEXdBe22gcdGZ.ino) (11.3 KB)

![grafik.png|1919x1015](upload://5H3Lhve5eLfw5ggG04Toy5gO4jX.png)

[MPU6050_Wrapper.h|attachment](upload://g2W6uJ6xQe9o3Ny1lJFvMaGzzIR.h) (6.43 KB)

[DeathTimer.h|attachment](upload://62C09BG8efqTbJnYA8dj8PR4zGk.h) (2.03 KB)

[TogglePin.h|attachment](upload://vLKWx7IUJpaK7Yyv7ncMnSjRJzY.h) (1.93 KB)

with each sec data is growing up without touching any sensors.

I don't understand what that means, and don't see anything wrong with your printed output.

Get one sensor working, and make sure you understand how it is working, before adding a second one.

Jremington thank you for answering! Each of sensors working fine without any troubels.
In the output angles of both is growing up, but I am not toching sensors (Angle grows up by himself without any reasons)

You have not correctly measured and subtracted the gyro offsets.

As stated, you need to fully understand how ONE sensor can be made to work properly, before trying to work with two.