MPU6050 dmp I2C reading time change with sensor rate in freeRTOS

Hi Forum.

I’m a mechanical student and have very limited coding knowledge. Hope this question is not sounds stupid… Currently I’m struggled with the DMP reading from MPU6050. Here is the story:

My basic idea is, in FreeRTOS, I use every 5ms task to read DMP if gets interrupt signal. I have no issue to reading angles from MPU6050. But when I add code to toggle an output pin before and after the DMP reading lines and use oscilloscope to capture the wave (to measure code cycle time) , I found the cycle time of DMP reading changes, follow by the MPU6050 DMP sample rate.

For example, If I change the DMP sample rate (in “MPU6050_6Axis_MotionApps_V6_12” ) to 100Hz (10ms), then I got 11ms-ish output high on oscilloscope. If I change sample rate to 50Hz (20ms), I got 21ms-ish output high.

I study a lot about my code and cannot find which part causes this problem. So I just wondering if someone can help with this issue?

I’m sorry I cannot upload my original .cpp files since I’m a new user. So I post my code below, 1st is the DMP.h .cpp, just borrow from DMP example with small changes. 2nd is main.cpp

Any help is appreciated!!
Bests,

G

#include <Arduino.h>

#include "MPU6050_6Axis_MotionApps_V6_12.h"

#include "DMP.h" // just define some variables

#include "ESP32_DMPGimble.h"  // just define pins and consts

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

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

MPU6050 mpu;

bool DMP::begin()

{

    Wire.begin(SDA, SCL, 400000);

  mpu.initialize();

    

//   mpu.setXGyroOffset(220);

//   mpu.setYGyroOffset(76);

//   mpu.setZGyroOffset(-85);

//   mpu.setZAccelOffset(1788);

  devStatus = mpu.dmpInitialize();

  mpu.setDMPEnabled(true);

  mpuIntStatus = mpu.getIntStatus();

  

  dmpReady = true;

  packetSize = mpu.dmpGetFIFOPacketSize();

  

  return 1;

}

bool DMP::getQuaternion()

{

  mpuIntStatus = mpu.getIntStatus();    // 

  fifoCount = mpu.getFIFOCount();

  fifoOverFlow = mpu.getIntFIFOBufferOverflowStatus();

    if ((mpuIntStatus & fifoOverFlow) || fifoCount >= 1024)

    {

        mpu.resetFIFO(); 

        Serial.println("================== Overflow =================");

        

    }

        //Serial.println("No Overflow ~~~~~~~~~~~~~~~~~");

    while (fifoCount < packetSize)         // 

    {

        fifoCount = mpu.getFIFOCount();

    }

    mpu.getFIFOBytes(fifoBuffer, packetSize);  // 

    

    fifoCount -= packetSize;                   // 

    mpu.resetFIFO();

    

  mpu.dmpGetQuaternion(&q, fifoBuffer);

  mpu.dmpGetGravity(&gravity, &q);

  mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);   

}

float DMP::getYaw(int unit)

{

    float yaw = ypr[0];

    if (unit == 1)

    {

        yaw = yaw * 180 / 3.14;

    }

    return yaw;

}

float DMP::getPitch(int unit)

{

    float pitch = ypr[1];

    if (unit == 1)

    {

        pitch = pitch * 180 / 3.14;

    }

    return pitch;

}

float DMP::getRoll(int unit)

{

    float roll = ypr[2];

    if (unit == 1)

    {

        roll = roll * 180 / 3.14;

    }

    return roll;

}

#include <Arduino.h>

#include "LS7366R.h"

#include "ESP32_DMPGimble.h"        // Just define 

#include "DMP.h"

// =========================== Define Variables ======================

portMUX_TYPE synch = portMUX_INITIALIZER_UNLOCKED;

DMP dmp;

class axis

{

    public:

    float angle;

};

axis roll, pitch, yaw;

volatile bool timerFlag = false;

volatile long value = 0;

volatile bool mpuInterrupt = false;

void IRAM_ATTR dmpDataReady()

{

    mpuInterrupt = true;

}

void DMPReading(void * para)

{

    for(;;)

    {

        if (mpuInterrupt == true)

        {

            digitalWrite(TestPin1, HIGH);

            dmp.getQuaternion();

            digitalWrite(TestPin1, LOW);

            roll.angle = dmp.getRoll(angleInDegree);

            pitch.angle = dmp.getPitch(angleInDegree);

            yaw.angle = dmp.getYaw(angleInDegree); 

            mpuInterrupt = false;

        }

    vTaskDelay(5 / portTICK_PERIOD_MS);

    }

}

void setup()

{

    Serial.begin(115200);

    dmp.begin();

    pinMode(DMP_INTERRUPT_PIN, INPUT);

    attachInterrupt(digitalPinToInterrupt(DMP_INTERRUPT_PIN), dmpDataReady, FALLING);

    pinMode(TestPin1, OUTPUT);

    pinMode(TestPin2, OUTPUT);

    digitalWrite(TestPin1, LOW);

    digitalWrite(TestPin2, LOW);

    xTaskCreate(

        DMPReading,

        "PIDLoop",

        2048,

        NULL,

        1,

        NULL

    );

}

void loop()

{

}

That code doesn’t compile so I guess it isn’t complete. Maybe the error is in the part you’re hiding from us.

I miss a wiring diagram and a link to the used libraries.

As you seem to change the library code, explain in detail which changes you did!

Hi Pylon,

Thanks for the reply. Here is the code for DMP.h and ESP32_DMPGimble. They are simple defines for pins and functions

#ifndef DMP_H

#define DMP_H

#include <Arduino.h>

#include "I2Cdev.h"

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

    #include "Wire.h"

#endif

#include "ESP32_DMPGimble.h"

 

class DMP

{

public: 

  bool fifoOverFlow;

  bool begin();

  bool getQuaternion();

  float getYaw(int unit);

  float getPitch(int unit);

  float getRoll(int unit);

};

#endif
#ifndef ESP32_DMPGIMBLE_H

#define ESP32_DMPGIMBLE_H

#include <Arduino.h>

#define VSPI_MISO   MISO

#define VSPI_MOSI   MOSI

#define VSPI_SCLK   SCK

#define spiClk 1000000 // LS7366R SPI Clock: 1 MHz

//#define PIDAngleStepSignalDebug

//#define PIDAngleStepSignalDebugPlot

//#define Debug_PrintDetail

//#define PIDAngleFollowDebug

//#define PIDParameterPrint

// ======================== Pin Map Define (For ESP32) =================

#define TestPin1 0            //  Monitoring (to Oscilloscope )

#define TestPin2 2            // Monitoring (to Oscilloscope )

#define encoder1Pin 4

#define encoder2Pin 5

#define DMP_INTERRUPT_PIN 25

#define m1DirPin 33

#define m1PWMPin 32

#define m2DirPin 35

#define m2PWMPin 34

For the changes I made in the DMP.cpp (contain the basic dmp begin, read and clear function), I add a line to clear FIFO buffer after every time I read the dmp (to avoid FIFO overflow).

Thanks so much for the help.

As the delay is below 1ms in every case I guess the reading of the value is done outside of that period. That means that given the delay is 20ms this delay starts after all the values are read out which needs almost 1ms.

Hi pylon,

Thanks for the reply. Sorry I didn’t catch up your idea. As you can see in the “DMPReading” task, I put my “scope function”(write pin low and high) right before and after the "dmp.getQuaternion();
" function. For my understanding, even I change the sample rate, the reading time from registers by I2C should not change. So I feel very wired on this issue, maybe I change I2C speed by…somehow? ?

Today, while I debug my code, I found out another bug. Although, I got my dmp data, seems like this data has some lag to my motion. For example. If I rotate my sensor 90 in x, the data can go to 60 very fast, but from 60 to 90, it will take like 5-10s to finish. I’m not sure if those issues relate to each. I’m still searching to see if someone else had the same issue before.

Thanks,

Hi pylon,

I think I found which cause this issue. “mpu.dmpGetCurrentFIFOPacket(fifoBuffer)” this function always cause the main loop miss one signal. So if I put “pin toggle” function before and after it, the cycle I measured is always a little long than interrupt cycle.

As you can see in the new code, the “fifoState” is always 1, 0, 1, 0 cycling, even the ESP32 gets the interrupt signal!! I’m very confused, if an interrupt signal is generated by MPU, why the FIFO is not ready?? Is there anything I missed?

Thanks for the patient for my entry level question, I’m very appreciated.


#include <Arduino.h>

// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v6.12)

// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>

// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib

//

// Changelog:

//      2019-07-10 - Uses the new version of the DMP Firmware V6.12

//                 - Note: I believe the Teapot demo is broken with this versin as

//                 - the fifo buffer structure has changed

//      2016-04-18 - Eliminated a potential infinite loop

//      2013-05-08 - added seamless Fastwire support

//                 - added note about gyro calibration

//      2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error

//      2012-06-20 - improved FIFO overflow handling and simplified read process

//      2012-06-19 - completely rearranged DMP initialization code and simplification

//      2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly

//      2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING

//      2012-06-05 - add gravity-compensated initial reference frame acceleration output

//                 - add 3D math helper file to DMP6 example sketch

//                 - add Euler output and Yaw/Pitch/Roll output formats

//      2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)

//      2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250

//      2012-05-30 - basic DMP initialization working

/* ============================================

  I2Cdev device library code is placed under the MIT license

  Copyright (c) 2012 Jeff Rowberg

  Permission is hereby granted, free of charge, to any person obtaining a copy

  of this software and associated documentation files (the "Software"), to deal

  in the Software without restriction, including without limitation the rights

  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

  copies of the Software, and to permit persons to whom the Software is

  furnished to do so, subject to the following conditions:

  The above copyright notice and this permission notice shall be included in

  all copies or substantial portions of the Software.

  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN

  THE SOFTWARE.

  ===============================================

*/

// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files

// for both classes must be in the include path of your project

#include "I2Cdev.h"

#include "ESP32_DMPGimble.h"

#include "MPU6050_6Axis_MotionApps_V6_12.h"

//#include "MPU6050.h" // not necessary if using MotionApps include file

// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation

// is used in I2Cdev.h

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

#include "Wire.h"

#endif

// class default I2C address is 0x68

// specific I2C addresses may be passed as a parameter here

// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)

// AD0 high = 0x69

MPU6050 mpu;

#define OUTPUT_READABLE_YAWPITCHROLL

#define M_PI 3.14 

#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

VectorInt16 aa;         // [x, y, z]            accel sensor measurements

VectorInt16 gy;         // [x, y, z]            gyro 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

int fifoState = 0;

// packet structure for InvenSense teapot demo

// ================================================================

// ===               INTERRUPT DETECTION ROUTINE                ===

// ================================================================

volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high

void dmpDataReady() 

{

  mpuInterrupt = true;

}

// ================================================================

// ===                      INITIAL SETUP                       ===

// ================================================================

void setup() {

  pinMode(TestPin1, OUTPUT);

  // join I2C bus (I2Cdev library doesn't do this automatically)

#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE

  Wire.begin();

  Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties

#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE

  Fastwire::setup(400, true);

#endif

  Serial.begin(115200);

  mpu.initialize();

  pinMode(DMP_INTERRUPT_PIN, INPUT);

  devStatus = mpu.dmpInitialize();

  // supply your own gyro offsets here, scaled for min sensitivity

  mpu.setXGyroOffset(51);

  mpu.setYGyroOffset(8);

  mpu.setZGyroOffset(21);

  mpu.setXAccelOffset(1150);

  mpu.setYAccelOffset(-50);

  mpu.setZAccelOffset(1060);

  // make sure it worked (returns 0 if so)

  if (devStatus == 0) {

    // Calibration Time: generate offsets and calibrate our MPU6050

    mpu.CalibrateAccel(6);

    mpu.CalibrateGyro(6);

    Serial.println();

    mpu.PrintActiveOffsets();

    // turn on the DMP, now that it's ready

    Serial.println(F("Enabling DMP..."));

    mpu.setDMPEnabled(true);

    // enable Arduino interrupt detection

    Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));

    Serial.print(digitalPinToInterrupt(DMP_INTERRUPT_PIN));

    Serial.println(F(")..."));

    attachInterrupt(digitalPinToInterrupt(DMP_INTERRUPT_PIN), dmpDataReady, RISING);

    mpuIntStatus = mpu.getIntStatus();

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

    Serial.println(F("DMP ready! Waiting for first interrupt..."));

    dmpReady = true;

    // get expected DMP packet size for later comparison

    packetSize = mpu.dmpGetFIFOPacketSize();

  } else {

    // ERROR!

    // 1 = initial memory load failed

    // 2 = DMP configuration updates failed

    // (if it's going to break, usually the code will be 1)

    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 (mpuInterrupt == true)

  {

    mpuInterrupt = false;

    fifoState = mpu.dmpGetCurrentFIFOPacket(fifoBuffer);

    Serial.print("fifoState: "); Serial.println(fifoState);

    if (fifoState) { // Get the Latest packet 

      // display Euler angles in degrees

      digitalWrite(TestPin1, HIGH);

      mpu.dmpGetQuaternion(&q, fifoBuffer);

      digitalWrite(TestPin1, LOW);

      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.print(ypr[2] * 180 / M_PI);

      Serial.println();

      // blink LED to indicate activity

      blinkState = !blinkState;

      digitalWrite(LED_PIN, blinkState);

    }

  }

}

How long is the pin high?

Where in the code do you set the sample rate? I still couldn’t find that code part.

Hi Pylon,

I fiiiiiiiiiiinally found what’s going on for the lag between my motion and dmp data. As I mentioned previously, I tried to change the sample rate for dmp, the code I changed in line 366 in “MPU6050_6Axis_MotionApps_V6_12”.
I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x13)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
The default value is 0X04 for register 0X19 (200Hz), when I change the value to 0X13, the sample rate did change from 200Hz to 50Hz. However, the lag is becoming huge!!
The reason why I’m try to change this value is I believe at the original Arduino MPU6050.h, I changed sample rate somehow by changing the register value. So I believe this can be done, I must miss something, like forget setup other corresponding register maybe??

Thanks so much for the help, I’m very appreciated.

Why do you change the line in the library and don’t simply overwrite the value in your setup()?

This topic was automatically closed 120 days after the last reply. New replies are no longer allowed.