MPU6050 FIFO overflow and freezing problems

Question

Im using the sensor MPU6050 to control movements of my robotic arm. The codes work fine when it is a standalone program but i keep encountering 'FIFO overflow' when the codes are complied into the main program. This is the code that i am using.

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;

RF24 radio(9, 8);  // CE, CSN

const byte address[6] = "00001";

const int AccReadings = 10;
//Wrist Roll
int DataX[AccReadings];
int WRIndex = 0;
int WRtotal = 0;
int WRaverage = 0;
//Wrist Pitch
int DataY[AccReadings];
int WPIndex = 0;
int WPtotal = 0;
int WPaverage = 0;
//Shoulder Lift
int DataY2[AccReadings];
int SLIndex = 0;
int SLtotal = 0;
int SLaverage = 0;
//Elbow Lift
int ELaverage = 0;
//Arm Rotation
int ARaverage = 0;
float correct;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;

// 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

// 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

struct Sensor_Data
{
  int WristRoll;
  int WristPitch;
  int ShoulderLift;
  int ElbowLift;
  int ArmRotation;
};
Sensor_Data data;

//Interrupt Detection
volatile bool mpuInterrupt = false;
void dmpDataReady()
{
  mpuInterrupt = true;
}

void setup()
{
  radio.begin();
  radio.openWritingPipe(address);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();

  //Zero-fill Arrays
  for (int i = 0; i < AccReadings; i++)
  {
   DataX[i] = 0;
  }
  for (int j = 0; j < AccReadings; j++)
  {
   DataY[j] = 0;
  }
  for (int k = 0; k < AccReadings; k++)
  {
   DataY2[k] = 0;
  }

  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
    Wire.begin();
    Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
    Fastwire::setup(400, true);
  #endif

  Serial.begin(115200);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(49);
  mpu.setYGyroOffset(-18);
  mpu.setZGyroOffset(9);
  mpu.setZAccelOffset(4427);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.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(devStatus);
    //Serial.println(F(")"));
  }
}

void loop()
{
  /*smoothWR();
  movementWR();
  smoothWP();
  movementWP();
  smoothSL();
  movementSL();*/
  ElbowMovement();
  radio.write(&data, sizeof(Sensor_Data));
}

void smoothWR()
{
  WRtotal = WRtotal - DataX[WRIndex];
  DataX[WRIndex] = analogRead(A0);
  WRtotal = WRtotal + DataX[WRIndex];
  WRIndex = WRIndex + 1;

  if (WRIndex >= AccReadings)
  {
    WRIndex = 0;
  }

  WRaverage = WRtotal / AccReadings;
  //Serial.println(WRaverage);
}

void movementWR()
{            
  WRaverage = map(WRaverage, 278, 419, 0, 180);
  data.WristRoll = constrain(WRaverage, 0, 180);                                      
  //Serial.println(data.WristRoll);
}

void smoothWP()
{
  WPtotal = WPtotal - DataY[WPIndex];
  DataY[WPIndex] = analogRead(A1);
  WPtotal = WPtotal + DataY[WPIndex];
  WPIndex = WPIndex + 1;

  if (WPIndex >= AccReadings)
  {
    WPIndex = 0;
  }

  WPaverage = WPtotal / AccReadings;
  //Serial.println(WPaverage);
}

void movementWP()
{            
  WPaverage = map(WPaverage, 280, 421, 0 , 135);
  data.WristPitch = constrain(WPaverage, 0, 135);                                      
  //Serial.println(data.WristPitch);
}

void smoothSL()
{
  SLtotal = SLtotal - DataY2[SLIndex];
  DataY2[SLIndex] = analogRead(A2);
  SLtotal = SLtotal + DataY2[SLIndex];
  SLIndex = SLIndex + 1;

  if (SLIndex >= AccReadings)
  {
    SLIndex = 0;
  }

  SLaverage = SLtotal / AccReadings;
  //Serial.println(SLaverage);
}

void movementSL()
{                       
  SLaverage = map(SLaverage, 410, 270, 0 , 180);
  data.ShoulderLift = constrain(SLaverage, 35, 180);                                   
  //Serial.println(data.ShoulderLift);
}

void ElbowMovement()
{
  // if programming failed, don't try to do anything
  if (!dmpReady) return;

  // wait for MPU interrupt or extra packet(s) available
  while (!mpuInterrupt && fifoCount < packetSize) 
  {
    if (mpuInterrupt && fifoCount < packetSize) 
    {
      // try to get out of the infinite loop
      fifoCount = mpu.getFIFOCount();
    }
  }

  // reset interrupt flag and get INT_STATUS byte
  mpuInterrupt = false;
  mpuIntStatus = mpu.getIntStatus();

  // get current FIFO count
  fifoCount = mpu.getFIFOCount();

  // check for overflow (this should never happen unless our code is too inefficient)
  if ((mpuIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024) 
  {
    // reset so we can continue cleanly
    mpu.resetFIFO();
    fifoCount = mpu.getFIFOCount();
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
  } 
  else if (mpuIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)) 
  {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    // read a packet from FIFO
    mpu.getFIFOBytes(fifoBuffer, packetSize);

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

    // Get Yaw, Pitch and Roll values
#ifdef OUTPUT_READABLE_YAWPITCHROLL
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    // Yaw, Pitch, Roll values - Radians to degrees
    ypr[0] = ypr[0] * 180 / M_PI;
    ypr[1] = ypr[1] * 180 / M_PI;
    ypr[2] = ypr[2] * 180 / M_PI;
    
    // Skip 300 readings (self-calibration process)
    if (int l = 0; l <= 300) {
      correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
      l++;
    }
    // After 300 readings
    else {
      ypr[0] = ypr[0] - correct; // Set the Yaw to 0 deg - subtract  the last random Yaw value from the currrent value to make the Yaw 0 degrees
      // Map the values of the MPU6050 sensor from -90 to 90 to values suatable for the servo control from 0 to 180
      ELaverage = map(ypr[0], -90, 90, 0, 180);
      data.ElbowLift = constrain(ELaverage, 30, 110);
      ARaverage = map(ypr[1], -90, 90, 0, 180);
      data.ArmRotation = constrain(ARaverage, 0, 180);     
      //Serial.println(data.ElbowLift);
      Serial.println(ypr[1]);
    }
#endif
  }
}

Is there any ways to get rid of the FIFO overflow? Also When i tried to used Jeff Rowberg's example codes MPU6050_DMP6 the program will freeze after a few seconds. Is there any solution to that? These are the example codes that i am using.

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
MPU6050 mpu;
float correct;
int j = 0;

#define OUTPUT_READABLE_YAWPITCHROLL
#define INTERRUPT_PIN 2
bool blinkState = false;

// 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

// 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

//Interrupt Detection
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
  mpuInterrupt = true;
}

void setup()
{
  #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
  Wire.begin();
  Wire.setClock(400000);
  #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
  Fastwire::setup(400, true);
  #endif
  Serial.begin(38400);
  while (!Serial);

  mpu.initialize();
  pinMode(INTERRUPT_PIN, INPUT);
  devStatus = mpu.dmpInitialize();
  mpu.setXGyroOffset(17);
  mpu.setYGyroOffset(-69);
  mpu.setZGyroOffset(27);
  mpu.setZAccelOffset(1551);

  if (devStatus == 0) 
  {
    mpu.setDMPEnabled(true);
    attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
    mpuIntStatus = mpu.getIntStatus();
    dmpReady = true;
    packetSize = mpu.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(devStatus);
    //Serial.println(F(")"));
  }
}

void loop()
{
  if (!dmpReady) return;
  if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer))
  {
    #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            if (j <= 300) 
            {
              correct = ypr[0]; // Yaw starts at random value, so we capture last value after 300 readings
              j++;
            }
            else
            {
              ypr[0] = ypr[0] - correct;
              Serial.print("ypr\t");
              Serial.print(ypr[0] * 180/M_PI);
              Serial.print("\t");
              Serial.print(ypr[1] * 180/M_PI);
              Serial.print("\t");
              Serial.println(ypr[2] * 180/M_PI);
            }
    #endif
  }
}

Answer 1

You're using DMP (Digital Motion Processor) with mean calculation running on the MPU itself, this gives more precise and less CPU consumption but you need to update the FIFO frequently or the track goes wrong.

Encountering 'FIFO overflow' means your loop code was too slow, you should increase the speed of another task in your loop code. Or just use other code that not use the DMP.