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;
}
}
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:
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.
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.
