-1

I am reading from two MPU6050 sensors using Arduino Uno but it give trouble in squence. The code is perfect and also sensors reading is done. See image of serial monitor.

Reading of sensors data

You can see the above image in which the output data of serial monitor is not lining up. It breaks the lines after some time interval. I want to expect the serial data in lining. Here is the code:

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"

#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is req1uired if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
//MPU6050 mpu;
MPU6050 mpu1(0x68);
MPU6050 mpu2(0x69); // <-- use for AD0 high


// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the q1uaternions coming
// from the FIFO. Note this also req1uires gravity1 vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_YAWPITCHROLL

int pre_count = 0;

#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
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]         q1uaternion container
VectorInt16 aa1;         // [x, y, z]            accel sensor measurements
VectorInt16 aa1Real1;     // [x, y, z]            gravity1-free accel sensor measurements
VectorInt16 aa1World1;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity1;    // [x, y, z]            gravity1 vector
float euler1[3];         // [psi, theta, phi]    euler1 angle container
float ypr1[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity1 vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket1[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' };

Quaternion q2;           // [w, x, y, z]         q1uaternion container
VectorInt16 aa2;         // [x, y, z]            accel sensor measurements
VectorInt16 aa1Real2;     // [x, y, z]            gravity1-free accel sensor measurements
VectorInt16 aa1World2;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity2;    // [x, y, z]            gravity1 vector
float euler2[3];         // [psi, theta, phi]    euler1 angle container
float ypr2[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity1 vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket2[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;
//  Serial.println("Mpu interrupt true");
}

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 = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif

  // initialize serial communication
  // (115200 chosen because it is req1uired for Teapot Demo output, but it's
  // really up to you depending on your project)
  Serial.begin(115200);
  while (!Serial); // wait for Leonardo enumeration, others continue immediately

  // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio
  // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to
  // the baud timing being too misaligned with processor ticks. You must use
  // 38400 or slower in these cases, or use some kind of external separate
  // crystal solution for the UART timer.

  // initialize device
  Serial.println(F("Initializing I2C devices 1..."));
  mpu1.initialize();

   // initialize device
  Serial.println(F("Initializing I2C devices 2..."));
  mpu2.initialize();

  // verify connection
  Serial.println(F("Testing device 1 connections..."));
  Serial.println(mpu1.testConnection() ? F("MPU6050 1 connection successful") : F("MPU6050 1 connection failed"));

  // verify connection
  Serial.println(F("Testing device 2 connections..."));
  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();

  // load and configure the DMP
  Serial.println(F("Initializing DMP 2..."));
  devStatus2 = mpu2.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu1.setXGyroOffset(220);
  mpu1.setYGyroOffset(76);
  mpu1.setZGyroOffset(-85);
  mpu1.setZAccelOffset(1788); // 1688 factory default for my test chip

  // supply your own gyro offsets here, scaled for min sensitivity
  mpu2.setXGyroOffset(74);
  mpu2.setYGyroOffset(14);
  mpu2.setZGyroOffset(-19);
  mpu2.setZAccelOffset(253); // 1688 factory default for my test chip

  // make sure it worked (returns 0 if so)
  if (devStatus1 == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP 1..."));
    mpu1.setDMPEnabled(true);


    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady1, RISING);
    mpuIntStatus1 = mpu1.getIntStatus();


    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP 1 ready! Waiting for first interrupt..."));
    dmpReady1 = true;


    // get expected DMP packet size for later comparison
    packetSize1 = mpu1.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 1 Initialization failed (code "));
    Serial.print(devStatus1);
    Serial.println(F(")"));
  }

  // make sure it worked (returns 0 if so)
  if (devStatus2 == 0) {
    // turn on the DMP, now that it's ready
    Serial.println(F("Enabling DMP 2..."));
    mpu2.setDMPEnabled(true);


    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, 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 2 ready! Waiting for first interrupt..."));
    dmpReady2 = true;


    // get expected DMP packet size for later comparison
    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 2 Initialization failed (code "));
    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;

  // if programming failed, don't try to do anything
  if (!dmpReady2) return;

// Serial.println("code access here 1");
  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt1 && fifoCount1 < packetSize1) {
    // other program behavior stuff here
    // .
    // .
    // .
    // if you are really paranoid you can freq1uently test in between other
    // stuff to see if mpuInterrupt1 is true, and if so, "break;" from the
    // while() loop to immediately process the MPU data
    // .
    // .
    // .
     break;

//    Serial.println("code access here 2");
  }

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt2 && fifoCount2 < packetSize2) {
    // other program behavior stuff here
    // .
    // .
    // .
    // if you are really paranoid you can freq1uently test in between other
    // stuff to see if mpuInterrupt1 is true, and if so, "break;" from the
    // while() loop to immediately process the MPU data
    // .
    // .
    // .
    break;
  }



  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt1 = false;
  mpuIntStatus1 = mpu1.getIntStatus();

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt2 = false;
  mpuIntStatus2 = mpu2.getIntStatus();

  // get current FIFO count
  fifoCount1 = mpu1.getFIFOCount();

  // get current FIFO count
  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 overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen freq1uently)
  } else if (mpuIntStatus1 & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount1 < packetSize1) fifoCount1 = mpu1.getFIFOCount();

    // read a packet from FIFO
    mpu1.getFIFOBytes(fifoBuffer1, packetSize1);

    // track FIFO count here in case there is > 1 packet available
    // (this lets us immediately read more without waiting for an interrupt)
    fifoCount1 -= packetSize1;


#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display euler1 angles in degrees
    mpu1.dmpGetQuaternion(&q1, fifoBuffer1);
    mpu1.dmpGetGravity(&gravity1, &q1);
    mpu1.dmpGetYawPitchRoll(ypr1, &q1, &gravity1);
    int count = 0;
    count++;
    pre_count += count;
    Serial.print(pre_count);
    Serial.print("\t");
    Serial.print(ypr1[0] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr1[1] * 180 / M_PI);
    Serial.print("\t");
    Serial.print(ypr1[2] * 180 / M_PI);
    Serial.print("\t");
#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 ((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 freq1uently)
  } else if (mpuIntStatus2 & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount2 < packetSize2) fifoCount2 = mpu2.getFIFOCount();

    // read a packet from FIFO
    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)
    fifoCount2 -= packetSize2;


#ifdef OUTPUT_READABLE_YAWPITCHROLL
    // display euler1 angles in degrees
    mpu2.dmpGetQuaternion(&q2, fifoBuffer2);
    mpu2.dmpGetGravity(&gravity2, &q2);
    mpu2.dmpGetYawPitchRoll(ypr2, &q2, &gravity2);
    Serial.print("\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


//    // blink LED to indicate activity
//    blinkState = !blinkState;
//    digitalWrite(LED_PIN, blinkState);
  }

   // blink LED to indicate activity
    blinkState = !blinkState;
    digitalWrite(LED_PIN, blinkState);
}

What is the problem? Thanks in advance!

  • 1
    I totally don't understand what you mean by "trouble in sequence". Please explain more. What are you expecting and what is actually happening? – chrisl Aug 29 '19 at 18:33
  • @chrisl you can see the above image in which the output data of serial monitor is not lining up. It breaks the lines after some time interval. I want to expect the serial data in lining. – Yawar Aug 30 '19 at 15:04
0

Both printing codes are inside of different if statements, which involve testing mpuIntStatus1 and mpuIntStatus2. This works good, as long as in one void loop() iteration both of these if statements are executed. But that's a timing issue and you cannot assume, that these things happen regularly and always this way. Without completely analyzing your code, the variables mpuIntStatus1/2 seem to indicate, that new data arrived. There is no reason, why this should be synchronized with the void loop() iterations.

To solve this you should rewrite your code to include all these print statements into one single if statement at the end of void loop(). In the if statements from above you can set a flag to indicate, that data arrived. This flag will be tested by the if statement at the end of loop. It is up to you to decide, how this should behave, when only data from 1 sensor arrived. You can only print the values from this sensor, or you can print both and using dummy values, that can be distinguished by humans (for example all values zero). Or you can choose to only print out values, if both sensors have given you results.

| improve this answer | |

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.