MPU-6050: Correctly reading data from the FIFO register

Question

Introduction

The MPU-6050 is a popular module that contains a temperature sensor, accelerometer, and gyroscope. A user may read the sensor information over I2C or SPI. Two documents are publicly available for reading data out of the IC registers. These are:

Context

Reading individual registers of the IMU over I2C skews samples across time because of bus communication latency. Consequently, a sequential read of the X, Y, and Z axis registers of a sensor are not synchronized. To address this, the device provides a 1024-byte internal FIFO queue. Data configured to be pushed to the queue are pushed together at the sample rate. Hence reading the FIFO yields synchronized data.

See (2), section 7.17:

The MPU-60X0 contains a 1024-byte FIFO register that is accessible via the Serial Interface. The FIFO configuration register determines which data is written into the FIFO. Possible choices include gyro data, accelerometer data, temperature readings, auxiliary sensor readings, and FSYNC input. A FIFO counter keeps track of how many bytes of valid data are contained in the FIFO. The FIFO register supports burst reads. The interrupt function may be used to determine when new data is available

Problem

The datasheets specify that in order to read from the FIFO, you must perform the following:

Enable the FIFO (bit 6, register 0x6A, Document (1), Section 4.29)
Configure the FIFO with what sensor information to push (register 0x23, Document (1), Section 4.7). I enable XG_FIFO_EN, YG_FIFO_EN, ZG_FIFO_EN, and ACCEL_FIFO_EN by setting bits 6, 5, 4, and 3 respectively.

If you have performed these steps, then it claims (Document (1), Section 4.33) that:

Data is written to the FIFO in order of register number (from lowest to highest). If all the FIFO enable flags (see below) are enabled and all External Sensor Data registers (Registers 73 to 96) are associated with a Slave device, the contents of registers 59 through 96 will be written in order at the Sample Rate. The contents of the sensor data registers (Registers 59 to 96) are written into the FIFO buffer when their corresponding FIFO enable flags are set to 1 in FIFO_EN (Register 35).

However, I find that this does not hold true. Given the flags I have enabled in the configuration register, I expect the following sequence to come from the FIFO:

 * ----------------------------------------------------------- *
 *     BYTE #    |         VALUE          |    Register (dec)  *
 * ----------------------------------------------------------- *
 *       0       |     ACCEL_XOUT[15:8]   |         59         *
 *       1       |     ACCEL_XOUT[7:0]    |         60         *
 * ----------------------------------------------------------- *
 *       2       |     ACCEL_YOUT[15:8]   |         61         *
 *       3       |     ACCEL_YOUT[7:0]    |         62         *
 * ----------------------------------------------------------- *
 *       4       |     ACCEL_ZOUT[15:8]   |         63         *
 *       5       |     ACCEL_ZOUT[7:0]    |         64         *
 * ----------------------------------------------------------- *
 *       6       |     GYRO_XOUT[15:8]    |         67         *
 *       7       |     GYRO_XOUT[7:0]     |         68         *
 * ----------------------------------------------------------- *
 *       8       |     GYRO_YOUT[15:8]    |         69         *
 *       9       |     GYRO_YOUT[7:0]     |         70         *
 * ----------------------------------------------------------- *
 *      10       |     GYRO_ZOUT[15:8]    |         71         *
 *      11       |     GYRO_ZOUT[7:0]     |         72         *
 * ----------------------------------------------------------- *

Yet reading 12 bytes from the FIFO does not correspond with the same data when reading individual registers. It also doesn't seem to make much sense when I accelerate the IMU, or rotate it. I therefore am not sure how exactly to read the FIFO. This is the problem I face

Q&A

Are you sure you are correctly writing to registers?: Yes, I am able to set various configurations such as the sampling rate, interrupts, etc. I am confident I am correctly able to read from the FIFO
Are you sure there is anything in the FIFO to read?: Yes, I have enabled FIFO overflow interrupts. I currently wait for an interrupt, and then read from the FIFO register.
Are you checking the FIFO length register before reading? Yes, it contains 1024 bytes (maximum capacity) when the FIFO-overflow interrupt occurs.
Haven't other people done this before?: Nobody has a concrete explanation on how to read the FIFO (e.g: this similar question on another forum that gets an RTFM). A majority of searchable questions related to reading the FIFO are (a) unanswered, (b) told to use generic XYZ Arduino library (I cannot use it), (c) told to read the data sheet (I have).

@0andriy I am not aware of a Linux kernel driver for this MPU. It's typically used in small embedded projects. Do you know of one? Perhaps I've just been ignorant of it. — Micrified, Feb 27 '20 at 21:01
Driver is here: https://elixir.bootlin.com/linux/latest/source/drivers/iio/imu/inv_mpu6050 AFAICS it uses FIFO: https://elixir.bootlin.com/linux/latest/source/drivers/iio/imu/inv_mpu6050/inv_mpu_core.c#L693, etc. — 0andriy, Feb 27 '20 at 21:10

score 10 · Accepted Answer · answered Mar 02 '20 at 19:22

Okay, so I've figured out the problem. The issue was that I was failing to reset the FIFO prior to reading it - otherwise everything was more or less okay. I'll show you exactly how I setup the IMU now.

Source Files

I created a source file to read the MPU-6050 registers. I've attached them here for reference in the following explanation:

Setup

In order to setup the IMU, I performed the following steps within a FreeRTOS task (prior to the main loop).

// Performs the I2C configuration for the MPU-6050 IMU. Saves handle
static mpu6050_err_t init_imu (mpu6050_i2c_cfg_t **handle) {
    mpu6050_err_t err = MPU6050_ERR_OK;
    uint8_t flags;

    // Configure the MPU-6050 I2C data structure
    static mpu6050_i2c_cfg_t i2c_cfg = (mpu6050_i2c_cfg_t) {
        .sda_pin        = I2C_SDA_PIN,
        .scl_pin        = I2C_SCL_PIN,
        .slave_addr     = I2C_IMU_SLAVE_ADDR,
        .i2c_port       = I2C_IMU_PORT_NUM,
        .clk_speed      = I2C_APB_CLK_FREQ / 200,    // Requires 400kHz
        .sda_pullup_en  = IMU_ENABLE_INTERNAL_PULLUPS,
        .scl_pullup_en  = IMU_ENABLE_INTERNAL_PULLUPS
    };

    // Initialize I2C
    if ((err = mpu6050_init(&i2c_cfg)) != MPU6050_ERR_OK) {
        return err;
    }

    // Configure Power Management 1 to wake the IMU (don't reset)
    flags = 0x0;
    if ((err = mpu6050_configure_power(&i2c_cfg, flags)) != MPU6050_ERR_OK) {
        return err;
    }

    // Configure accelerometer sensitivity
    flags = A_CFG_8G;
    if ((err = mpu6050_configure_accelerometer(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Configure gyro sensitivity
    flags = G_CFG_500;
    if ((err = mpu6050_configure_gyroscope(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Configure the Digital-Low-Pass-Filter
    flags = DLFP_CFG_FILTER_2;
    if ((err = mpu6050_configure_dlfp(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Set the sampling rate to ~50Hz
    flags = 19;
    if ((err = mpu6050_set_sample_rate_divider(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Configure interrupt behavior
    flags = 0x0;
    if ((err = mpu6050_configure_interrupt(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Enable interrupts after every sensor refresh
    flags = INTR_EN_DATA_RDY;
    if ((err = mpu6050_enable_interrupt(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Enable + Reset the FIFO
    flags = USER_CTRL_FIFO_EN | USER_CTRL_FIFO_RST;
    if ((err = mpu6050_enable_fifo(&i2c_cfg, flags)) 
        != MPU6050_ERR_OK) {
        return err;
    }

    // Configure the data pushed to the FIFO
    flags = FIFO_CFG_GX | FIFO_CFG_GY | FIFO_CFG_GZ | FIFO_CFG_AXYZ;
    if ((err = mpu6050_configure_fifo(&i2c_cfg, flags)) != MPU6050_ERR_OK) {
        return err;
    }

    // Save the configuration
    *handle = &i2c_cfg;

    return err;
}

If you configure as I described, then it should work. Of course, you may be using a different library or wrapper for the device, but the functions you can enable should be similarly accessible. Once I had done all this, I was able to read the FIFO at each interrupt as follows:

// Read the FIFO length
if (mpu6050_get_fifo_length(i2c_cfg_p, &len) != MPU6050_ERR_OK) {
    ERR("FIFO length fetch error!");
    break;
} 

// Check if enough samples are ready - else continue (check later)
if (len < FIFO_BURST_LEN) {
    continue;
}

// Fetch data from FIFO
if (mpu6050_receive_fifo(i2c_cfg_p, &data) != MPU6050_ERR_OK) {
    ERR("FIFO data fetch error!");
    break;
}

So much diligent work. And so useful. Thank you. Is MPU6500 / 9250 similar? — Slavic, Oct 30 '22 at 09:28

score 1 · Answer 2 · edited Mar 09 '23 at 13:04

I've gone through this same process trying to get the MPU6050 to work in XC8 on a PIC18F47Q43 (running at 64MHz). I just thought I'd share some straight forward code and tips because there's basically nothing online outside of Arduino libraries and at least you can know that this works (for me).

A big part of this success was in the initialization of the module. If you don't leave enough time to power on, reset, and set signal paths, your settings don't get set correctly. Here's what my setup function looks like (I'm only only setting up the accelerometer in this):

void MPU6050setup(void)
{
    __delay_ms(200);
    
    I2Cwrite(MPU, 0x6B, 0b10000000); // Reset device
    __delay_ms(100);
    I2Cwrite(MPU, 0x68, 0b00000111); // Reset sensors
    __delay_ms(100);
    
    I2Cwrite(MPU, 0x6B, 0b00000001); // Internal Clock set to Gyro output
    I2Cwrite(MPU, 0x6A, 0b00000100); // Reset FIFO
    I2Cwrite(MPU, 0x19, 7);          // Sample frequency
    I2Cwrite(MPU, 0x1A, 0b00000001); // DLPF on set to lowest filter
    I2Cwrite(MPU, 0x1C, 0b00000000); // Accelerometer sensitivity
    I2Cwrite(MPU, 0x23, 0b00001000); // Load Accel to FIFO
    I2Cwrite(MPU, 0x38, 0b00000001); // Data ready Interrupt
    I2Cwrite(MPU, 0x6A, 0b01000000); // Enable FIFO
}

Two more tips to make sure you can read the data properly:

1. Read the FIFO by relying on the data ready interrupt flag.

I've read that the FIFO overflow interrupt flag is quite temperamental so I've just steered clear entirely. This approach using the data ready flag means you have to poll the flag constantly.

2. You have to burst read the FIFO.

This is the only way to get correct readings, and its the same for the FIFO Sample Count registers. If you're reading it a byte at a time with all the addresses, stop and start bits, you're not going to get the correct data.

With this approach I've described, when you see the data ready flag = 1, you should just be able to just read the 0x74 register the amount of times of the expected data . Each sensor has 2 registers, so say to get all the accelerometer readings, you read the 0x74 register 6 times, and you get the data for XH, XL, YH, YL, ZH, ZL respectively. You can just repeat this the next time round, no need to clear or reset FIFO.