STM32 Control of MPU6050 for Six‑Axis Data and Euler Angles Using DMP

The MPU6050 combines a 3‑axis gyroscope, a 3‑axis accelerometer, and an embedded Digital Motion Processor (DMP). The auxiliary I2C port can host an external magnetometer to extend to nine axes. When the DMP is enabled, quaternions and fused orinetation can be produced on-chip, reducing MCU load.

• Output formats: rotation matrix, quaternion, and optionally Euler angles via host computation
• Gyroscope ranges: ±250/±500/±1000/±2000 °/s (131 LSB/(°/s) at ±250)
• Accelerometer ranges: ±2/±4/±8/±16 g
• 1 kB FIFO, Digital Low-Pass Filter (DLPF), on-chip temperature sansor
• I2C up to 400 kHz; VLOGIC as low as 1.8 V; typical GYRO current ≈ 5 mA

Pin usage for six-axis output typically requries: VCC (3.3 V), GND, SCL, SDA, and AD0. AD0 selects the I2C adress (0x68 when low, 0x69 when high). INT can be used for data-ready or DMP interrupts. XCL/XDA are used only when an external sensor is connected to the auxiliary bus.

Key registers (7-bit address 0x68 when AD0=0):

• Power Management 1 (0x6B)
  • DEVICE_RESET, SLEEP, TEMP_DIS, CLKSEL[2:0] (commonly use PLL with X‑gyro)
• Gyro Config (0x1B)
  • FS_SEL[1:0] sets gyro full-scale
• Accel Config (0x1C)
  • AFS_SEL[1:0] sets accel full-scale
• Sample Rate Divider (0x19)
  • Sample Rate = Gyro Output Rate / (1 + SMPLRT_DIV)
• Config (0x1A)
  • DLPF settings; typical choice is half of sample rate
• FIFO Enable (0x23)
  • Per-sensor FIFO enables (often unused for simple polled reads)
• Temperature (0x41/0x42)
  • Temp(°C) = 36.53 + raw/340

Firmware layout on STM32:

• A software I2C (bit‑bang) layer to talk to the sensor
• Register access helpers for single and burst read/writes
• Device init and configuration (FS ranges, rate, DLPF)
• DMP integration (InvenSense driver port) to fetch quaternions
• Quaternion to Euler angle conversion on the MCU

I2C bit‑bang layer (PB10=SCL, PB11=SDA; AD0 on PA15):

Header (imu_i2c.h):

#ifndef IMU_I2C_H
#define IMU_I2C_H

#include "stm32f10x.h"
#include "delay.h"

/* PB11 dynamic direction control for SDA */
#define IMU_SDA_IN()   {GPIOB->CRH &= 0xFFFF0FFF; GPIOB->CRH |= 8 << 12;}   /* Input with pull-up */
#define IMU_SDA_OUT()  {GPIOB->CRH &= 0xFFFF0FFF; GPIOB->CRH |= 3 << 12;}   /* Push-pull output */

#define IMU_SCL_HIGH()   GPIO_SetBits(GPIOB, GPIO_Pin_10)
#define IMU_SCL_LOW()    GPIO_ResetBits(GPIOB, GPIO_Pin_10)
#define IMU_SDA_HIGH()   GPIO_SetBits(GPIOB, GPIO_Pin_11)
#define IMU_SDA_LOW()    GPIO_ResetBits(GPIOB, GPIO_Pin_11)

#define IMU_AD0_HIGH()   GPIO_SetBits(GPIOA, GPIO_Pin_15)
#define IMU_AD0_LOW()    GPIO_ResetBits(GPIOA, GPIO_Pin_15)

#define IMU_SDA_READ()   GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11)

static inline void imu_i2c_delay(void) { delay_us(2); }

void imu_i2c_init(void);
void imu_i2c_start(void);
void imu_i2c_stop(void);
uint8_t imu_i2c_wait_ack(void);
void imu_i2c_send_ack(void);
void imu_i2c_send_nack(void);
void imu_i2c_write_byte(uint8_t byte);
uint8_t imu_i2c_read_byte(uint8_t ack);

#endif

Source (imu_i2c.c):

#include "imu_i2c.h"

/* SCL:PB10, SDA:PB11, AD0:PA15 */
void imu_i2c_init(void)
{
    GPIO_InitTypeDef gpio;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE);

    /* Free PA15 from JTAG */
    GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE);

    /* PB10/PB11 outputs default high */
    gpio.GPIO_Speed = GPIO_Speed_50MHz;
    gpio.GPIO_Mode  = GPIO_Mode_Out_PP;
    gpio.GPIO_Pin   = GPIO_Pin_10 | GPIO_Pin_11;
    GPIO_Init(GPIOB, &gpio);

    /* PA15 as AD0 control */
    gpio.GPIO_Pin = GPIO_Pin_15;
    gpio.GPIO_Mode = GPIO_Mode_Out_PP;
    GPIO_Init(GPIOA, &gpio);

    IMU_SCL_HIGH();
    IMU_SDA_HIGH();
    IMU_AD0_LOW(); /* I2C address = 0x68 */
}

void imu_i2c_start(void)
{
    IMU_SDA_OUT();
    IMU_SDA_HIGH();
    IMU_SCL_HIGH();
    imu_i2c_delay();
    IMU_SDA_LOW();
    imu_i2c_delay();
    IMU_SCL_LOW();
}

void imu_i2c_stop(void)
{
    IMU_SDA_OUT();
    IMU_SDA_LOW();
    IMU_SCL_HIGH();
    imu_i2c_delay();
    IMU_SDA_HIGH();
    imu_i2c_delay();
}

uint8_t imu_i2c_wait_ack(void)
{
    uint16_t t = 0;
    IMU_SDA_IN();
    IMU_SDA_HIGH(); /* release */
    IMU_SCL_HIGH();
    imu_i2c_delay();
    while (IMU_SDA_READ()) {
        if (++t > 500) {
            imu_i2c_stop();
            return 1; /* NACK timeout */
        }
    }
    IMU_SCL_LOW();
    return 0;
}

void imu_i2c_send_ack(void)
{
    IMU_SDA_OUT();
    IMU_SDA_LOW();
    imu_i2c_delay();
    IMU_SCL_HIGH();
    imu_i2c_delay();
    IMU_SCL_LOW();
}

void imu_i2c_send_nack(void)
{
    IMU_SDA_OUT();
    IMU_SDA_HIGH();
    imu_i2c_delay();
    IMU_SCL_HIGH();
    imu_i2c_delay();
    IMU_SCL_LOW();
}

void imu_i2c_write_byte(uint8_t byte)
{
    IMU_SDA_OUT();
    for (uint8_t i = 0; i < 8; ++i) {
        IMU_SCL_LOW();
        if (byte & 0x80) IMU_SDA_HIGH(); else IMU_SDA_LOW();
        byte <<= 1;
        imu_i2c_delay();
        IMU_SCL_HIGH();
        imu_i2c_delay();
    }
    IMU_SCL_LOW();
}

uint8_t imu_i2c_read_byte(uint8_t ack)
{
    uint8_t data = 0;
    IMU_SDA_IN();
    for (uint8_t i = 0; i < 8; ++i) {
        IMU_SCL_LOW();
        imu_i2c_delay();
        IMU_SCL_HIGH();
        data <<= 1;
        if (IMU_SDA_READ()) data |= 0x01;
        imu_i2c_delay();
    }
    if (ack) imu_i2c_send_ack(); else imu_i2c_send_nack();
    return data;
}

MPU6050 register helpers and configuraton:

Header (mpu6050.h):

#ifndef MPU6050_IFACE_H
#define MPU6050_IFACE_H

#include "stm32f10x.h"
#include "imu_i2c.h"

#define MPU_I2C_ADDR       0x68

/* Selected register addresses */
#define MPU_SAMPLE_RATE_REG   0x19
#define MPU_CFG_REG           0x1A
#define MPU_GYRO_CFG_REG      0x1B
#define MPU_ACCEL_CFG_REG     0x1C
#define MPU_FIFO_EN_REG       0x23
#define MPU_INTBP_CFG_REG     0x37
#define MPU_INT_EN_REG        0x38
#define MPU_ACCEL_XOUTH_REG   0x3B
#define MPU_ACCEL_XOUTL_REG   0x3C
#define MPU_ACCEL_YOUTH_REG   0x3D
#define MPU_ACCEL_YOUTL_REG   0x3E
#define MPU_ACCEL_ZOUTH_REG   0x3F
#define MPU_ACCEL_ZOUTL_REG   0x40
#define MPU_TEMP_OUTH_REG     0x41
#define MPU_TEMP_OUTL_REG     0x42
#define MPU_GYRO_XOUTH_REG    0x43
#define MPU_GYRO_XOUTL_REG    0x44
#define MPU_GYRO_YOUTH_REG    0x45
#define MPU_GYRO_YOUTL_REG    0x46
#define MPU_GYRO_ZOUTH_REG    0x47
#define MPU_GYRO_ZOUTL_REG    0x48
#define MPU_USER_CTRL_REG     0x6A
#define MPU_PWR_MGMT1_REG     0x6B
#define MPU_PWR_MGMT2_REG     0x6C
#define MPU_DEVICE_ID_REG     0x75

/* I2C accessors */
uint8_t imu_read_reg8(uint8_t reg);
uint8_t imu_write_reg8(uint8_t reg, uint8_t val);
uint8_t imu_burst_read(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);
uint8_t imu_burst_write(uint8_t addr, uint8_t reg, uint8_t len, const uint8_t *buf);

/* High-level API */
uint8_t imu_init(void);
uint8_t imu_set_gyro_fsr(uint8_t fsr);
uint8_t imu_set_accel_fsr(uint8_t fsr);
uint8_t imu_set_lpf(uint16_t hz);
uint8_t imu_set_rate(uint16_t hz);
short   imu_get_temperature(void);
uint8_t imu_get_gyro_raw(short *gx, short *gy, short *gz);
uint8_t imu_get_accel_raw(short *ax, short *ay, short *az);

#endif

Source (mpu6050.c):

#include "mpu6050.h"
#include "delay.h"

static void imu_select_addr_low(void)
{
    /* AD0 already configured in imu_i2c_init */
    IMU_AD0_LOW();
}

uint8_t imu_read_reg8(uint8_t reg)
{
    uint8_t val;
    imu_i2c_start();
    imu_i2c_write_byte((MPU_I2C_ADDR << 1) | 0);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 0xFF; }
    imu_i2c_write_byte(reg);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 0xFF; }
    imu_i2c_start();
    imu_i2c_write_byte((MPU_I2C_ADDR << 1) | 1);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 0xFF; }
    val = imu_i2c_read_byte(0);
    imu_i2c_stop();
    return val;
}

uint8_t imu_write_reg8(uint8_t reg, uint8_t val)
{
    imu_i2c_start();
    imu_i2c_write_byte((MPU_I2C_ADDR << 1) | 0);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_write_byte(reg);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_write_byte(val);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_stop();
    return 0;
}

uint8_t imu_burst_read(uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
    imu_i2c_start();
    imu_i2c_write_byte((addr << 1) | 0);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_write_byte(reg);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_start();
    imu_i2c_write_byte((addr << 1) | 1);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    while (len--) {
        *buf++ = imu_i2c_read_byte(len ? 1 : 0);
    }
    imu_i2c_stop();
    return 0;
}

uint8_t imu_burst_write(uint8_t addr, uint8_t reg, uint8_t len, const uint8_t *buf)
{
    imu_i2c_start();
    imu_i2c_write_byte((addr << 1) | 0);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    imu_i2c_write_byte(reg);
    if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    while (len--) {
        imu_i2c_write_byte(*buf++);
        if (imu_i2c_wait_ack()) { imu_i2c_stop(); return 1; }
    }
    imu_i2c_stop();
    return 0;
}

uint8_t imu_set_gyro_fsr(uint8_t fsr)
{
    /* fsr: 0=±250,1=±500,2=±1000,3=±2000 dps */
    return imu_write_reg8(MPU_GYRO_CFG_REG, fsr << 3);
}

uint8_t imu_set_accel_fsr(uint8_t fsr)
{
    /* fsr: 0=±2g,1=±4g,2=±8g,3=±16g */
    return imu_write_reg8(MPU_ACCEL_CFG_REG, fsr << 3);
}

uint8_t imu_set_lpf(uint16_t hz)
{
    /* Map to DLPF config codes (per datasheet) */
    uint8_t cfg;
    if (hz >= 188) cfg = 1;
    else if (hz >= 98) cfg = 2;
    else if (hz >= 42) cfg = 3;
    else if (hz >= 20) cfg = 4;
    else if (hz >= 10) cfg = 5;
    else cfg = 6;
    return imu_write_reg8(MPU_CFG_REG, cfg);
}

uint8_t imu_set_rate(uint16_t hz)
{
    if (hz > 1000) hz = 1000;
    if (hz < 4) hz = 4;
    uint8_t div = (uint8_t)(1000 / hz - 1);
    if (imu_write_reg8(MPU_SAMPLE_RATE_REG, div)) return 1;
    return imu_set_lpf(hz / 2);
}

short imu_get_temperature(void)
{
    uint8_t b[2];
    imu_burst_read(MPU_I2C_ADDR, MPU_TEMP_OUTH_REG, 2, b);
    int16_t raw = ((int16_t)b[0] << 8) | b[1];
    float t = 36.53f + ((float)raw) / 340.0f;
    return (short)(t * 100.0f); /* centi-degrees C */
}

uint8_t imu_get_gyro_raw(short *gx, short *gy, short *gz)
{
    uint8_t b[6];
    if (imu_burst_read(MPU_I2C_ADDR, MPU_GYRO_XOUTH_REG, 6, b)) return 1;
    *gx = (int16_t)(((uint16_t)b[0] << 8) | b[1]);
    *gy = (int16_t)(((uint16_t)b[2] << 8) | b[3]);
    *gz = (int16_t)(((uint16_t)b[4] << 8) | b[5]);
    return 0;
}

uint8_t imu_get_accel_raw(short *ax, short *ay, short *az)
{
    uint8_t b[6];
    if (imu_burst_read(MPU_I2C_ADDR, MPU_ACCEL_XOUTH_REG, 6, b)) return 1;
    *ax = (int16_t)(((uint16_t)b[0] << 8) | b[1]);
    *ay = (int16_t)(((uint16_t)b[2] << 8) | b[3]);
    *az = (int16_t)(((uint16_t)b[4] << 8) | b[5]);
    return 0;
}

uint8_t imu_init(void)
{
    uint8_t who;

    imu_i2c_init();
    imu_select_addr_low();

    /* Reset then wake */
    imu_write_reg8(MPU_PWR_MGMT1_REG, 0x80);
    delay_ms(100);
    imu_write_reg8(MPU_PWR_MGMT1_REG, 0x00);

    /* Disable interrupts/FIFO, set INT active low */
    imu_write_reg8(MPU_INT_EN_REG, 0x00);
    imu_write_reg8(MPU_USER_CTRL_REG, 0x00);
    imu_write_reg8(MPU_FIFO_EN_REG, 0x00);
    imu_write_reg8(MPU_INTBP_CFG_REG, 0x80);

    /* WHO_AM_I should read 0x68 with AD0 low */
    who = imu_read_reg8(MPU_DEVICE_ID_REG);
    if (who != MPU_I2C_ADDR) return 1;

    /* Clock = PLL with X‑gyro; enable all axes */
    imu_write_reg8(MPU_PWR_MGMT1_REG, 0x01);
    imu_write_reg8(MPU_PWR_MGMT2_REG, 0x00);

    /* FS ranges and rate */
    if (imu_set_gyro_fsr(3)) return 2;     /* ±2000 dps */
    if (imu_set_accel_fsr(0)) return 3;    /* ±2 g */
    if (imu_set_rate(50)) return 4;        /* 50 Hz */

    return 0;
}

DMP integration (using InvenSense drivers):

Provide the I2C and delay hooks expected by the DMP library:

/* Map to local I2C helpers used by InvenSense drivers */
#define i2c_write  imu_burst_write
#define i2c_read   imu_burst_read
#define delay_ms   delay_ms

Initialization of the DMP:

#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

/* Orientation matrix (example for board alignment); adjust to your mount */
static const signed char gyro_orientation[9] = {
     1, 0, 0,
     0, 1, 0,
     0, 0, 1
};

#define DEFAULT_MPU_HZ  50

uint8_t dmp_startup(void)
{
    uint8_t r;

    /* Basic device init already done by imu_init() */
    if (mpu_init() != 0) return 10;

    r = mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);      if (r) return 1;
    r = mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);   if (r) return 2;
    r = mpu_set_sample_rate(DEFAULT_MPU_HZ);                if (r) return 3;
    r = dmp_load_motion_driver_firmware();                  if (r) return 4;
    r = dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));
    if (r) return 5;
    r = dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT |
                           DMP_FEATURE_TAP |
                           DMP_FEATURE_ANDROID_ORIENT |
                           DMP_FEATURE_SEND_RAW_ACCEL |
                           DMP_FEATURE_SEND_CAL_GYRO |
                           DMP_FEATURE_GYRO_CAL);
    if (r) return 6;
    r = dmp_set_fifo_rate(DEFAULT_MPU_HZ);                  if (r) return 7;
    r = run_self_test();                                    if (r) return 8;
    r = mpu_set_dmp_state(1);                               if (r) return 9;
    return 0;
}

Getting Euler angles from the DMP quaternion:

#include <math.h>

uint8_t dmp_read_euler(float *pitch, float *roll, float *yaw)
{
    /* DMP writes quaternions in q30 format */
    const float q30 = 1073741824.0f; /* 2^30 */
    long q[4];
    unsigned long ts;
    short g[3], a[3];
    short sensors;
    unsigned char more;

    if (dmp_read_fifo(g, a, q, &ts, &sensors, &more)) return 1;
    if (!(sensors & INV_WXYZ_QUAT)) return 2;

    float q0 = q[0] / q30;
    float q1 = q[1] / q30;
    float q2 = q[2] / q30;
    float q3 = q[3] / q30;

    /* ZYX convention: pitch (x), roll (y), yaw (z) in degrees */
    *pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)) * 57.29578f;
    *roll  = atan2f( 2.0f * (q2 * q3 + q0 * q1),
                    1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 57.29578f;
    *yaw   = atan2f( 2.0f * (q1 * q2 + q0 * q3),
                     q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3) * 57.29578f;
    return 0;
}

Example main() using the Standard Peripheral Library:

#include "stm32f10x.h"
#include "usart.h"
#include "delay.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"
#include <stdio.h>

int main(void)
{
    float pitch = 0, roll = 0, yaw = 0;
    short ax, ay, az;
    short gx, gy, gz;
    short centiC;

    NVIC_PriorityGroupConfig(2);
    delay_init();
    USART1_Init(115200);

    if (imu_init() != 0) {
        printf("IMU init failed\n");
        while (1) {}
    }
    if (dmp_startup() != 0) {
        printf("DMP init failed\n");
        while (1) {}
    }

    while (1) {
        if (dmp_read_euler(&pitch, &roll, &yaw) == 0) {
            centiC = imu_get_temperature();
            imu_get_accel_raw(&ax, &ay, &az);
            imu_get_gyro_raw(&gx, &gy, &gz);
            printf("pitch:%0.2f roll:%0.2f yaw:%0.2f temp:%0.2fC\r\n",
                   pitch, roll, yaw, centiC / 100.0f);
        }
        delay_ms(100);
    }
}

Notes:

• If PA15 is used for AD0, disable JTAG with GPIO_Remap_SWJ_JTAGDisable to free the pin
• Ensure I2C lines have pull-ups when bit‑banging
• DLPF and sample rate must be selected with aliasing in mind; a common choice is DLPF ≈ rate/2
• The orientation matrix must reflect the physical board mounting to obtain correct Euler angles