How to Read MPU6050 and BNO055 IMU Data on STM32 Using I2C and Madgwick Filter

Introduction

If you need orientation data from an IMU—think robotics, drones, motion tracking—raw accelerometer and gyroscope readings aren't enough on their own. You need a sensor fusion algorithm to turn those noisy readings into a stable orientation estimate. The Madgwick filter is one of the best options: it's computationally cheap, runs easily on an STM32, and gives you quaternion output that avoids gimbal lock.

This guide covers reading accelerometer and gyroscope data from an MPU6050 (and optionally magnetometer data from a BNO055) over I2C on an STM32, then feeding it through the Madgwick filter. We'll use STM32CubeIDE v1.16+ and the HAL drivers.

Prerequisites

STM32 microcontroller (e.g., STM32F4 series or similar)
STM32CubeIDE v1.16+ installed
Solid understanding of C programming
MPU6050 IMU module (6-axis: accel + gyro)
BNO055 IMU module (9-axis: accel + gyro + mag, or use just its magnetometer)
Breadboard and jumper wires
Madgwick filter source files (freely available from Sebastian Madgwick's original implementation)

Parts/Tools

STM32 Development Board (e.g., Nucleo-F446RE or similar)
MPU6050 Breakout Module
BNO055 Breakout Module
Breadboard and Jumper Wires
STM32CubeIDE v1.16+
Madgwick filter source code (MadgwickAHRS.c / MadgwickAHRS.h)

Steps

Connect the IMUs to the STM32
- Both the MPU6050 and BNO055 sit on the same I2C bus, but they have different default addresses (MPU6050: 0x68, BNO055: 0x28), so there's no conflict.
```
MPU6050:
  VCC  -> 3.3V
  GND  -> GND
  SDA  -> I2C1_SDA (e.g., PB7)
  SCL  -> I2C1_SCL (e.g., PB6)

BNO055:
  VCC  -> 3.3V
  GND  -> GND
  SDA  -> I2C1_SDA (same bus)
  SCL  -> I2C1_SCL (same bus)
```
- Watch out: the MPU6050's AD0 pin sets the LSB of its I2C address. If AD0 is floating, you might get intermittent communication failures. Tie it to GND (address 0x68) or VCC (address 0x69) explicitly.
- Use 4.7kΩ pull-up resistors on SDA and SCL if your breakout boards don't already have them. Many do, but if you're running two modules on the same bus, double-check that you don't end up with parallel pull-ups that are too strong (below ~2kΩ combined).
Set Up STM32CubeIDE
- Create a new STM32 project in STM32CubeIDE v1.16+. Select your target MCU.
- In the .ioc pinout configurator, enable I2C1.
- Set I2C speed to Fast Mode (400 kHz)—both the MPU6050 and BNO055 support it, and you want the fastest read times possible for sensor fusion.
- Under Project Manager, make sure "Generate peripheral initialization as a pair of .c/.h files" is checked. Keeps things organized.
- Generate the project code.
Add the Madgwick Filter
- Drop MadgwickAHRS.c and MadgwickAHRS.h into your project's Src and Inc folders respectively.
- Include the header in your main source file:
```
#include "MadgwickAHRS.h"
```
- The filter has a beta gain parameter that controls how aggressively it corrects drift. Start with 0.1 and tune from there. Higher values respond faster to accelerometer data but are noisier; lower values are smoother but drift more.

Initialize I2C and IMUs


// I2C is already initialized by HAL_I2C_Init() in the generated MX_I2C1_Init()

// Wake up the MPU6050 (it starts in sleep mode)
uint8_t data = 0x00;
HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x6B, 1, &data, 1, HAL_MAX_DELAY);

// Configure MPU6050 accel range (+/- 2g) and gyro range (+/- 250 deg/s)
data = 0x00;
HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x1C, 1, &data, 1, HAL_MAX_DELAY);
HAL_I2C_Mem_Write(&hi2c1, 0x68 << 1, 0x1B, 1, &data, 1, HAL_MAX_DELAY);

// BNO055: set to NDOF mode for fused 9-axis output, or CONFIG mode if you only want raw mag
data = 0x0C; // NDOF mode
HAL_I2C_Mem_Write(&hi2c1, 0x28 << 1, 0x3D, 1, &data, 1, HAL_MAX_DELAY);

Tip: After writing the BNO055 mode register, wait at least 20ms before reading data. The datasheet specifies mode switch times, and skipping the delay produces garbage on the first few reads.

Read Data from the IMUs


int16_t raw[3];
uint8_t buf[6];
float accelData[3], gyroData[3], magData[3];

// Read MPU6050 accelerometer (registers 0x3B-0x40)
HAL_I2C_Mem_Read(&hi2c1, 0x68 << 1, 0x3B, 1, buf, 6, HAL_MAX_DELAY);
for (int i = 0; i < 3; i++) {
    raw[i] = (int16_t)(buf[i*2] << 8 | buf[i*2+1]);
    accelData[i] = raw[i] / 16384.0f;  // +/- 2g range
}

// Read MPU6050 gyroscope (registers 0x43-0x48)
HAL_I2C_Mem_Read(&hi2c1, 0x68 << 1, 0x43, 1, buf, 6, HAL_MAX_DELAY);
for (int i = 0; i < 3; i++) {
    raw[i] = (int16_t)(buf[i*2] << 8 | buf[i*2+1]);
    gyroData[i] = raw[i] / 131.0f;  // +/- 250 deg/s range
}

// Read BNO055 magnetometer (registers 0x0E-0x13)
HAL_I2C_Mem_Read(&hi2c1, 0x28 << 1, 0x0E, 1, buf, 6, HAL_MAX_DELAY);
for (int i = 0; i < 3; i++) {
    raw[i] = (int16_t)(buf[i*2] | buf[i*2+1] << 8);  // BNO055 is little-endian
    magData[i] = raw[i] / 16.0f;  // micro-Tesla
}

Watch out for byte order: the MPU6050 is big-endian (MSB first), but the BNO055 is little-endian (LSB first). Mix these up and your data will be wildly wrong.

Compute Quaternion Orientation


// Convert gyro from deg/s to rad/s for the Madgwick filter
float gx = gyroData[0] * (M_PI / 180.0f);
float gy = gyroData[1] * (M_PI / 180.0f);
float gz = gyroData[2] * (M_PI / 180.0f);

MadgwickAHRSupdate(gx, gy, gz,
                   accelData[0], accelData[1], accelData[2],
                   magData[0], magData[1], magData[2]);

If you're not using the magnetometer, call MadgwickAHRSupdateIMU() instead—it takes just the accel and gyro data. You'll get 6DOF orientation (no absolute heading, but pitch and roll are fine).

Output the Quaternion
```
extern volatile float q0, q1, q2, q3;  // Madgwick filter globals
printf("Quaternion: %.4f %.4f %.4f %.4f\r\n", q0, q1, q2, q3);
```
Tip: run this in a timed loop (use a hardware timer or SysTick) at a consistent rate—100Hz is a good starting point. The Madgwick filter's sample rate parameter needs to match your actual loop rate, otherwise the orientation estimate will drift or oscillate.

Troubleshooting

IMU Not Responding (HAL_ERROR or HAL_TIMEOUT): Run an I2C address scan to confirm the devices are visible on the bus. Check pull-up resistors and wiring. Also verify you're left-shifting the 7-bit address by 1 for the HAL functions (e.g., 0x68 << 1).
Data Looks Wrong or Jumpy: Verify your scaling factors match the configured range. If you set the accelerometer to +/- 4g but divide by 16384 (the +/- 2g scale factor), your readings will be off by 2x.
Quaternion Output Drifts or Is Unstable: Check that gyro data is in radians per second, not degrees. Make sure the filter's sample frequency matches your actual loop rate. Tune the beta parameter—start at 0.1 and adjust.
BNO055 Returns Zeros: After changing operating mode, you need to wait for the mode switch delay (up to 600ms for NDOF mode). Also check that the BNO055 has finished its self-calibration (status registers 0x35-0x36).

Conclusion

You've got MPU6050 accel/gyro and BNO055 magnetometer data flowing over I2C into a Madgwick filter, producing clean quaternion orientation on your STM32. This setup works well for drones, robotics, motion controllers, and any project where you need to know which way something is pointing. For your next steps, consider adding a complementary filter for comparison, logging data to an SD card for post-analysis, or converting quaternions to Euler angles for easier visualization.