Jump to content
I2Cdevlib Forums

Recommended Posts

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);
    }
}
Link to comment
Share on other sites

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

Loading...
 Share

×
×
  • Create New...