How to code and use IMU sensor?

Here’s a compact, end-to-end playbook for coding and using a 6-axis/9-axis IMU on common boards (Arduino, STM32, Raspberry Pi). I’ll use the very common MPU-6050 (accel+gyro) as a register-level example; the same flow applies to LSM6DS3/ICM-20948/etc. with different registers.

1) What you need to decide up front

• Bus: I²C is simplest (SPI for higher rates).
• Full-scale ranges: accel ±2/4/8/16 g; gyro ±250/500/1000/2000 °/s. Pick the smallest that won’t saturate.
• ODR / filtering: choose sample rate (e.g., 200–1000 Hz) and enable a low-pass filter to reduce noise.
• Fusion target: raw accel/gyro → orientation via Madgwick/Mahony/Kalman (or sensor’s built-in DMP if available).
• Calibration: you must remove gyro bias and accel bias; add mag hard/soft iron if using a 9-axis IMU.

2) Wire it (I²C)

• VCC → 3.3 V or 5 V (check your breakout)
• GND → GND
• SDA → SDA (Arduino A4, STM32 I2C SDA, Pi GPIO2)
• SCL → SCL (Arduino A5, STM32 I2C SCL, Pi GPIO3)
• Address (MPU-6050): 0x68 (AD0=0) or 0x69 (AD0=1)

3) Initialize the IMU (MPU-6050 example)

Registers you’ll touch:

• PWR_MGMT_1(0x6B): wake from sleep (write 0x00)
• SMPLRT_DIV(0x19): sample rate divider
• CONFIG(0x1A): DLPF (e.g., 0x03)
• GYRO_CONFIG(0x1B): FS_SEL
• ACCEL_CONFIG(0x1C): AFS_SEL
• Data regs: ACCEL_XOUT_H(0x3B)…GYRO_ZOUT_L(0x48)

Scaling (defaults):

• accel ±2 g → 16384 LSB/g
• gyro ±250 °/s → 131 LSB/(°/s)

A) Arduino (Wire) — minimal register-level read

#include <Wire.h>

const uint8_t MPU = 0x68;

int16_t read16(uint8_t reg) {
  Wire.beginTransmission(MPU); Wire.write(reg); Wire.endTransmission(false);
  Wire.requestFrom(MPU, (uint8_t)2);
  int16_t hi = Wire.read(), lo = Wire.read();
  return (hi << 8) | lo;
}

void write8(uint8_t reg, uint8_t val){
  Wire.beginTransmission(MPU); Wire.write(reg); Wire.write(val); Wire.endTransmission();
}

void setup() {
  Serial.begin(115200);
  Wire.begin();

  // Wake
  write8(0x6B, 0x00);         // PWR_MGMT_1: clear sleep
  write8(0x1A, 0x03);         // CONFIG: DLPF ~44 Hz gyro
  write8(0x1B, 0x00);         // GYRO_CONFIG: ±250 dps
  write8(0x1C, 0x00);         // ACCEL_CONFIG: ±2 g
  write8(0x19, 0x07);         // SMPLRT_DIV: 1 kHz/(7+1)=125 Hz
  delay(100);
}

void loop() {
  int16_t ax = read16(0x3B), ay = read16(0x3D), az = read16(0x3F);
  int16_t gx = read16(0x43), gy = read16(0x45), gz = read16(0x47);

  // Convert to physical units
  float ax_g = ax / 16384.0f, ay_g = ay / 16384.0f, az_g = az / 16384.0f;
  float gx_dps = gx / 131.0f,  gy_dps = gy / 131.0f,  gz_dps = gz / 131.0f;

  Serial.print("A[g]: "); Serial.print(ax_g); Serial.print(", ");
  Serial.print(ay_g); Serial.print(", "); Serial.print(az_g);
  Serial.print("  G[dps]: "); Serial.print(gx_dps); Serial.print(", ");
  Serial.print(gy_dps); Serial.print(", "); Serial.println(gz_dps);

  delay(8); // ~125 Hz loop
}

Prefer a tested library (Adafruit/SparkFun) for other IMUs; the rest of the pipeline (scaling, fusion) stays the same.

B) STM32 (HAL) — read 14 bytes burst (I²C)

// Assume hi2c1 is configured. IMU at 0x68<<1 for HAL.
#define MPU_ADDR (0x68<<1)

void MPU_Write(uint8_t reg, uint8_t val){
  HAL_I2C_Mem_Write(&hi2c1, MPU_ADDR, reg, 1, &val, 1, 100);
}
void MPU_ReadBurst(uint8_t startReg, uint8_t *buf, uint16_t len){
  HAL_I2C_Mem_Read(&hi2c1, MPU_ADDR, startReg, 1, buf, len, 100);
}

void MPU_Init(void){
  MPU_Write(0x6B, 0x00); // wake
  MPU_Write(0x1A, 0x03); // DLPF
  MPU_Write(0x1B, 0x00); // gyro ±250
  MPU_Write(0x1C, 0x00); // accel ±2g
  MPU_Write(0x19, 0x07); // 125 Hz
  HAL_Delay(100);
}

void ReadIMU(float *ax, float *ay, float *az, float *gx, float *gy, float *gz){
  uint8_t d[14];
  MPU_ReadBurst(0x3B, d, 14);
  int16_t AX = (d[0]<<8)|d[1], AY=(d[2]<<8)|d[3], AZ=(d[4]<<8)|d[5];
  int16_t GX = (d[8]<<8)|d[9], GY=(d[10]<<8)|d[11], GZ=(d[12]<<8)|d[13];
  *ax = AX / 16384.0f; *ay = AY / 16384.0f; *az = AZ / 16384.0f;
  *gx = GX / 131.0f;   *gy = GY / 131.0f;   *gz = GZ / 131.0f;
}

Use FIFO + data-ready interrupt for stable timing at higher rates.

C) Raspberry Pi (Python, smbus2)

import time, struct
from smbus2 import SMBus
MPU = 0x68

with SMBus(1) as bus:
    # wake & config
    bus.write_byte_data(MPU, 0x6B, 0x00)
    bus.write_byte_data(MPU, 0x1A, 0x03)
    bus.write_byte_data(MPU, 0x1B, 0x00)
    bus.write_byte_data(MPU, 0x1C, 0x00)
    bus.write_byte_data(MPU, 0x19, 0x07)
    time.sleep(0.1)

    def read_vec():
        data = bus.read_i2c_block_data(MPU, 0x3B, 14)
        AX, AY, AZ, _, GX, GY, GZ = struct.unpack(">hhhxhhhh", bytes(data))
        ax, ay, az = AX/16384.0, AY/16384.0, AZ/16384.0
        gx, gy, gz = GX/131.0,  GY/131.0,  GZ/131.0
        return ax, ay, az, gx, gy, gz

    while True:
        ax, ay, az, gx, gy, gz = read_vec()
        print(f"A[g]={ax:.3f},{ay:.3f},{az:.3f}  G[dps]={gx:.1f},{gy:.1f},{gz:.1f}")
        time.sleep(0.008)  # ~125 Hz

4) Sensor fusion (turn raw data into orientation)

Madgwick and Mahony are lightweight and perfect for MCUs.

Workflow:

1. Sample accel (g), gyro (°/s), (optional) mag (µT).
2. Convert gyro to rad/s.
3. Call the filter update with Δt.
4. Get quaternion; convert to Euler (roll/pitch/yaw) if needed.

Pseudocode (Madgwick with 6-axis):

// given ax, ay, az [g]; gx, gy, gz [deg/s]; dt [s]
gx *= (M_PI/180.0f); gy *= ...; gz *= ...;
MadgwickUpdateIMU(gx, gy, gz, ax, ay, az, dt);
// then read q0,q1,q2,q3; convert to roll/pitch/yaw if you like

On Arduino, you can use MadgwickAHRS or MahonyAHRS libraries. On STM32, pull in the small C source. On Pi, pip packages exist or port the C file.

Important: Euler angles gimbal-lock; keep quaternions in your math pipeline and only convert for display.

5) Calibration (don’t skip)

• Gyro bias: keep the board still for 3–5 s, average gyro → subtract each frame.
• Accel bias/scale: six-position method (±X/±Y/±Z faces up). Fit bias & scale so measured vector magnitude ≈ 1 g on each face.
• Magnetometer (if 9-axis): figure-eight motion, run ellipsoid fit → hard-iron offset & soft-iron matrix.
• Align frames: define your app’s body frame and rotate sensor readings if the IMU is mounted at an angle.

6) Timing & performance tips

• Stable Δt: use a hardware timer or data-ready interrupt; avoid delay() jitter.
• FIFO: read bursts from the sensor FIFO to prevent overrun at high ODR.
• DLPF: enable to cut noise and aliasing (e.g., ~42–98 Hz for human motion).
• Ranges: start small (±2 g, ±250 dps) for resolution; increase only if you see clipping.
• Units: keep gyro in rad/s and accel in m/s² inside your fusion math.

7) Quick “works-first-time” checklist

• See the correct WHO_AM_I value before configuring (MPU-6050: 0x68).
• Verify counts change when you tilt (accel) and rotate (gyro).
• Remove gyro bias; you should see yaw drift greatly reduced.
• Fusion output (quaternion) should settle with gravity pointing down (pitch/roll ~0 when level).
• If using a magnetometer, yaw should match a compass after calibration.