Jeff Rowberg MPU6050_DMP6 code understanding

Projects Programming
1

Hello everyone,

I have to do some work with MPU6050 module. Now I'm trying to understand Jeff Rowberg MPU6050_DMP6 example code. Here is the part of code which is not clear to me at all. Maybe someone could try to explain in simple words what this part does in code? Here is the code part:

while (!mpuInterrupt && fifoCount < packetSize) {
   
    }
    mpuInterrupt = false;
    mpuIntStatus = mpu.getIntStatus();

    fifoCount = mpu.getFIFOCount();

    if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
        
        mpu.resetFIFO();
        Serial.println(F("FIFO overflow!"));
    } 
    else if (mpuIntStatus & 0x02) {
  
        while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();

    
        mpu.getFIFOBytes(fifoBuffer, packetSize);
        
        fifoCount -= packetSize;

Thank you :slight_smile:

2

This sample code didn't make sense to me either. The while loop locks you into waiting till the next packet is ready, depending on some settings may take a while. This code is blocking!!!!

What is happening:
While the interrupt trigger usually on pin2 of the UNO and while the FIFO packet size is too small
check the packet size over and over again across the i2c bus

fifoCount = mpu.getFIFOCount();

This process takes time!
My observations
The interrupt (Pin 2 connected to the INT pin on the mpu6050) is an excellent indicator that there is data in the buffer.
If the packet is too small or not the correct size forget it and wait till next interrupt. the packet will be corrupt and not usable!

So the solution :slight_smile:
I created a function to get the data

// ================================================================
// ===                    MPU DMP Get Data                      ===
// ================================================================
void GetDMP() { // Best version I have made so far
  // Serial.println(F("FIFO interrupt at:"));
  // Serial.println(micros());
  static unsigned long LastGoodPacketTime;
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
    digitalWrite(LED_PIN, LOW); // lets turn off the blinking light so we can see we are failing.
    mpu.resetFIFO();// clear the buffer and start over
  } else {
    while (fifoCount  >= packetSize) { // Get the packets until we have the latest!
      mpu.getFIFOBytes(fifoBuffer, packetSize); // lets do the magic and get the data
      fifoCount -= packetSize;
    }
    LastGoodPacketTime = millis();
    MPUMath(); // <<<<<<<<<<<<<<<<<<<<<<<<<<<< On success MPUMath() <<<<<<<<<<<<<<<<<<<
    digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // Blink the Light
  }
}
// ____________________________________
// *********** in the MPUMath() function ***********
// DPRINTSFN is a macro to simplify the printing of numbers. a macro is copied exactly into the location where it is needed when compiling
// all it is a fancy Serial.print(); that provides clean output for my debugging purposes
#define  DPRINTSFN(StrSize,Name,...) {char S[StrSize];Serial.print("\t");Serial.print(Name);Serial.print(" "); Serial.print(dtostrf((float)__VA_ARGS__ ,S));}//StringSize,Name,Variable,Spaces,Percision
/*
the above  macro basically does all this :)
{
  char S[StrSize]; // allocates memory for the number string to be generated.
  Serial.print("\t"); // tab
  Serial.print(Name);
  Serial.print(" ");
  Serial.print(dtostrf((float)__VA_ARGS__ ,S)); // the ... is the copied to __VA_ARGS__ 
}
*/
// ================================================================
// ===                        MPU Math                          ===
// ================================================================
float Yaw, Pitch, Roll;
void MPUMath() {
  mpu.dmpGetQuaternion(&q, fifoBuffer);
  mpu.dmpGetGravity(&gravity, &q);
  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
  Yaw = (ypr[0] * 180.0 / M_PI);
  Pitch = (ypr[1] *  180.0 / M_PI);
  Roll = (ypr[2] *  180.0 / M_PI);
 
  static unsigned long SpamTimer;
  if ((millis() - SpamTimer) >= (100)) {
    SpamTimer= millis();
    DPRINTSFN(15, " W:", q.w, -6, 4);
    DPRINTSFN(15, " X:", q.x, -6, 4);
    DPRINTSFN(15, " Y:", q.y, -6, 4);
    DPRINTSFN(15, " Z:", q.z, -6, 4);

    DPRINTSFN(15, " Yaw:", Yaw, -6, 2);
    DPRINTSFN(15, " Pitch:", Pitch, -6, 2);
    DPRINTSFN(15, " Roll:", Roll, -6, 2);
    DPRINTSFN(15, " Yaw:", ypr[0], -6, 2);
    DPRINTSFN(15, " Pitch:", ypr[1], -6, 2);
    DPRINTSFN(15, " Roll:", ypr[2], -6, 2);
    DPRINTLN();
  }
}

The full code for MPU-6050 MPU6050_Latest_code.ino (8 KB)

to get the correct offsets for your MPU6050 chip, use the attached calibration code
Z

MPU6050_Latest_code.ino (7.95 KB)

MPU6050_calibration.ino (7.64 KB)