Go Down

Topic: Three MPU-6050 on Mega with error data. (Read 257 times) previous topic - next topic

teacher1065

Hello

I successfully connect three MPU-6050 on my Mega with using AD0 high and low to switch between

them. And have "most" right result, but somehow, sometimes I will get a little wrong result and FIFO

overflow.

I just line up my MPUs on table, let the code get the YPR, and reform them as string with some code.

But as you can see like in 2583 data, the data in bold is very different from others, and I

never move those MPUs after the Mega started, so it should be close to other data, not like this.

           _______MPU1_______  ___ MPU2________  _____MPU3______
 
2578 : -156.01 +000.80 -000.12  -114.82 -002.11 -000.38  -117.99 +001.38 -001.95
2579 : -156.01 +000.79 -000.12  -114.82 -002.11 -000.38  -117.99 +001.38 -001.94
2580 : -156.01 +000.79 -000.12  -114.82 -002.11 -000.38  -117.99 +001.38 -001.94
2581 : -156.01 +000.80 -000.12  -114.82 -002.11 -000.38  -117.99 +001.38 -001.94
2582 : -156.01 +000.80 -000.13  -114.82 -002.11 -000.38  -117.99 +001.38 -001.94
2583 : -156.01 +000.80 -000.12  -010.90 -175.08 -153.90 -17.99  +001.38-001.94
2584 : -156.01 +000.79 -000.12  -114.82 -002.11 -000.38  -117.99 +001.38 -001.94
2585 : -156.01 +000.79 -000.12  -114.82 -002.11 -000.38  -117.99  +001.39 -001.95


In about every 15 data, there will be one with this strange phenomenon, and I have to use

mpu.resetFIFO() everytime I change the high/low of AD0, or I will get a lot of FIFO overflow message.

But all of this won't happen if I use only one MPU, so I assume maybe I miss something in my code.

I connect 5V GND SDA SCL on Mega to MPU-6050, and connect MPU-6050 with each other in the pin I

mentioned.

AD0 on MPU 1 to Mega pin7
AD0 on MPU 2 to Mega pin6
AD0 on MPU 3 to Mega pin5






Code: [Select]
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL
bool blinkState = false;


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


Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector
int Count;
String Data;

void setup()
{
  pinMode(5, OUTPUT);
  pinMode(6, OUTPUT);
  pinMode(7, OUTPUT);
  Serial.begin(115200);
  /////////////////////////////////////////////////////////////////////////////////
  // join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
#endif
  while (!Serial);
  for (int i = 1; i < 4; i++)
  {
    if (i == 1)
    {
      digitalWrite(5, LOW);
      digitalWrite(6, HIGH);
      digitalWrite(7, HIGH);
    }
    else if (i == 2)
    {
      digitalWrite(5, HIGH);
      digitalWrite(6, LOW);
      digitalWrite(7, HIGH);
    }
    else if (i == 3)
    {
      digitalWrite(5, HIGH);
      digitalWrite(6, HIGH);
      digitalWrite(7, LOW);
    }

    mpu.initialize();
    devStatus = mpu.dmpInitialize();
    mpu.setXGyroOffset(220);
    mpu.setYGyroOffset(76);
    mpu.setZGyroOffset(-85);
    mpu.setZAccelOffset(1788);
    if (devStatus == 0)
    {
      mpu.setDMPEnabled(true);
      mpuIntStatus = mpu.getIntStatus();
      dmpReady = true;
      packetSize = mpu.dmpGetFIFOPacketSize();
    }
    else
    {
      Serial.print(F("DMP Initialization failed (code "));
      Serial.print(devStatus);
      Serial.println(F(")"));
    }
  }
}

void loop()
{
  for (int i = 1 ; i < 4; i++)
  {
    if (i == 1)
    {
      digitalWrite(5, LOW);
      digitalWrite(6, HIGH);
      digitalWrite(7, HIGH);
    }
    else if (i == 2)
    {
      digitalWrite(5, HIGH);
      digitalWrite(6, LOW);
      digitalWrite(7, HIGH);
    }
    else if (i == 3)
    {
      digitalWrite(5, HIGH);
      digitalWrite(6, HIGH);
      digitalWrite(7, LOW);
    }
    if (!dmpReady) return;
    while (fifoCount < packetSize)
    {
      fifoCount = mpu.getFIFOCount();
    }
    mpuIntStatus = mpu.getIntStatus();
    fifoCount = mpu.getFIFOCount();
    if ((mpuIntStatus & 0x10) || fifoCount == 1024)  // check for overflow (this should never happen unless our code is too inefficient)
    {
      mpu.resetFIFO();  // reset so we can continue cleanly
      Serial.println(F("FIFO overflow!"));
      return;
    }
    else if (mpuIntStatus & 0x02) // otherwise, check for DMP data ready interrupt (this should happen frequently)
    {
      while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
      mpu.getFIFOBytes(fifoBuffer, packetSize);
      fifoCount -= packetSize;
#ifdef OUTPUT_READABLE_YAWPITCHROLL
      mpu.dmpGetQuaternion(&q, fifoBuffer);
      mpu.dmpGetGravity(&gravity, &q);
      mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

      //Reform MPU data to strings

      for (int i = 0; i < 3; i++)
      {
        if ((ypr[i] * 180 / M_PI) >= 0)
        {
          Data += "+";
          if ((ypr[i] * 180 / M_PI) / 100 < 1  )
          {
            Data += "0";
            if ((ypr[i] * 180 / M_PI) / 100 < 0.1 )
            {
              Data += "0";
            }
          }
          Data += String((ypr[i] * 180 / M_PI));
        }
        else if ((ypr[i] * 180 / M_PI) < 0)
        {
          Data += "-";
          if ((ypr[i] * 180 / M_PI) / 100 > -1  )
          {
            Data += "0";
            if ((ypr[i] * 180 / M_PI) / 100 > -0.1 )
            {
              Data += "0";
            }
          }
          Data += String((ypr[i] * 180 / M_PI) * -1);
        }
      }
#endif
    }
    mpu.resetFIFO();
  }
  if (Data.length() != 63)
  {
    Data = "";
  }
  else if (Data.length() == 63)
  {
    Count++;
    Serial.print(Count);
    Serial.print(" : ");
    Serial.println(Data);
    Data = "";
  }
}



If someone who can help me, I will be very appreciate.

johnwasser

I would print out the raw data from the fifo to see if the bad data comes from a loss of synchronization.
Send Bitcoin tips to: 1G2qoGwMRXx8az71DVP1E81jShxtbSh5Hp
See who has no social life: https://forum.arduino.cc/index.php?action=stats :)

Go Up