fifo overflow while using MPU-6050.

Hello,

I’m working on a quadcopter, which is controlled by means of a visual studio application. I’m making use of those $5 transmitter/receiver to communicate between the PC and the quadcopter. I’m using the MPU-6050 to obtain the Euler angles. Everything worked out well when i coded the wireless transmission, getting euler angles of the DMP of mpu-6050, but when i tried combining the two, FIFO overflow keeps occurring. I understand that I’m not taking data off the fifobuffer, as frequently as it requires, or i’m taking data at a much slower rate.

How do i time my intervals when i take data off the MPU-6050 such that this overflow doesn’t occur?

Here’s the code,

#include <VirtualWire.h>
#include <ServoTimer2.h>
#include <PID_v1.h>
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
MPU6050 mpu;


ServoTimer2 motor1,motor2,motor3,motor4;

int j=0,pwm,a,b;






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

float euler[3];         // [psi, theta, phi]    Euler angle container
Quaternion q;           // [w, x, y, z]         quaternion container



// PID control variables and initialisation

//0.38  0.08  0.1

float conskp=.38, conski=.08, conskd=.1, aggkp=.38, aggki=.08, aggkd=.1;
/*conskp=.38;
conski=.08;
conskd=.1;
aggkp=.38;
aggki=.08;
aggkd=.1;*/
double setPoint_pitch, input_pitch, output_pitch;
PID pid_pitch(&input_pitch, &output_pitch, &setPoint_pitch, conskp, conski, conskd, DIRECT);


//*******INTERRUPT DETECTION SUB-ROUTINE***********//

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







void setup()
{
  
  motor1.attach(9);
  motor2.attach(6);
  motor3.attach(5);
  motor4.attach(7);
  
  pinMode(13,OUTPUT);
  pinMode(12,OUTPUT);
   pinMode(3,OUTPUT);
    //****************************PID INITIALISING CODE*******************//
    
    pid_pitch.SetOutputLimits(-20,20);
    pid_pitch.SetMode(AUTOMATIC);
    setPoint_pitch=0;
   
   //*******************ASK receiver INITIALISING CODE*******************// 
  vw_set_rx_pin(11);
  vw_set_ptt_inverted(true);
  vw_setup(2000);
  vw_rx_start(); //start the PLL running
  
  
  //*******************MPU-6050 INITIALISING CODE*******************//
  
   Wire.begin();
  Serial.begin(115200);
  mpu.initialize();
  
   while (Serial.available() && Serial.read());// empty buffer
   devStatus = mpu.dmpInitialize();// load and configure DMP
   
    if (devStatus == 0) {
             
        mpu.setDMPEnabled(true);  // turn on the DMP, now that it's ready

        // enable Arduino interrupt detection
        
        attachInterrupt(0, dmpDataReady, RISING);
        mpuIntStatus = mpu.getIntStatus();

        // set our DMP Ready flag so the main loop() function knows it's okay to use it
        
        dmpReady = true;

        // get expected DMP packet size for later comparison
        packetSize = mpu.dmpGetFIFOPacketSize();
    }
    else
    Serial.print("DMP initialization failed");
  
  
  
}

void loop()
{
  digitalWrite(12,HIGH);
   if (!dmpReady) return;
  
   while (!mpuInterrupt && fifoCount < packetSize)
   {
   }
  
  // 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);
        
        // track FIFO count here in case there is > 1 packet available
        // (this lets us immediately read more without waiting for an interrupt)
        fifoCount -= packetSize;
    }  
        
          
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
       
//  transmission();
 // if(j==5)
 // hover();
  
  
  
}

void transmission()
{
    uint8_t buf[VW_MAX_MESSAGE_LEN],i=0;
    uint8_t buflen = VW_MAX_MESSAGE_LEN;
    char msg[10],starter1[2]="a",starter2[2]="b",starter3[2]="c",starter4[2]="d",starter5[2]="e";
    
   //
   
    if (vw_get_message(buf, &buflen))
    {
      
      digitalWrite(13,HIGH);
      //pwm=atoi(buf);
      for(i=0;i<buflen;i++)         
      msg[i]=buf[i];      
      msg[i]='\0';
      if(strcmp(msg,starter1)==0)
      start1();
      else if(strcmp(msg,starter2)==0)
      start2();
      else if(strcmp(msg,starter3)==0)
      start3();
      else if(strcmp(msg,starter4)==0)
      start4();
       else if(strcmp(msg,starter5)==0)
      start5();
       digitalWrite(13,LOW);
    }
    
      switch(j)
      {
        case 1:
        
        digitalWrite(2,HIGH);
        pwm=atoi(msg);
        Serial.println(pwm);
        motor1.write(pwm);
        digitalWrite(2,LOW);
        break;
        
        case 2:
       
        digitalWrite(3,HIGH);
         pwm=atoi(msg);
        Serial.println(pwm);
        motor2.write(pwm);
        digitalWrite(3,LOW);
        break;
        
         case 3:
        
        digitalWrite(3,HIGH);
         pwm=atoi(msg);
        Serial.println(pwm);
        motor3.write(pwm);
        digitalWrite(3,LOW);
        break;
        
          case 4:
         
        digitalWrite(3,HIGH);
         pwm=atoi(msg);
        Serial.println(pwm);
        motor4.write(pwm);
        digitalWrite(3,LOW);
        break;
        
        case 5:
        digitalWrite(3,HIGH);

        pwm=atoi(msg);
              
        a=pwm;
        b=pwm;
        hover();
         digitalWrite(3,LOW);
        
        
      }
}

/*void angle()
{
 
}*/
  
void start1()
{
  j=1;
  delay(20);
}
void start2()
{
  j=2;
  delay(20);
}
void start3()
{
  j=3;
  delay(20);
}
void start4()
{
  j=4;
  delay(20);
}

void start5()
{
  j=5;
  delay(20);
}

void hover()
{
  
 
  float pitch_gap= abs(setPoint_pitch-euler[1]);
   if(pitch_gap<3)
  pid_pitch.SetTunings(conskp,conski,conskd);
  else
  pid_pitch.SetTunings(aggkp,aggki,aggkd);
  
 pid_pitch.Compute();
 if(output_pitch>0)
 {
   a++;
   motor4.write(a);
    
  
 }
 else
 {
   b++;
   motor2.write(b);
   
 }
             
             Serial.print(a);
              Serial.print("\t");
            
             Serial.println(b);
}

You could probably start by losing these  delay(20);

    if(strcmp(msg,starter1)==0)
      start1();
    else if(strcmp(msg,starter2)==0)
      start2();
    else if(strcmp(msg,starter3)==0)
      start3();
    else if(strcmp(msg,starter4)==0)
      start4();
    else if(strcmp(msg,starter5)==0)
      start5();
...
...
void start1()
{
  j=1;
  delay(20);
}
void start2()
{
  j=2;
  delay(20);
}
void start3()
{
  j=3;
  delay(20);
}
void start4()
{
  j=4;
  delay(20);
}

void start5()
{
  j=5;
  delay(20);
}

aka

if (msg [0] >= 'a' && msg [0] <= 'e')
  j = (msg [0] - 'a') + 1;

There’s a fair bit of serial output there which will slow things down hugely, and those delays AWOL mentioned would have to go too.

I don’t understand the reason for this code and I’m skeptical that it works:

	while (!mpuInterrupt && fifoCount < packetSize)
	{
	}

Since you don’t update fifoCount in the loop or in an interrupt, how does the while loop terminate?

Is there some reason why you don’t simply poll mpu.getFIFOCount() and process the packet when it is available? As in:

void loop()
{
    if(mpu.getFIFOCount() >= packetSize)
    {
        mpu.getFIFOBytes(fifoBuffer, packetSize);
	mpu.dmpGetQuaternion(&q, fifoBuffer);
	mpu.dmpGetEuler(euler, &q);
    }
    // handle any other events for VW etc here
}

Also, give these global variables meaningful names, or get rid of them:

int j=0,pwm,a,b;