Fixing FIFO Overflow on Arduino - arduino

Question

I'm multiplexing 3 IMU-6050 using a MUX4051. This is the original code:
#include "Wire.h"
const int MPU=0x68; // I2C address of the MPU-6050
int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
int Acc_ctrl_1 = 9;
int Acc_ctrl_2 = 10;
int Acc_ctrl_3 = 11;
int chip_enable1 = 5;
void setup() {
Wire.begin(); // wake up I2C bus
// set I/O pins to outputs
Wire.beginTransmission(MPU);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);
Serial.begin(115200);
pinMode(Acc_ctrl_1, OUTPUT); //S0
pinMode(Acc_ctrl_2, OUTPUT); //S1
pinMode(Acc_ctrl_3, OUTPUT); //S2 address lines
pinMode(chip_enable1, OUTPUT);
//S0=1, S1=2 and S2=4 so Y0= S0=0,S1=0,S2=0, Y4=S0=0,S1=0,S2=1
}
void loop() {
//Enable the MUX Chip 1 - Active Low
digitalWrite(chip_enable1, LOW);
// control signal for First Accelerometer
Serial.println("IMU 1");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for SECOND Accelerometer
Serial.println("IMU 2");
digitalWrite(Acc_ctrl_1, HIGH);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for THIRD Accelerometer
Serial.println("IMU 3");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, HIGH);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
}
void readAccele()
{
Wire.beginTransmission(MPU);// I2C address code thanks to John Boxall
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU,14,true); // request a total of 14 registers
AcX=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcY=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZ=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmp=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyX=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyY=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZ=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
Serial.print("AcX = "); Serial.print(AcX);
Serial.print(" | AcY = "); Serial.print(AcY);
Serial.print(" | AcZ = "); Serial.print(AcZ);
Serial.print(" | Tmp = "); Serial.print(Tmp/340.00+36.53); //equation for temperature in degrees C from datasheet
Serial.print(" | GyX = "); Serial.print(GyX);
Serial.print(" | GyY = "); Serial.print(GyY);
Serial.print(" | GyZ = "); Serial.println(GyZ);
delay(5);
}
I implemented it into the Jeff Rowberg example code:
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff#rowberg.net>
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
bool dmpReady = false; // set true if DMP init was successful
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];
// 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 "MPU6050_6Axis_MotionApps20.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)
MPU6050 mpu;
// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
//#define OUTPUT_READABLE_QUATERNION
// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
#define OUTPUT_READABLE_EULER
// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
#define OUTPUT_READABLE_WORLDACCEL
int Acc_ctrl_1 = 9;
int Acc_ctrl_2 = 10;
int Acc_ctrl_3 = 11;
int chip_enable1 = 5;
int chip_enable2 = 6;
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
//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
// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE ===
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP ===
// ================================================================
void setup() {
pinMode(Acc_ctrl_1, OUTPUT); //S0
pinMode(Acc_ctrl_2, OUTPUT); //S1
pinMode(Acc_ctrl_3, OUTPUT); //S2 address lines
pinMode(chip_enable1, OUTPUT);
// join I2C bus (I2Cdev library doesn't do this automatically)
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz)
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// initialize device
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
// verify connection
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready
Serial.println(F("\nSend any character to begin DMP programming and demo: "));
while (Serial.available() && Serial.read()); // empty buffer
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip
// make sure it worked (returns 0 if so)
if (devStatus == 0) {
// turn on the DMP, now that it's ready
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection
Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
attachInterrupt(0, 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() {
//Enable the MUX Chip 1 - Active Low
digitalWrite(chip_enable1, LOW);
// control signal for First Accelerometer
Serial.println("IMU 1");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for SECOND Accelerometer
Serial.println("IMU 2");
digitalWrite(Acc_ctrl_1, HIGH);
digitalWrite(Acc_ctrl_2, LOW);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
// control signal for THIRD Accelerometer
Serial.println("IMU 3");
digitalWrite(Acc_ctrl_1, LOW);
digitalWrite(Acc_ctrl_2, HIGH);
digitalWrite(Acc_ctrl_3, LOW);
readAccele();
delay(500);
}
void readAccele(){
// if programming failed, don't try to do anything
if (!dmpReady) return;
// wait for MPU interrupt or extra packet(s) available
while (!mpuInterrupt && fifoCount < packetSize) {
// other program behavior stuff here
}
// 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;
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z
mpu.dmpGetQuaternion(&q, fifoBuffer);
Serial.print("quat\t");
Serial.print(q.w);
Serial.print("\t");
Serial.print(q.x);
Serial.print("\t");
Serial.print(q.y);
Serial.print("\t");
Serial.println(q.z);
#endif
#ifdef OUTPUT_READABLE_EULER
// 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);
#endif
#ifdef OUTPUT_READABLE_WORLDACCEL
// display initial world-frame acceleration, adjusted to remove gravity
// and rotated based on known orientation from quaternion
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetAccel(&aa, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
Serial.print("aworld\t");
Serial.print(aaWorld.x);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
;
}
}
But this is what the serial prints, FIFO Overflow.. I tried to fix it but couldn't. I can't upload an image so Ill copy and paste the serial output here as code...
Send any character to begin DMP programming and demo:
Initializing DMP...
Enabling DMP...
Enabling interrupt detection (Arduino external interrupt 0)...
DMP ready! Waiting for first interrupt...
IMU 1
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1
FIFO overflow!
IMU 2
IMU 3
IMU 1

Answer 1

I have been working on a stable library to get all the useful information using the MPU6050_6Axis_MotionApps20 provided by I2Cdev package. I don't know if it is useful for you, but I upload it for everyone having problems dealing with it:
GYRO.h:
#ifndef GYRO
#define GYRO
#include "Arduino.h"
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
#define INTERRUPT_PIN 2 //24
#define CALIBRATION_LOOPS 500
//Error codes
#define EC_NO_ERROR 0
#define EC_DMP_MEMORY_WRITING_FAILED 1
#define EC_DMP_CONFIG_WRITING_FAILED 2
#define EC_NO_CONNECTION 3
//Offsets, put your own offsets here
#define GYRO_GX_OFFSET 93
#define GYRO_GY_OFFSET 0
#define GYRO_GZ_OFFSET 7
#define GYRO_AX_OFFSET -2550
#define GYRO_AY_OFFSET 1978
#define GYRO_AZ_OFFSET 499
class GYRO{
public:
GYRO(void);
uint8_t begin(void);
void update(void);
//All are made following the arrows with the right hand rule
inline VectorFloat getGyroscope(){return gyroscope; }
//All are made following the arrows
inline VectorDouble getAccelerometer(){return accelerometer; }
//A positive 1, indicates that the gravity is going in the oppsite way of the arrow drawed on the sensor
inline VectorFloat getGravity(){return gravity; }
//Returns the temperature in ºC
inline float getTemperature(){return temperature; }
//The time that has passed for calculating the speed with the acceleration
inline unsigned long getMicrosSpent(){return microsSpent; }
inline bool isGyroscopeUpdated(){return gyroscopeUpdated; }
private:
//Gyroscope
MPU6050 gyro; //Gyroscope configured with pin ADO-LOW
//Gyroscope readings
// MPU control/status vars
uint8_t gyroIntStatus; // 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
VectorFloat gravity; // [x, y, z] gravity vector
float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
VectorFloat gyroscope; // [x, y, z] gyroscope vector
VectorInt16 rawAccelerometer; // [x, y, z] raw acceleromer
VectorDouble accelerometer; // [x, y, z] accelerometer in m/s2
float temperature; // temperature in ºC
unsigned long lastMicros;
unsigned long actualMicros;
unsigned long microsSpent;
bool gyroscopeUpdated;
int loops_before_calibration;
};
#endif
GYRO.cpp:
#include "GYRO.h"
#include "Arduino.h"
#include "MPU6050_6Axis_MotionApps20.h"
#include "Wire.h"
volatile bool interrupt = false;
void dmpDataReady(void) {
interrupt = true;
}
GYRO::GYRO(void){
gyro = MPU6050(0x68); //Change to 0x69 if the AD0 pin of your MPU6050 is HIGH
}
uint8_t GYRO::begin(void){
Wire.begin();
Wire.setClock(400000);
uint8_t errorCode;
//Gyro initialization
gyro.initialize();
pinMode(INTERRUPT_PIN, INPUT);
if(gyro.testConnection()){
Serial.println(F("\nSend any character to begin the calibration and initialization of the gyro: "));
while (Serial.available() && Serial.read());
while (!Serial.available());
while (Serial.available() && Serial.read());
devStatus = gyro.dmpInitialize();
gyro.setXGyroOffset(GYRO_GX_OFFSET);
gyro.setYGyroOffset(GYRO_GY_OFFSET);
gyro.setZGyroOffset(GYRO_GZ_OFFSET);
gyro.setXAccelOffset(GYRO_AX_OFFSET);
gyro.setYAccelOffset(GYRO_AY_OFFSET);
gyro.setZAccelOffset(GYRO_AZ_OFFSET);
if (devStatus == 0) {
gyro.setDMPEnabled(true);
lastMicros = micros();
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
gyroIntStatus = gyro.getIntStatus();
packetSize = gyro.dmpGetFIFOPacketSize();
}else if(devStatus == 1) return EC_DMP_MEMORY_WRITING_FAILED;
else if(devStatus == 2) return EC_DMP_CONFIG_WRITING_FAILED;
}else return EC_NO_CONNECTION;
loops_before_calibration = 0;
return EC_NO_ERROR;
}
void GYRO::update(void){
//temperature = gyro.getTemperature()/340.0+36.53; //When discommenting this line i get rarely FIFO overflows, but without it, i have not recived eaither one
gyroscopeUpdated = false;
while (!interrupt && fifoCount < packetSize){
if (interrupt && fifoCount < packetSize){
fifoCount = gyro.getFIFOCount();
}
}
interrupt = false;
gyroIntStatus = gyro.getIntStatus();
fifoCount = gyro.getFIFOCount();
if ((gyroIntStatus & _BV(MPU6050_INTERRUPT_FIFO_OFLOW_BIT)) || fifoCount >= 1024){
gyro.resetFIFO();
fifoCount = gyro.getFIFOCount();
Serial.println(F("FIFO overflow!"));
}else if (gyroIntStatus & _BV(MPU6050_INTERRUPT_DMP_INT_BIT)){
while (fifoCount < packetSize) fifoCount = gyro.getFIFOCount();
gyro.getFIFOBytes(fifoBuffer, packetSize);
actualMicros = micros();
microsSpent = actualMicros - lastMicros;
lastMicros = actualMicros;
fifoCount -= packetSize;
gyro.dmpGetQuaternion(&q, fifoBuffer);
gyro.dmpGetGravity(&gravity, &q);
gyro.dmpGetYawPitchRoll(ypr, &q, &gravity);
gyro.dmpGetAccel(&rawAccelerometer, fifoBuffer);
gyro.dmpGetLinearAccel(&rawAccelerometer, &rawAccelerometer, &gravity);
//gyro.dmpGetLinearAccelInWorld(&rawAccelerometer, &rawAccelerometer, &q);
accelerometer.x = rawAccelerometer.x/8192.0*9.8;
accelerometer.y = rawAccelerometer.y/8192.0*9.8;
accelerometer.z = rawAccelerometer.z/8192.0*9.8;
gyroscope.x = ypr[2];
gyroscope.y = ypr[1]*-1;
gyroscope.z = ypr[0]*-1;
if(loops_before_calibration > CALIBRATION_LOOPS){
gyroscopeUpdated = true;
}else loops_before_calibration++;
}
}
Example code(To upload to arduino):
#include <GYRO.h>
#include <helper_3dmath.h>
GYRO gyro = GYRO();
VectorFloat gravity;
VectorFloat gyroscope;
VectorDouble accelerometer;
void setup() {
Serial.begin(115200);
Serial.println(gyro.begin());
}
void loop() {
gyro.update();
if(gyro.isGyroscopeUpdated()){
gravity = gyro.getGravity();
accelerometer = gyro.getAccelerometer();
gyroscope = gyro.getGyroscope();
Serial.print("gyroscope\t");
Serial.print(gyroscope.x * 180/M_PI);
Serial.print("\t");
Serial.print(gyroscope.y * 180/M_PI);
Serial.print("\t");
Serial.print(gyroscope.z * 180/M_PI);
Serial.print("\taccelerometer\t");
Serial.print(accelerometer.x);
Serial.print("\t");
Serial.print(accelerometer.y);
Serial.print("\t");
Serial.print(accelerometer.z);
Serial.print("\tgravity\t");
Serial.print(gravity.x);
Serial.print("\t");
Serial.print(gravity.y);
Serial.print("\t");
Serial.print(gravity.z);
Serial.print("\ttemperature\t");
Serial.print(gyro.getTemperature());
Serial.print(", micros: ");
Serial.println(gyro.getMicrosSpent());
}
}
You have to provide your own offsets. To obtain it, you can use this link
I hope this code is useful.

Answer 2

I know it is too late but maybe will help others...
The first MPU6050 initialization part in the setup() is done before even the mux initialized.
So you have to do the following, inside the setup(), at the right order:
initialize wire
initialize the outputs for the mux
select the first MPU using the right output levels
initialize the first MPU6050
select the second MPU using the right output levels
initialize the second MPU6050
select the third MPU using the right output levels
initialize the third MPU6050