How to clear FIFO buffer on MPU6050?

Question

my MPU6050 works very unstable , the problem is unable clear excessed FIFO buffer on MPU6050, I am using Jeff Rowberg's MPU library and according to his DMP example but without using interruptions,here is my modified code but I've tried several method listed in the library to clear buffer but doesn't work..

Thanks very much!

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"

MPU6050 mpu(0x69); //AD0=High


//MPU control/status vars 
bool dmpReady = false; 
uint8_t mpuIntStatus;  //actual interrupt status byte 
uint8_t deviceStatus; //device status , 0 = success , 
uint16_t packetSize; //expected DMP packet size (defult 42) -- ?
uint16_t fifoCount; //count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO buffer storage 

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





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


// put your setup code here, to run once:
void setup() {
  //join I2C bus 
    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        Wire.begin();
        TWBR = 48; // 400kHz I2C clock (200kHz if CPU is 8MHz)
    #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
        Fastwire::setup(400, true);
    #endif
  //initialize serial communication
  Serial.begin(115200);
  while(!Serial); //common for Leonardo issues

  //initialize the mpu 
  Serial.println("Call MPU6050 Lib to initialize devices...");
  mpu.initialize(); //initialize I2C device by using MPU6050 library 

  //verify connection
  Serial.println("Tesing device connections");
  Serial.println(mpu.testConnection() ? F("MPU6050 connection test successed ") : F("MPU6050 connection test failed"));

//  //wait for begin , uncomment if we need wait user interruption
//  Serial.println("Press any button to begin");
//  while (Serial.available() && Serial.read()); // empty buffer
//  while (!Serial.available());                 // wait for data
//  while (Serial.available() && Serial.read()); // empty buffer again


  //load and configure DMP 
  Serial.println("initializing DMP"); 
  deviceStatus = mpu.dmpInitialize(); //use MPU6050 library to inilisalize the dmp 

 //feed offsets 
  mpu.setXGyroOffset(220);
  mpu.setYGyroOffset(76);
  mpu.setZGyroOffset(-85);
  mpu.setZAccelOffset(1788); // 1688 factory default for my test chip

  //make sure it works 
  if (deviceStatus == 0) {
    Serial.println("DMP initialization success, now enable DMP for use");
    //turn on DMP 
    mpu.setDMPEnabled(true); //use MPU6050 library to enable DMP)

    //wait for first interrup . currently just leave it false automatically 
    mpuInterrupt == false; 

    //let main llop know it's ok to use DMP, set dmpRead flag to ture 
    dmpReady = true;
    Serial.println("DMP is ready to use.");

    //get expected DMP packet size for later comparison 
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    //ERROR! , device status !=0 when initializing DMP
    Serial.print("DMP initialization failed when using MPU6050 library:");
    if (deviceStatus == 1) {
      Serial.println(" intial memory load failed");
    } else if (deviceStatus == 2) {
      Serial.println(" failed to update DMP configuration");
    } else {
      Serial.print(" unknow error with code: ");
      Serial.println(deviceStatus);
    }
}
}


int ticket = 1;
void printOnlyOnce (String message){
  if (ticket == 1){
   Serial.println(message);
   ticket = 0 ;
  } else {
    return;
  }
}

void loop() {
  //if DMP not ready don't do anything 
  if (!dmpReady) {
    printOnlyOnce("MAIN LOOP: DMP disabled");
    return;
  } else {

    //testing overflow 
    if (fifoCount == 1024) {
      mpu.resetFIFO();
      Serial.println("FIFO overflow");
    } else {


    //wait for enough avaliable data length
    while (fifoCount < packetSize) {
      //waiting until get enough
      fifoCount = mpu.getFIFOCount();
    }


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

    //track FIFO count here is more then one packeage avalible 

    //reset fifo count 
    fifoCount -= packetSize ;
    Serial.println(fifoCount);

    if (fifoCount > 2) {
        ////// clear fifo buffer 
    }
    //display stage 
                // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            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);
  }
  }
}

Answer 1

Yank, I ended up clearing the buffer after every read. In your code it would be following the line

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

I added

mpu.resetFIFO();

I came to this conclusion after watching when the over flows occurred. The fifo buffer is adding on to the end of the buffer, and when it gets full, rather than wrap, it over flows. Reading does not seem to remove data from buffer in any way. By clearing the buffer you might be discarding data, but the DMP is doing the integration for AHRS for you, so you really do not need to worry about dropping data since you are only interested in the current pitch,roll,yaw or Euler or whatever, at least that was my case. I am running it on an Arduino UNO shield prototype and it is rock solid stable (at the default 100 hz rate). See spiked3.com PiPlate / Pilot.

Answer 2

What if we can increase the size of the FIFo Buffer.

uint8_t fifoBuffer[64]; // FIFO storage buffer

What if I change it to lets say:

uint8_t fifoBuffer[1024]; // FIFO storage buffer

and then change :

if ((mpuIntStatus & 0x10) || fifoCount == 16384) { // I changed 1024 -> 16384
    // reset so we can continue cleanly
    mpu.resetFIFO(); // Resets the FIFO buffer when set to 1 while FIFO_EN equals 0.
    Serial.println(F("FIFO overflow!"));

// otherwise, check for DMP data ready interrupt (this should happen frequently)
}

Please check these changes.

Answer 3

To prevent overflow problems, please, go to MPU6050_6Axis_MotionApps20.h and modify that line:

0x02,   0x16,   0x02,   0x00, **0x01**                // D_0_22 inv_set_fifo_rate

The value in bold is the objective! Change it to 0x03 or 0x04 or 0x05 to reduce the Hz of rate. I am using 0x03 and not getting error values, junk data, or overflows anymore.