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); } }