adding another MPU6050 to the program

Hello everyone,

I've been trying to add another MPU6050 to that specific code below, but I couldn't do it.

I have tried to address the two mpu to different I2C adresses, created new variables for the second mpu and then just added them to the program, but it was unsuccessfull. So, I guess it's not that simple.

Could anyone tell me the exact changes I have to make in this specific code in order to add another MPU6050 so it could read the values from the two MPU6050 separately?

ps: I know there are a lot of different examples of DMP MPU6050 but this one it's the one that worked better for me.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
// in "MPU6050_6Axis_MotionApps20.h"
// I'm now using the default value on line 305 to:  0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate
// Correcting the PID code fixed my issues with the fifo buffer being too fast
#include "Wire.h"

MPU6050 mpu;

int MPUOffsets[6] = {  -1062,     -70,    1179,     128,     100,      42};

#define LED_PIN 13 // 

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected
// MPU control/status vars
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
float Yaw, Pitch, Roll; // in degrees



void MPU6050Connect() {
  static int MPUInitCntr = 0;
  // initialize device
  mpu.initialize(); // same
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();// same

  if (devStatus != 0) {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)

    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }

  mpu.setXAccelOffset(MPUOffsets[0]);
  mpu.setYAccelOffset(MPUOffsets[1]);
  mpu.setZAccelOffset(MPUOffsets[2]);
  mpu.setXGyroOffset(MPUOffsets[3]);
  mpu.setYGyroOffset(MPUOffsets[4]);
  mpu.setZGyroOffset(MPUOffsets[5]);

  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection

  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  attachInterrupt(0, dmpDataReady, FALLING); //pin 2 on the Uno

  mpuIntStatus = mpu.getIntStatus(); // Same
  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus();
  mpuInterrupt = false; // wait for next interrupt

}

void i2cSetup() {
  // 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
}



void setup() {
  Serial.begin(115200); //115200
  while (!Serial);

  i2cSetup();

  Serial.println(F("Alive"));
  MPU6050Connect();
  pinMode(LED_PIN, OUTPUT); // LED Blinks when you are recieving FIFO packets from your MPU6050

}

void loop() {
  if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
    GetDMP(); // Gets the MPU Data and canculates angles
  }

  static long QTimer = millis();
  if ((long)( millis() - QTimer ) >= 100) {
    QTimer = millis();
    Serial.print(F("\t Yaw")); Serial.print(Yaw);
    Serial.print(F("\t Pitch ")); Serial.print(Pitch);
    Serial.print(F("\t Roll ")); Serial.print(Roll);
    Serial.println();
  }
}



void GetDMP() { // Best version I have made so far
  // Serial.println(F("FIFO interrupt at:"));
  // Serial.println(micros());
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  /*
  fifoCount is a 16-bit unsigned value. Indicates the number of bytes stored in the FIFO buffer.
  This number is in turn the number of bytes that can be read from the FIFO buffer and it is
  directly proportional to the number of samples available given the set of sensor data bound
  to be stored in the FIFO
  */

  // PacketSize = 42; refference in MPU6050_6Axis_MotionApps20.h Line 527
  // FIFO Buffer Size = 1024;
  uint16_t MaxPackets = 20;// 20*42=840 leaving us with  2 Packets (out of a total of 24 packets) left before we overflow.
  // If we overflow the entire FIFO buffer will be corrupt and we must discard it!

  // At this point in the code FIFO Packets should be at 1 99% of the time if not we need to look to see where we are skipping samples.
  if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) { // we have failed Reset and wait till next time!
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    Serial.println(F("Reset FIFO"));
    if (fifoCount % packetSize) Serial.print(F("\t Packet corruption")); // fifoCount / packetSize returns a remainder... Not good! This should never happen if all is well.
    Serial.print(F("\tfifoCount ")); Serial.print(fifoCount);
    Serial.print(F("\tpacketSize ")); Serial.print(packetSize);

    mpuIntStatus = mpu.getIntStatus(); // reads MPU6050_RA_INT_STATUS       0x3A
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpuIntStatus , BIN);
    // MPU6050_RA_INT_STATUS       0x3A
    //
    // Bit7, Bit6, Bit5, Bit4          , Bit3       , Bit2, Bit1, Bit0
    // ----, ----, ----, FIFO_OFLOW_INT, I2C_MST_INT, ----, ----, DATA_RDY_INT

    /*
    Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
    Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
    Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
    */
    if (mpuIntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpuIntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpuIntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu.resetFIFO();// clear the buffer and start over
    mpu.getIntStatus(); // make sure status is cleared we will read it again.
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      fifoCount -= packetSize;
    }
    MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
    if (fifoCount > 0) mpu.resetFIFO(); // clean up any leftovers Should never happen! but lets start fresh if we need to. this should never happen.
  }
}
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180 / M_PI);
  Pitch = (ypr[1] * 180 / M_PI);
  Roll = (ypr[2] * 180 / M_PI);
}

MPU6050_TestCode.ino (9.79 KB)

pedrohortiz:
I have tried to address the two mpu to different I2C adresses, created new variables for the second mpu and then just added them to the program, but it was unsuccessfull. So, I guess it's not that simple.

using the AD0 pin you should be able to have two devices with I2C addresses pin LOW 0x68 or HIGH 0x69

using the AD0 pin you should be able to have two devices with I2C addresses pin LOW 0x68 or HIGH 0x69

I have already done that.

The problem I'm facing seems to be with the changes that needs to be made to the code itself for the program be able to read the two MPU6050.

I just need to know what are these modifications and where to put them in the code.

You need to make two of these objects, named differently and initialized with different I2C addresses, etc.

MPU6050 mpu;

the MPU6050 constructor defaults to address 0x68 but it is wise when using two devices to explicitly specify the addresses, e.g.

MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

horace:
the MPU6050 constructor defaults to address 0x68 but it is wise when using two devices to explicitly specify the addresses, e.g.

MPU6050 mpu1(0x68);

MPU6050 mpu2(0x69);

Besides creates the two mpu objects, specifies their adresses and initializes both of them

mpu1.initialize();
mpu2.initialize();

Are there other situations in the code that I need to specify the second mpu?

Like, on every line in the code that appears "mpu x" do I need to change it and specify the two mpu, like "mpu1 x" and "mpu2 x"?

Do I need to do that in some variables as well, like

uint8_t mpu1IntStatus;
uint8_t mpu2IntStatus;

And what changes need to be made in these variables:

float euler[3];        
float ypr[3];           
float Yaw, Pitch, Roll;

With two or more sensors, every time you read or interact with a sensor, you need to specify which one.

If you want to store values for later data averaging or other operations, then there must be a separate set of values for each sensor. For example, you need two sets of these:

int MPUOffsets[6] = {  -1062,     -70,    1179,     128,     100,      42};

Choose your variable names wisely.

pedrohortiz:
Are there other situations in the code that I need to specify the second mpu?

yes, any statement where you are communicating with a particular mpu, e.g.

 mpu1.setXAccelOffset(MPUOffsets[0]);
  mpu2.setXAccelOffset(MPUOffsets[0]);

it is probably wise to have seperate variables used by the mpus such as euler1[3], ypr1[3], euler2[3], ypr2[3]; etc

horace:
yes, any statement where you are communicating with a particular mpu, e.g.

I tried to do that, the program compiles but still don't work.

See the code below and a print of the monitor serial attached and tell me what I did wrong, please.

Note that in one line of the monitor serial there are the values of the ypr of the two mpu as if they are exactly the same.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

int MPUOffsets1[6] = {  -1062,     -70,    1179,     128,     100,      42};
int MPUOffsets2[6] = {  -1038,     -45,    1112,      87,     110,      33};

#define LED_PIN 13  

volatile bool mpu1Interrupt = false; 
volatile bool mpu2Interrupt = false;
void dmpDataReady() {
  mpu1Interrupt = true;
  mpu2Interrupt = true;
}

int FifoAlive = 0; 
int IsAlive = -20;     

uint8_t mpu1IntStatus;  
uint8_t mpu2IntStatus;
uint8_t dev1Status;      
uint8_t dev2Status;
uint16_t packetSize;    
uint16_t fifoCount;     
uint8_t fifoBuffer[64]; 

Quaternion q;           
VectorInt16 aa;         
VectorInt16 aaReal;     
VectorInt16 aaWorld;    
VectorFloat gravity;    
float euler1[3];         
float euler2[3];
float ypr1[3];           
float ypr2[3];
float Yaw1, Pitch1, Roll1;
float Yaw2, Pitch2, Roll2;

void MPU6050Connect() {
  static int MPUInitCntr = 0;
  
  mpu1.initialize(); // same
  mpu2.initialize();
  
  dev1Status = mpu1.dmpInitialize();
  dev2Status = mpu2.dmpInitialize();

  if (dev1Status != 0) {
    
    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[dev1Status]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; 
    delay(1000);
    MPU6050Connect(); 
    return;
  }
  if (dev2Status != 0) {

    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[dev2Status]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }

  mpu1.setXAccelOffset(MPUOffsets1[0]);
  mpu1.setYAccelOffset(MPUOffsets1[1]);
  mpu1.setZAccelOffset(MPUOffsets1[2]);
  mpu1.setXGyroOffset(MPUOffsets1[3]);
  mpu1.setYGyroOffset(MPUOffsets1[4]);
  mpu1.setZGyroOffset(MPUOffsets1[5]);

  mpu2.setXAccelOffset(MPUOffsets2[0]);
  mpu2.setYAccelOffset(MPUOffsets2[1]);
  mpu2.setZAccelOffset(MPUOffsets2[2]);
  mpu2.setXGyroOffset(MPUOffsets2[3]);
  mpu2.setYGyroOffset(MPUOffsets2[4]);
  mpu2.setZGyroOffset(MPUOffsets2[5]);

  Serial.println(F("Enabling DMP..."));
  mpu1.setDMPEnabled(true);
  mpu2.setDMPEnabled(true);

  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 and pin 3 on the Uno)..."));
  attachInterrupt(digitalPinToInterrupt(2), dmpDataReady, FALLING);
  attachInterrupt(digitalPinToInterrupt(3), dmpDataReady, FALLING);

  mpu1IntStatus = mpu1.getIntStatus();
  mpu2IntStatus = mpu2.getIntStatus();
  
  packetSize = mpu1.dmpGetFIFOPacketSize();
  packetSize = mpu2.dmpGetFIFOPacketSize();
  delay(1000);
  mpu1.resetFIFO();
  mpu2.resetFIFO();
  mpu1.getIntStatus();
  mpu2.getIntStatus();
  mpu1Interrupt = false;
  mpu2Interrupt = false;

}

void i2cSetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24; 
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
}

void setup() {
  Serial.begin(115200);
  while (!Serial);

  i2cSetup();

  Serial.println(F("Alive"));
  MPU6050Connect();
  pinMode(LED_PIN, OUTPUT);

}

void loop() {
  if (mpu1Interrupt && mpu2Interrupt ) { 
    GetDMP(); 
  }

  static long QTimer = millis();
  if ((long)( millis() - QTimer ) >= 100) {
    QTimer = millis();
    Serial.print(F("\t Yaw1 ")); Serial.print(Yaw1);
    Serial.print(F("\t Pitch1 ")); Serial.print(Pitch1);
    Serial.print(F("\t Roll1 ")); Serial.print(Roll1);
    Serial.print(F("\t Yaw2 ")); Serial.print(Yaw2);
    Serial.print(F("\t Pitch2 ")); Serial.print(Pitch2);
    Serial.print(F("\t Roll2 ")); Serial.print(Roll2);
    Serial.println();
  }
}

void GetDMP() { 

  mpu1Interrupt = false;
  mpu2Interrupt = false;
  FifoAlive = 1;
  fifoCount = mpu1.getFIFOCount();
  fifoCount = mpu2.getFIFOCount();
  
  uint16_t MaxPackets = 20;// 20*42=840 leaving us with  2 Packets (out of a total of 24 packets) left before we overflow.

  if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) { 
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    Serial.println(F("Reset FIFO"));
    if (fifoCount % packetSize) Serial.print(F("\t Packet corruption")); 
    Serial.print(F("\tfifoCount ")); Serial.print(fifoCount);
    Serial.print(F("\tpacketSize ")); Serial.print(packetSize);

    mpu1IntStatus = mpu1.getIntStatus(); // reads MPU6050_RA_INT_STATUS       0x3A
    mpu2IntStatus = mpu2.getIntStatus();
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpu1IntStatus , BIN);
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpu2IntStatus , BIN);

    /*
    Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
    Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
    Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
    */
    if (mpu1IntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpu1IntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpu1IntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu1.resetFIFO();// clear the buffer and start over
    mpu1.getIntStatus(); // make sure status is cleared we will read it again.
    if (mpu2IntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpu2IntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpu2IntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu2.resetFIFO();// clear the buffer and start over
    mpu2.getIntStatus(); // make sure status is cleared we will read it again.
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
      mpu1.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      mpu2.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
    }
    MPUMath();
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
    if (fifoCount > 0) mpu1.resetFIFO(); mpu2.resetFIFO(); // clean up any leftovers Should never happen! but lets start fresh if we need to. this should never happen.
  }
}

void MPUMath() {
  mpu1.dmpGetQuaternion(&q, fifoBuffer);
  mpu1.dmpGetGravity(&gravity, &q);
  mpu1.dmpGetYawPitchRoll(ypr1, &q, &gravity);
  Yaw1 = (ypr1[0] * 180 / M_PI);
  Pitch1 = (ypr1[1] * 180 / M_PI);
  Roll1 = (ypr1[2] * 180 / M_PI);

  mpu2.dmpGetQuaternion(&q, fifoBuffer);
  mpu2.dmpGetGravity(&gravity, &q);
  mpu2.dmpGetYawPitchRoll(ypr2, &q, &gravity);
  Yaw2 = (ypr2[0] * 180 / M_PI);
  Pitch2 = (ypr2[1] * 180 / M_PI);
  Roll2 = (ypr2[2] * 180 / M_PI);
}

2MPU6050_TestCode.ino (12.5 KB)

Is there a good reason that AD0 is connected to GND through a resistor?

It won't work to have two different interrupt pins, connected to two different device interrupt outputs, call the same interrupt routine.

void dmpDataReady() {
  mpu1Interrupt = true;
  mpu2Interrupt = true;
}

jremington:
Is there a good reason that AD0 is connected to GND through a resistor?

I believe there isn't any. I just used this picture as an example.

In fact and differently from the picture, I didn't do the mentioned connection and I connected the INT pin of the two mpu in the 2 and 3 digital pin of the arduino (I forgot to mention that above).

I connected the INT pin of the two mpu in the 2 and 3 digital pin of the arduino (I forgot to mention that above).

Yes, of course, and you need TWO interrupt routines, one for each sensor.

I just used this picture as an example.

So, you gave us someone else's wrong wiring diagram and expect us to fix the problems with your project? What an excellent idea!

So, you gave us someone else's wrong wiring diagram and expect us to fix the problems with your project? What an excellent idea!

Sorry if the wiring diagram confused you.

I believe the problem with my project doesn't have to do with the wiring (so my mistake for posting about that) but with the code writing.

As I was told,I specified the two mpu in any statement in the code I believed it was necessary. The program compiles but still it doesn't read the values of the two mpu separately and as I don't have much programming knowledge I, for sure, did something wrong in that process.

So, if you or anyone could check the code above, point out to me the mistakes and tell me like what and how I should've written I will really appreciate.

Did you read these comments in the earlier replies, pointing out a serious error in your program?

It won't work to have two different interrupt pins, connected to two different device interrupt outputs, call the same interrupt routine.

Code:

void dmpDataReady() {
mpu1Interrupt = true;
mpu2Interrupt = true;
}

you need TWO interrupt routines, one for each sensor.

jremington:
Did you read these comments in the earlier replies, pointing out a serious error in your program?

I called the same interrupt pin routine but the program still can't read the values from the two mpu's.

Now my monitor serial is like as shown below.

Do you have any clue of what might be the problem this time?

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69);

int MPUOffsets1[6] = {  -1062,     -70,    1179,     128,     100,      42};
int MPUOffsets2[6] = {  -1038,     -45,    1112,      87,     110,      33};

#define LED_PIN 13  

volatile bool mpuInterrupt = false;
void dmpDataReady() {
  mpuInterrupt = true;
}

int FifoAlive = 0;
int IsAlive = -20;    

uint8_t mpu1IntStatus;  
uint8_t mpu2IntStatus;
uint8_t dev1Status;      
uint8_t dev2Status;
uint16_t packetSize;    
uint16_t fifoCount;    
uint8_t fifoBuffer[64];

Quaternion q;          
VectorInt16 aa;        
VectorInt16 aaReal;    
VectorInt16 aaWorld;    
VectorFloat gravity;    
float euler1[3];        
float euler2[3];
float ypr1[3];          
float ypr2[3];
float Yaw1, Pitch1, Roll1;
float Yaw2, Pitch2, Roll2;

void MPU6050Connect() {
  static int MPUInitCntr = 0;
  
  mpu1.initialize(); // same
  mpu2.initialize();
  
  dev1Status = mpu1.dmpInitialize();
  dev2Status = mpu2.dmpInitialize();

  if (dev1Status != 0) {
    
    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[dev1Status]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return;
    delay(1000);
    MPU6050Connect();
    return;
  }
  if (dev2Status != 0) {

    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};

    MPUInitCntr++;

    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[dev2Status]);
    Serial.println(F(")"));

    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }

  mpu1.setXAccelOffset(MPUOffsets1[0]);
  mpu1.setYAccelOffset(MPUOffsets1[1]);
  mpu1.setZAccelOffset(MPUOffsets1[2]);
  mpu1.setXGyroOffset(MPUOffsets1[3]);
  mpu1.setYGyroOffset(MPUOffsets1[4]);
  mpu1.setZGyroOffset(MPUOffsets1[5]);

  mpu2.setXAccelOffset(MPUOffsets2[0]);
  mpu2.setYAccelOffset(MPUOffsets2[1]);
  mpu2.setZAccelOffset(MPUOffsets2[2]);
  mpu2.setXGyroOffset(MPUOffsets2[3]);
  mpu2.setYGyroOffset(MPUOffsets2[4]);
  mpu2.setZGyroOffset(MPUOffsets2[5]);

  Serial.println(F("Enabling DMP..."));
  mpu1.setDMPEnabled(true);
  mpu2.setDMPEnabled(true);

  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 and pin 3 on the Uno)..."));
  attachInterrupt(digitalPinToInterrupt(2), dmpDataReady, FALLING);

  mpu1IntStatus = mpu1.getIntStatus();
  mpu2IntStatus = mpu2.getIntStatus();
  
  packetSize = mpu1.dmpGetFIFOPacketSize();
  packetSize = mpu2.dmpGetFIFOPacketSize();
  delay(1000);
  mpu1.resetFIFO();
  mpu2.resetFIFO();
  mpu1.getIntStatus();
  mpu2.getIntStatus();
  mpuInterrupt = false;

}

void i2cSetup() {
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  TWBR = 24;
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
}

void setup() {
  Serial.begin(115200);
  while (!Serial);

  i2cSetup();

  Serial.println(F("Alive"));
  MPU6050Connect();
  pinMode(LED_PIN, OUTPUT);

}

void loop() {
  if (mpuInterrupt ) {
    GetDMP();
  }

  static long QTimer = millis();
  if ((long)( millis() - QTimer ) >= 100) {
    QTimer = millis();
    Serial.print(F("\t Yaw1 ")); Serial.print(Yaw1);
    Serial.print(F("\t Pitch1 ")); Serial.print(Pitch1);
    Serial.print(F("\t Roll1 ")); Serial.print(Roll1);
    Serial.print(F("\t Yaw2 ")); Serial.print(Yaw2);
    Serial.print(F("\t Pitch2 ")); Serial.print(Pitch2);
    Serial.print(F("\t Roll2 ")); Serial.print(Roll2);
    Serial.println();
  }
}

void GetDMP() {

  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu1.getFIFOCount();
  fifoCount = mpu2.getFIFOCount();
  
  uint16_t MaxPackets = 20;// 20*42=840 leaving us with  2 Packets (out of a total of 24 packets) left before we overflow.

  if ((fifoCount % packetSize) || (fifoCount > (packetSize * MaxPackets)) || (fifoCount < packetSize)) {
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    Serial.println(F("Reset FIFO"));
    if (fifoCount % packetSize) Serial.print(F("\t Packet corruption"));
    Serial.print(F("\tfifoCount ")); Serial.print(fifoCount);
    Serial.print(F("\tpacketSize ")); Serial.print(packetSize);

    mpu1IntStatus = mpu1.getIntStatus(); // reads MPU6050_RA_INT_STATUS       0x3A
    mpu2IntStatus = mpu2.getIntStatus();
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpu1IntStatus , BIN);
    Serial.print(F("\tMPU Int Status ")); Serial.print(mpu2IntStatus , BIN);

    /*
    Bit4 FIFO_OFLOW_INT: This bit automatically sets to 1 when a FIFO buffer overflow interrupt has been generated.
    Bit3 I2C_MST_INT: This bit automatically sets to 1 when an I2C Master interrupt has been generated. For a list of I2C Master interrupts, please refer to Register 54.
    Bit1 DATA_RDY_INT This bit automatically sets to 1 when a Data Ready interrupt is generated.
    */
    if (mpu1IntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpu1IntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpu1IntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu1.resetFIFO();// clear the buffer and start over
    mpu1.getIntStatus(); // make sure status is cleared we will read it again.
    if (mpu2IntStatus & B10000) { //FIFO_OFLOW_INT
      Serial.print(F("\tFIFO buffer overflow interrupt "));
    }
    if (mpu2IntStatus & B1000) { //I2C_MST_INT
      Serial.print(F("\tSlave I2c Device Status Int "));
    }
    if (mpu2IntStatus & B1) { //DATA_RDY_INT
      Serial.print(F("\tData Ready interrupt "));
    }
    Serial.println();
    //I2C_MST_STATUS
    //PASS_THROUGH, I2C_SLV4_DONE,I2C_LOST_ARB,I2C_SLV4_NACK,I2C_SLV3_NACK,I2C_SLV2_NACK,I2C_SLV1_NACK,I2C_SLV0_NACK,
    mpu2.resetFIFO();// clear the buffer and start over
    mpu2.getIntStatus(); // make sure status is cleared we will read it again.
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      if (fifoCount < packetSize) break; // Something is left over and we don't want it!!!
      mpu1.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      mpu2.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
    }
    MPUMath();
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
    if (fifoCount > 0) mpu1.resetFIFO(); mpu2.resetFIFO(); // clean up any leftovers Should never happen! but lets start fresh if we need to. this should never happen.
  }
}

void MPUMath() {
  mpu1.dmpGetQuaternion(&q, fifoBuffer);
  mpu1.dmpGetGravity(&gravity, &q);
  mpu1.dmpGetYawPitchRoll(ypr1, &q, &gravity);
  Yaw1 = (ypr1[0] * 180 / M_PI);
  Pitch1 = (ypr1[1] * 180 / M_PI);
  Roll1 = (ypr1[2] * 180 / M_PI);

  mpu2.dmpGetQuaternion(&q, fifoBuffer);
  mpu2.dmpGetGravity(&gravity, &q);
  mpu2.dmpGetYawPitchRoll(ypr2, &q, &gravity);
  Yaw2 = (ypr2[0] * 180 / M_PI);
  Pitch2 = (ypr2[1] * 180 / M_PI);
  Roll2 = (ypr2[2] * 180 / M_PI);
}

Hi,
Where is your schematic?

Is the a reason you need to use interrupts?
Why can't you just poll each module?

Tom... :slight_smile:

TomGeorge:
Is the a reason you need to use interrupts?

The sketch I'm using requires the use of interrupts.
Did you check the code? Do you think is that the problem?

TomGeorge:
Why can't you just poll each module?

I didn't understand, sorry.