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

• BrettFolkins: How do you know that the FIFO buffer is your problem?
• Yank: hi , because I've tested it :)
• BrettFolkins: But what method of testing did you do that isolated the problem to that buffer?
• Yank: By eliminating other potential problem sources one by one to this one , it took me about 8 hours :)

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.

• Yank: Thanks Spiked3! I'm currently hard testing it , in my situation I need use history data from 12 sensor's reading and 100% stable running for at least 10mins and feed into my machine learning algorithm,and I know even if I use interruptions , I think you enlighten my life ! I will give my testing results this afternoon and approve it ! :)
• Yank: After testing with different frequencies and hours , the sensor remain stable without any faulty data! Thanks @Spike3
• user6569: Leave comment on the other question as well, that this worked.

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.

• user6569: I know I had tried this in the past (it is what a simple google turns up), but I thought I would try it again to be sure since you posted it a month after my accepted answer. Same results as before, nothing but overflows. All it does is slow down the overflows, it does nothing to eliminate them.
• qwerty_so: Actually when I tried this it even gave me bad results. The values I read were just scraped. Can't down-vote here but this is a bad approach!