Why does this sketch give MPU FIFO overflows?

Question

I'm transmitting this program over an NRF24l01, data from mpu, and I'm getting a FIFO overflow. Im not sure what the problem is but it gives me a lag when I put it on processing so I know the problem is in the code. I posted the code the below. Pleas help find the issue.

// Include libraries necessary for the radios
#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>


RF24 radio(7, 8);


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


    #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
        #include "Wire.h"
    #endif

    MPU6050 mpu;


    #define OUTPUT_TEAPOT


    #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
    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
    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
    // packet structure for InvenSense teapot demo
    uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };



    // ================================================================
    // ===               INTERRUPT DETECTION ROUTINE                ===
    // ================================================================
    volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
    void dmpDataReady() {
        mpuInterrupt = true;
    }
    // ================================================================
    // ===                      INITIAL SETUP                       ===
    // ================================================================
    void setup() {
        // join I2C bus (I2Cdev library doesn't do this automatically)
        #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
            Wire.begin();
          //TWBR = 8; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR.

        #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
            Fastwire::setup(400, true);
        #endif

        radio.begin();

        radio.setRetries(15, 15);
        radio.openWritingPipe(0xF0F0F0F0A1LL);

        radio.stopListening();
        Serial.begin(115200);



        // load and configure the DMP
        Serial.println(F("Initializing DMP..."));
        devStatus = mpu.dmpInitialize();


        mpu.setXGyroOffset(54);
        mpu.setYGyroOffset(15);
        mpu.setZGyroOffset(33);
        mpu.setZAccelOffset(1613); // 1688 factory default for my test chip
        mpu.setXAccelOffset(-4009);
        mpu.setYAccelOffset(332);


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


            Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
            attachInterrupt(0, dmpDataReady, RISING);
            mpuIntStatus = mpu.getIntStatus();


            Serial.println(F("DMP ready! Waiting for first interrupt..."));
            dmpReady = true;
            // get expected DMP packet size for later comparison
            packetSize = mpu.dmpGetFIFOPacketSize();
        } else {

            Serial.print(F("DMP Initialization failed (code "));
            Serial.print(devStatus);
            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 (!dmpReady) return;
        // wait for MPU interrupt or extra packet(s) available
        while (!mpuInterrupt && fifoCount < packetSize) {

          /*Do some computational stuff here*/

        }

        // 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 & 0x10) || fifoCount == 1024) {
            // reset so we can continue cleanly
            mpu.resetFIFO();


            Serial.println(F("FIFO overflow!"));
        // otherwise, check for DMP data ready interrupt (this should happen frequently)
        } else if (mpuIntStatus & 0x02) {

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



            fifoCount -= packetSize;

                /*Object to Transmit Yaw, Pitch, and Roll*/
                typedef struct YPR_TX {
                  int y;
                  int p;
                  int r;

                } YPR_TX;

                YPR_TX ypr_tx;

             //   ypr_tx = calcYPR(ypr_tx.y, ypr_tx.p, ypr_tx.r);



                ypr_tx.y = ypr[0] * 180/M_PI;
                ypr_tx.p = ypr[1] * 180/M_PI;
                ypr_tx.r = ypr[2] * 180/M_PI;





            #ifdef OUTPUT_TEAPOT
                // display quaternion values in InvenSense Teapot demo format:
                teapotPacket[2] = fifoBuffer[0];
                teapotPacket[3] = fifoBuffer[1];
                teapotPacket[4] = fifoBuffer[4];
                teapotPacket[5] = fifoBuffer[5];
                teapotPacket[6] = fifoBuffer[8];
                teapotPacket[7] = fifoBuffer[9];
                teapotPacket[8] = fifoBuffer[12];
                teapotPacket[9] = fifoBuffer[13];


                Serial.print("teapotPacket/t");
                Serial.print("\t");
                Serial.print(teapotPacket[2]);
                Serial.print("\t");
                Serial.print(teapotPacket[3]);
                Serial.print("\t");
                Serial.print(teapotPacket[4]);
                Serial.print("\t");
                Serial.print(teapotPacket[5]);
                Serial.print("\t");
                Serial.print(teapotPacket[6]);
                Serial.print("\t");
                Serial.print(teapotPacket[7]);
                Serial.print("\t");
                Serial.print(teapotPacket[8]);
                Serial.print("\t");
                Serial.println(teapotPacket[9]);
               // radio.getDataRate();


                radio.write(&teapotPacket, sizeof(teapotPacket));

            #endif

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


    }

Answer 1

I have been playing around with the MPU6050 with DMP library too. I see you have used Jeff's example code (as have I). I was getting a lot of FIFO overflows too, but that was because my "do computational stuff here" was taking too long (code with delay()) and the FIFO on the DMP was filling up. Perhaps your radio.write() is taking too long?

The code as written by Jeff is really quite robust, but perhaps not what is necessary in all cases. I have put an extra mpu.resetFIFO() below the Serial.println's. Kind of a hack, but it works. I found the suggestion here:

How to clear FIFO buffer on MPU6050?

Also check on Jeff's own suggestion of lowering the data rate on the DMP by altering the header file:

https://www.i2cdevlib.com/forums/topic/27-fifo-overflow/#comment-99

Hope this helps.

Answer 2

You are declaring a FIFO overflow every time the mpu sends interrupt 0x10. Is that correct?

You are using the value in fifoCount before you have set it to a value. Now with the Arduino IDE you are OK doing that, but its not a good practice.

You are checking that the fifoCount is exactly 1024. It might be better to check if it is >1023 because that will handle the case when it is 1025.

Is there a reason why you have chosen 1024? Since the packet size defaults to 42 that's 24.3 packets.

Around

while (!mpuInterrupt && fifoCount < packetSize) {

You never get the fifoCount again, so fifoCount < packetSize is always true, therefore you wait until there is an interrupt. If there first time there is an interrupt is to tell you 0x10 (which I guess is) fifo full from the MPU's perspective) then that will be the reason it is always full.

   // wait for MPU interrupt or extra packet(s) available
    while (!mpuInterrupt && fifoCount < packetSize) {

      /*Do some computational stuff here*/
      fifoCount = mpu.getFIFOCount(); // Don't forget to get the latest fifo size.
    }