MPU-6050: Correctly reading data from the FIFO register

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).

Solution

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