IMU9250 every 2 seconds

Hello,

I am trying to get reading from imu9250 every 2 seconds = ignoring readings that are between sampling moments.
Unfortunately I think I do something not so good with FIFO.
Could you guys take a look?

#include <MPU6050_6Axis_MotionApps20.h>
#include <Wire.h>   // standardowa biblioteka Arduino


// ================================================================
// ===                        DEFINE                            ===
// ================================================================
#define interruptPin 7
#define LED_PIN 13

#define DEBUG
#ifdef DEBUG
//#define DPRINT(args...)  Serial.print(args)             //OR use the following syntax:
#define DPRINTSTIMER(t)    for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#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
#define DPRINTLN(...)      Serial.println(__VA_ARGS__)
#else
#define DPRINTSTIMER(t)    if(false)
#define DPRINTSFN(...)     //blank line
#define DPRINTLN(...)      //blank line
#endif

#define BACKLIGHT_PIN     3
#define En_pin  2
#define Rw_pin  1
#define Rs_pin  0
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7

unsigned long currentMillis, prvMillis, interval;


// ================================================================
// ===                        GLOBAL OBJECTS                    ===
// ================================================================

int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected

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 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
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)

MPU6050 mpu;



// ================================================================
// ================================================================
// ===                        METHODS                           ===
// ================================================================
// ================================================================


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
volatile bool mpuInterrupt = false;

void dmpDataReady() {
  mpuInterrupt = true;
}


// ================================================================
// ===                      MPU DMP SETUP                       ===
// ================================================================
void MPU6050Connect() {
  static int MPUInitCntr = 0;
  // initialize device
  mpu.initialize(); // same
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();// same

  if (devStatus != 0) {
    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
    MPUInitCntr++;
    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));
    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }
  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  Serial.print("mpu.getInterruptDrive=  ");
  Serial.println(mpu.getInterruptDrive());
  attachInterrupt(digitalPinToInterrupt(interruptPin), dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus(); // Same
  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus();
  mpuInterrupt = false; // wait for next interrupt
  Serial.println("MPU6050Connect end");
}


// ================================================================
// ===                    MPU DMP Get Data                      ===
// ================================================================
void GetDMP() { // Best version I have made so far
  Serial.println ("Here 1");
  static unsigned long LastGoodPacketTime;
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  Serial.println ("Here 2");
  if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
    Serial.println ("Here 3");
    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 {
    Serial.println ("Here 4");
    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() <<<<<<<<<<<<<<<<<<<
  }
}


// ================================================================
// ===                        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);
  Serial.println("------");
  Serial.print("Pitch");
  Serial.println(Pitch);
  Serial.print("Roll");
  Serial.println(Roll);
}



// ================================================================
// ===                      i2c SETUP Items                     ===
// ================================================================
void i2cSetup() {
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
}

// ================================================================
// ===                        Initial Setup                     ===
// ================================================================
void setup()
{
  //serial init
  Serial.begin(115200); //115200
  while (!Serial);

  i2cSetup();
  MPU6050Connect();
  //Serial.println("Setup complete");
  pinMode(LED_PIN, OUTPUT);
  interval = 2000;
}




// ================================================================
// ===                        Main loop                         ===
// ================================================================
void loop()
{
  //Serial.print("Loop");
  

  currentMillis = millis();
  //Serial.println(currentMillis);
  //Serial.println(prvMillis);
  if ((unsigned long)(currentMillis - prvMillis) >= interval) {
    //Serial.println ("Interval");
    prvMillis = currentMillis;
    if (mpuInterrupt ) { // wait for MPU interrupt or extra packet(s) available
      Serial.println ("Interval + int");
      GetDMP();
    }
    else {
      //Serial.println ("Wywal bufor");
      mpuInterrupt = false;
      mpu.resetFIFO();
    }

  }
}

Self reply: so I corrected my code. I get results every Interval. Values seems to be correct.
Do you guys think it is correct implementation? It is driven by “goRead” variable.
Setup is IMU9250 + Arduino Micro board.

#include <MPU6050_6Axis_MotionApps20.h>
#include <Wire.h>   // standardowa biblioteka Arduino


// ================================================================
// ===                        DEFINE                            ===
// ================================================================
#define interruptPin 7
#define LED_PIN 13

#define DEBUG
#ifdef DEBUG
//#define DPRINT(args...)  Serial.print(args)             //OR use the following syntax:
#define DPRINTSTIMER(t)    for (static unsigned long SpamTimer; (unsigned long)(millis() - SpamTimer) >= (t); SpamTimer = millis())
#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
#define DPRINTLN(...)      Serial.println(__VA_ARGS__)
#else
#define DPRINTSTIMER(t)    if(false)
#define DPRINTSFN(...)     //blank line
#define DPRINTLN(...)      //blank line
#endif

#define BACKLIGHT_PIN     3
#define En_pin  2
#define Rw_pin  1
#define Rs_pin  0
#define D4_pin  4
#define D5_pin  5
#define D6_pin  6
#define D7_pin  7

unsigned long currentMillis, prvMillis, interval;
boolean goRead = false;

// ================================================================
// ===                        GLOBAL OBJECTS                    ===
// ================================================================

int FifoAlive = 0; // tests if the interrupt is triggering
int IsAlive = -20;     // counts interrupt start at -20 to get 20+ good values before assuming connected

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 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
byte StartUP = 100; // lets get 100 readings from the MPU before we start trusting them (Bot is not trying to balance at this point it is just starting up.)

MPU6050 mpu;



// ================================================================
// ================================================================
// ===                        METHODS                           ===
// ================================================================
// ================================================================


// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// ================================================================
volatile bool mpuInterrupt = false;

void dmpDataReady() {
  mpuInterrupt = true;
}


// ================================================================
// ===                      MPU DMP SETUP                       ===
// ================================================================
void MPU6050Connect() {
  static int MPUInitCntr = 0;
  // initialize device
  mpu.initialize(); // same
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();// same

  if (devStatus != 0) {
    char * StatStr[5] { "No Error", "initial memory load failed", "DMP configuration updates failed", "3", "4"};
    MPUInitCntr++;
    Serial.print(F("MPU connection Try #"));
    Serial.println(MPUInitCntr);
    Serial.print(F("DMP Initialization failed (code "));
    Serial.print(StatStr[devStatus]);
    Serial.println(F(")"));
    if (MPUInitCntr >= 10) return; //only try 10 times
    delay(1000);
    MPU6050Connect(); // Lets try again
    return;
  }
  Serial.println(F("Enabling DMP..."));
  mpu.setDMPEnabled(true);
  // enable Arduino interrupt detection
  Serial.println(F("Enabling interrupt detection (Arduino external interrupt pin 2 on the Uno)..."));
  Serial.print("mpu.getInterruptDrive=  ");
  Serial.println(mpu.getInterruptDrive());
  attachInterrupt(digitalPinToInterrupt(interruptPin), dmpDataReady, RISING);
  mpuIntStatus = mpu.getIntStatus(); // Same
  // get expected DMP packet size for later comparison
  packetSize = mpu.dmpGetFIFOPacketSize();
  delay(1000); // Let it Stabalize
  mpu.resetFIFO(); // Clear fifo buffer
  mpu.getIntStatus();
  mpuInterrupt = false; // wait for next interrupt
  Serial.println("MPU6050Connect end");
}


// ================================================================
// ===                    MPU DMP Get Data                      ===
// ================================================================
void GetDMP() { // Best version I have made so far
  //Serial.println ("Here 1");
  static unsigned long LastGoodPacketTime;
  mpuInterrupt = false;
  FifoAlive = 1;
  fifoCount = mpu.getFIFOCount();
  //Serial.println ("Here 2");
  if ((!fifoCount) || (fifoCount % packetSize)) { // we have failed Reset and wait till next time!
    //Serial.println ("Here 3");
    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 {
    //Serial.println ("Here 4");
    goRead = false;
    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() <<<<<<<<<<<<<<<<<<<
  }
}


// ================================================================
// ===                        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);
  Serial.println("------");
  Serial.print("Pitch: ");
  Serial.println(Pitch);
  Serial.print("Roll : ");
  Serial.println(Roll);
}



// ================================================================
// ===                      i2c SETUP Items                     ===
// ================================================================
void i2cSetup() {
  Wire.begin();
  TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
}

// ================================================================
// ===                        Initial Setup                     ===
// ================================================================
void setup()
{
  //serial init
  Serial.begin(115200); //115200
  while (!Serial);
  i2cSetup();
  MPU6050Connect();
  //Serial.println("Setup complete");
  pinMode(LED_PIN, OUTPUT);
  interval = 500;
}




// ================================================================
// ===                        Main loop                         ===
// ================================================================
void loop()
{
  //Serial.print("Loop");
  if (goRead) {
    if (mpuInterrupt) { // wait for MPU interrupt or extra packet(s) available
      Serial.println ("Interval + int");
      GetDMP();
      prvMillis = currentMillis;
    }
  return;
  }

  currentMillis = millis();
  //Serial.println(currentMillis);
  //Serial.println(prvMillis);
  if ((unsigned long)(currentMillis - prvMillis) < interval) {
    mpuInterrupt = false;
  }
  else {
    mpu.resetFIFO();
    goRead = true;
  }

}

tnts:
Hello,

I am trying to get reading from imu9250 every 2 seconds = ignoring readings that are between sampling moments.
Unfortunately I think I do something not so good with FIFO.
Could you guys take a look?

The fifo buffer is becoming corrupted if it fills up all the way it will not contain correct data
so there is a couple thoughts
Option 1: Reset the fifo buffer prior to your reading a new batch of DMP data that gets generated automatically every 10 milliseconds. so reset it wait 10 milliseconds and get the buffer.

mpu.resetFIFO();
// some kind of delay(10) Blink without delay is best or use the next interrupt from the MPU.
 GetDMP();

Option 2: change the rate at which the fifo buffer is populated
About line 305 in the library you included MPU6050_6Axis_MotionApps20.h

    0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate

    // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
    // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
    // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))

    // It is important to make sure the host processor can keep up with reading and processing
    // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.

Haven't tried this but with the above comment (200Hz / (1 + 199)) = 1 Hz
so changing the line to:

   0x02,   0x16,   0x02,   0x00, 0xC7                // D_0_22 inv_set_fifo_rate

C7 HEX = 199

Now use interrupts to trigger your GetDMP ( ). you can either get it every second and ignore one or skipping one the FIFO buffer can handle several DMP Sets before it corrupts. If you choose the latter you will need to continue to get the FIFO a second time to get the last set for the latest readings. always trigger mpu.resetFIFO ( ) ; after to make sure the buffer is cleaned for the next set of data.

Z

Thank you.
I did something similar to your 1st solution. Dump all in fifo, wait for nearest interrupt, wait till fifi is complete, read.

2nd solution seems more elegant - will see if it works :slight_smile:

Hi, Second solution is working too. It's great because I could save a lot of FLASH area bytes and it seems to be more "as should be" to drive everything with interrupt.