Unstable output with two MPU6050s

[ivaylopg]

So after struggling for a long time trying to set up two MPU5060s on one Arduino, I think I'm almost there. I can run the Teapot demo code on each single sensor perfectly, and I get nice smooth output.

 

My problem is that when I add the second sensor and use the code below, the output from both suddenly becomes very unstable, frequently jumping to some erratic value and back again. At first I thought it had something to do with how I was receiving the serial data in processing, maybe some values getting misaligned, but after changing the Arduino's output to something more human-readable and watching it in the serial monitor, it seems like the Arduino is actually outputting this erratic data.

 

Anybody have any thoughts?

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

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

#define OUTPUT_TEAPOT
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;

// First 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

// First MPU 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


// Second MPU control/status vars
bool dmpReady2 = false;  // set true if DMP init was successful
uint8_t mpu2IntStatus;   // holds actual interrupt status byte from MPU
uint8_t dev2Status;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize2;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount2;     // count of all bytes currently in FIFO
uint8_t fifoBuffer2[64]; // FIFO storage buffer

// Second MPU orientation/motion vars
Quaternion q2;           // [w, x, y, z]         quaternion container
VectorInt16 aa2;         // [x, y, z]            accel sensor measurements
VectorInt16 aa2Real;     // [x, y, z]            gravity2-free accel sensor measurements
VectorInt16 aa2World;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity2;    // [x, y, z]            gravity2 vector
float euler2[3];         // [psi, theta, phi]    Euler angle container
float ypr2[3];           // [yaw2, pitch2, roll2]   yaw2/pitch2/roll2 container and gravity2 vector


// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
uint8_t teapotPacket2[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;
}
volatile bool mpu2Interrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady2() {
    mpu2Interrupt = true;
}


// ================================================================
// ===                      INITIAL SETUP                       ===
// ================================================================

void setup() {
    // join I2C bus (I2Cdev library doesn't do this automatically)
    Wire.begin();
    
    Serial.begin(38400);
    while (!Serial); // wait for Leonardo enumeration, others continue immediately

    mpu.initialize();
    mpu2.initialize();

    // 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
    devStatus = mpu.dmpInitialize();
    dev2Status = mpu2.dmpInitialize();
 
    if (devStatus == 0) {
        mpu.setDMPEnabled(true);
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();
        dmpReady = true;
        packetSize = mpu.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
    }
    if (dev2Status == 0) {
        mpu2.setDMPEnabled(true);
        attachInterrupt(1, dmpDataReady2, RISING);
        mpu2IntStatus = mpu2.getIntStatus();
        dmpReady2 = true;
        packetSize2 = mpu2.dmpGetFIFOPacketSize();
    } else {
        // ERROR!
    }

    // 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;
    if (!dmpReady2) return;

    // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {
        // other program behavior stuff here
    }

    // reset interrupt flag and get INT_STATUS byte
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();
    mpu2Interrupt = false;
    mpu2IntStatus = mpu2.getIntStatus();

    // get current FIFO count
    fifoCount = mpu.getFIFOCount();
    fifoCount2 = mpu2.getFIFOCount();

    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        mpu.resetFIFO();
    } else if (mpuIntStatus & 0x02) {
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        fifoCount -= packetSize;
    
        #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);
    }
    
    // check for overflow (this should never happen unless our code is too inefficient)
    if ((mpu2IntStatus & 0x10) || fifoCount2 == 1024) {
        mpu2.resetFIFO();
    } else if (mpu2IntStatus & 0x02) {
        while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();
        mpu2.getFIFOBytes(fifoBuffer2, packetSize2);
        fifoCount2 -= packetSize2;

          
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket2[2] = fifoBuffer2[0];
            teapotPacket2[3] = fifoBuffer2[1];
            teapotPacket2[4] = fifoBuffer2[4];
            teapotPacket2[5] = fifoBuffer2[5];
            teapotPacket2[6] = fifoBuffer2[8];
            teapotPacket2[7] = fifoBuffer2[9];
            teapotPacket2[8] = fifoBuffer2[12];
            teapotPacket2[9] = fifoBuffer2[13];
            Serial.write(teapotPacket2, 14);
            teapotPacket2[11]++; // packetCount, loops at 0xFF on purpose
        #endif
        
        // blink LED to indicate activity
        //blinkState = !blinkState;
        //digitalWrite(LED_PIN, blinkState);
    }
    
}

[Jeff Rowberg]

How fast are you running each DMP, and can you tell whether the FIFO is overflowing due to trying to do too much at once? Can you try cutting down the DMP output rate? See line 261 on the MotionApps21 include file:

 

https://github.com/jrowberg/i2cdevlib/blob/master/Arduino/MPU6050/MPU6050_6Axis_MotionApps20.h#L261

    0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate

    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

    // It is important to make sure the host processor can keep up with reading and processing
    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.