two mpu6050

[hatt0002]

Hi Jeff,
Firstly, I would like to thank you for the nice library 12cdevlib. I am using your library with MPU6050 (GY521) for my school project involving orientation of a wooden beam.. Using your library, I have managed to determine the yaw, pitch and roll of the beam using one mpu6050 and displaying the beam using the teapot demo on processing. Now I need to add another beam, meaning that I need another MPU6050. Please can you tell me how can I modify your code to get two teapot outputs from two separate mpu6050 (eg. teapotPacket1 and teapotPacket2) so that I can display the two beams in processing. I only require the arduino uno sketch, I can manage alone for the processing sketch. Also, please tell me how to connect the two mpu6050 with the arduino uno board. (I am connect the two MPUs in parrallel except that AD0 of one MPU is 3.3V and the other floating)

 

I am first trying to output readable yaw, pitch and roll and FIFO overflow error is occurring at the serial monitor.

 

Thanks in advance

 

 

 

#include "I2Cdev.h"

 

#include "MPU6050_6Axis_MotionApps20.h"

 

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

    #include "Wire.h"

#endif

MPU6050 mpu1(0x69);

MPU6050 mpu2(0x68);

 

//#define OUTPUT_READABLE_QUATERNION //for actual quaternion components in a [w, x, y, z] format

//#define OUTPUT_READABLE_EULER //for Euler angles in degrees

#define OUTPUT_READABLE_YAWPITCHROLL //for yaw, pitch and roll in degrees

//#define OUTPUT_READABLE_REALACCEL //for acceleration components with gravity removed

//#define OUTPUT_READABLE_WORLDACCEL //for acceleration components with gravity removed and adjusted for the world frame reference

//#define OUTPUT_Oar //for Oar demonstation in processing.org

 

 

 

#define LED_PIN 13 

bool blinkState = false;

 

// MPU control/status vars

bool dmpReady1 = false;  // set true if DMP init was successful

uint8_t mpuIntStatus1;   // holds actual interrupt status byte from MPU

uint8_t devStatus1;      // return status after each device operation (0 = success, !0 = error)

uint16_t packetSize1;    // expected DMP packet size (default is 42 bytes)

uint16_t fifoCount1;     // count of all bytes currently in FIFO

uint8_t fifoBuffer1[64]; // FIFO storage buffer

 

// MPU control/status vars

bool dmpReady2 = false;  // set true if DMP init was successful

uint8_t mpuIntStatus2;   // holds actual interrupt status byte from MPU

uint8_t devStatus2;      // 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

 

// orientation/motion vars

Quaternion q1;           // [w, x, y, z]         quaternion container

VectorInt16 aa1;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal1;     // [x, y, z]            gravity-free accel sensor measurements

VectorInt16 aaWorld1;    // [x, y, z]            world-frame accel sensor measurements

VectorFloat gravity1;    // [x, y, z]            gravity vector

float euler1[3];         // [psi, theta, phi]    Euler angle container

float ypr1[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 

// orientation/motion vars

Quaternion q2;           // [w, x, y, z]         quaternion container

VectorInt16 aa2;         // [x, y, z]            accel sensor measurements

VectorInt16 aaReal2;     // [x, y, z]            gravity-free accel sensor measurements

VectorInt16 aaWorld2;    // [x, y, z]            world-frame accel sensor measurements

VectorFloat gravity2;    // [x, y, z]            gravity vector

float euler2[3];         // [psi, theta, phi]    Euler angle container

float ypr2[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

 

 

// packet structure for InvenSense Oar demo

uint8_t OarPacket1[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

uint8_t OarPacket2[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

 

//interrupt detection routine

volatile bool mpuInterrupt1 = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady1() 

{

    mpuInterrupt1 = true;

}

//interrupt detection routine

volatile bool mpuInterrupt2 = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady2() 

{

    mpuInterrupt2 = true;

}

//initial setup

void setup() 

{

    // join I2C bus (I2Cdev library doesn't do this automatically)

    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

        Wire.begin();

        TWBR = 12; // 400kHz I2C clock (200kHz if CPU is 8MHz)

    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

        Fastwire::setup(400, true);

    #endif

 

    // initialize serial communication

    Serial.begin(9600);

    while (!Serial); 

 

    // initialize device

    Serial.println(F("Initializing I2C devices..."));

    mpu1.initialize();

    mpu2.initialize();

    // verify connection

    Serial.println(F("Testing device connections..."));

    Serial.println(mpu1.testConnection() ? F("MPU6050 #1 connection successful") : F("MPU6050 #1 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 #1 ..."));

    devStatus1 = mpu1.dmpInitialize();

    devStatus2 = mpu2.dmpInitialize();

   

    // make sure it worked (returns 0 if so)

    if (devStatus1 == 0 || devStatus2 == 0) {

        // turn on the DMP, now that it's ready

        Serial.println(F("Enabling DMP ..."));

        mpu1.setDMPEnabled(true);

        mpu2.setDMPEnabled(true);

        // enable Arduino interrupt detection

        Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));

        attachInterrupt(0, dmpDataReady1, RISING);

        mpuIntStatus1 = mpu1.getIntStatus();

      attachInterrupt(1, dmpDataReady2, RISING);

        mpuIntStatus2 = mpu2.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..."));

        dmpReady1 = true;

        dmpReady2 = true;

 

        // get expected DMP packet size for later comparison

         packetSize1 = mpu1.dmpGetFIFOPacketSize();

         packetSize2 = mpu2.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(devStatus1);

        Serial.print(devStatus2);

        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 (!dmpReady1) return;

    else if (!dmpReady2) return;

 

    // wait for MPU interrupt or extra packet(s) available

    while (!mpuInterrupt1 && fifoCount1 < packetSize1 && !mpuInterrupt2 && fifoCount2 < packetSize2) {}

 

    // reset interrupt flag and get INT_STATUS byte

    mpuInterrupt1 = false;

    mpuIntStatus1 = mpu1.getIntStatus();

    mpuInterrupt2 = false;

    mpuIntStatus2 = mpu2.getIntStatus();

 

    // get current FIFO count

    fifoCount1 = mpu1.getFIFOCount();

    fifoCount2 = mpu2.getFIFOCount();

 

    // check for overflow (this should never happen unless our code is too inefficient)

    if ((mpuIntStatus1 & 0x10) || fifoCount1 == 1024 ) {

        // reset so we can continue cleanly

        mpu1.resetFIFO();

        Serial.println(F("FIFO #1 overflow!"));

 

    // otherwise, check for DMP data ready interrupt (this should happen frequently)

    } 

     else if ((mpuIntStatus2 & 0x10) || fifoCount2 == 1024 ) {

        // reset so we can continue cleanly

        mpu2.resetFIFO();

        Serial.println(F("FIFO #2 overflow!"));

 

    // otherwise, check for DMP data ready interrupt (this should happen frequently)

    }

    else if (mpuIntStatus1 & 0x02 && mpuIntStatus2 & 0x02) 

    {

        // wait for correct available data length, should be a VERY short wait

        while (fifoCount1 < packetSize1 && fifoCount2 < packetSize2 ) 

        fifoCount1 = mpu1.getFIFOCount();

        fifoCount2 = mpu2.getFIFOCount();

 

        // read a packet from FIFO

        mpu1.getFIFOBytes(fifoBuffer1, packetSize1);

        mpu2.getFIFOBytes(fifoBuffer2, packetSize2);

        // track FIFO count here in case there is > 1 packet available

        // (this lets us immediately read more without waiting for an interrupt)

        fifoCount1 -= packetSize1;

         fifoCount2 -= packetSize2;

 

        

        #ifdef OUTPUT_READABLE_YAWPITCHROLL

            // display Euler angles in degrees

            mpu1.dmpGetQuaternion(&q1, fifoBuffer1);

            mpu1.dmpGetGravity(&gravity1, &q1);

            mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);

            Serial.print("ypr1\t");

            Serial.print(ypr1[0] * 180/M_PI);

            Serial.print("\t");

            Serial.print(ypr1[1] * 180/M_PI);

            Serial.print("\t");

            Serial.println(ypr1[2] * 180/M_PI);

            

            // display Euler angles in degrees

            mpu2.dmpGetQuaternion(&q2, fifoBuffer2);

            mpu2.dmpGetGravity(&gravity2, &q2);

            mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);

            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_Oar

            // display quaternion values in InvenSense Oar demo format:

            OarPacket1[2] = fifoBuffer1[0];

            OarPacket1[3] = fifoBuffer1[1];

            OarPacket1[4] = fifoBuffer1[4];

            OarPacket1[5] = fifoBuffer1[5];

            OarPacket1[6] = fifoBuffer1[8];

            OarPacket1[7] = fifoBuffer1[9];

            OarPacket1[8] = fifoBuffer1[12];

            OarPacket1[9] = fifoBuffer1[13];

            Serial.write(OarPacket1, 14);

            OarPacket1[11]++; // packetCount, loops at 0xFF on purpose

            

             // display quaternion values in InvenSense Oar demo format:

            OarPacket2[2] = fifoBuffer2[0];

            OarPacket2[3] = fifoBuffer2[1];

            OarPacket2[4] = fifoBuffer2[4];

            OarPacket2[5] = fifoBuffer2[5];

            OarPacket2[6] = fifoBuffer2[8];

            OarPacket2[7] = fifoBuffer2[9];

            OarPacket2[8] = fifoBuffer2[12];

            OarPacket2[9] = fifoBuffer2[13];

            Serial.write(OarPacket2, 14);

            OarPacket2[11]++; // packetCount, loops at 0xFF on purpose

            

        #endif

   

 

 

 

        // blink LED to indicate activity

        blinkState = !blinkState;

        digitalWrite(LED_PIN, blinkState);

    }

}