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