Here’s a practical, STM32 + IMU recipe (CubeIDE/HAL) to get orientation (roll/pitch/yaw). I’ll show a working 6-DoF path (MPU-6050 over I²C + complementary filter) and how to step up to 9-DoF with Madgwick.

**0) Hardware at a glance**

* MCU: any [STM32](https://www.ampheo.com/search/STM32) with I²C (e.g., F0/F1/F4/ G4/H7, etc.).
* IMU (example): [MPU-6050](https://www.ampheo.com/product/mpu-6050-26900976) ([Accel](https://www.onzuu.com/category/accelerometers)+[Gyro](https://www.onzuu.com/category/gyroscopes), 6-DoF). For full yaw, add a magnetometer (e.g., [HMC5883L](https://www.ampheo.com/search/HMC5883L)) or use a 9-DoF IMU ([MPU-9250](https://www.ampheo.com/product/mpu-9250-26900747) / [ICM-20948](https://www.ampheo.com/product/icm-20948-26900336)).
* Wiring (3.3 V logic):

* VCC 3V3, GND
* SDA → I²C SDA (e.g., PB7), SCL → I²C SCL (e.g., PB6)
* Pull-ups: 4.7 kΩ to 3V3 on SDA/SCL if the board doesn’t have them
* MPU-6050 address: 0x68 (AD0=0) or 0x69 (AD0=1)

**1) CubeMX setup (quick)**

* I²C1 at 400 kHz (Fast Mode). Keep default timings if unsure.
* Optionally enable a hardware timer (e.g., TIM2) for a steady sample rate (200 Hz).
(You can also use HAL_GetTick() for a first version.)
* Generate the project (CubeIDE) so you get MX_I2C1_Init() and hi2c1.

**2) MPU-6050 init + read (HAL, C)**

Paste these into a .c file (e.g., imu.c). Assumes I2C_HandleTypeDef hi2c1; exists.

#include "main.h"
#include <math.h>
#include <string.h>

#define MPU6050_ADDR         (0x68 << 1)  // HAL expects 8-bit address
#define REG_PWR_MGMT_1       0x6B
#define REG_SMPLRT_DIV       0x19
#define REG_CONFIG           0x1A
#define REG_GYRO_CONFIG      0x1B
#define REG_ACCEL_CONFIG     0x1C
#define REG_ACCEL_XOUT_H     0x3B
#define REG_WHO_AM_I         0x75

// Sensitivities at ±2g and ±250 dps:
#define ACC_SENS_2G          16384.0f     // LSB/g
#define GYRO_SENS_250DPS     131.0f       // LSB/(deg/s)
#define DEG2RAD              (0.0174532925199f)

extern I2C_HandleTypeDef hi2c1;

static HAL_StatusTypeDef mpu_write(uint8_t reg, uint8_t val) {
  return HAL_I2C_Mem_Write(&hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, &val, 1, 100);
}

static HAL_StatusTypeDef mpu_read(uint8_t reg, uint8_t *buf, uint16_t len) {
  return HAL_I2C_Mem_Read(&hi2c1, MPU6050_ADDR, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 100);
}

HAL_StatusTypeDef mpu6050_init(void) {
  uint8_t id = 0;
  if (mpu_read(REG_WHO_AM_I, &id, 1) != HAL_OK) return HAL_ERROR;
  // WHO_AM_I should be 0x68 (with AD0=0). Not all clones follow this strictly.
  // Wake up, set config, sample rates:
  if (mpu_write(REG_PWR_MGMT_1, 0x00) != HAL_OK) return HAL_ERROR;  // clear sleep
  HAL_Delay(10);
  // DLPF: e.g., 0x03 (~44 Hz accel BW, 42 Hz gyro BW), stable for handheld
  if (mpu_write(REG_CONFIG, 0x03) != HAL_OK) return HAL_ERROR;
  // Sample rate: Fs = 1kHz / (1 + SMPLRT_DIV). For 200 Hz → 1k / (1 + 4) = 200 Hz
  if (mpu_write(REG_SMPLRT_DIV, 4) != HAL_OK) return HAL_ERROR;
  // Gyro ±250 dps (00), Accel ±2g (00):
  if (mpu_write(REG_GYRO_CONFIG,  0x00) != HAL_OK) return HAL_ERROR;
  if (mpu_write(REG_ACCEL_CONFIG, 0x00) != HAL_OK) return HAL_ERROR;
  return HAL_OK;
}

typedef struct {
  float ax, ay, az;   // g
  float gx, gy, gz;   // rad/s
} imu_sample_t;

HAL_StatusTypeDef mpu6050_read_sample(imu_sample_t *s) {
  uint8_t raw[14];
  if (mpu_read(REG_ACCEL_XOUT_H, raw, 14) != HAL_OK) return HAL_ERROR;

  int16_t ax = (raw[0] << 8) | raw[1];
  int16_t ay = (raw[2] << 8) | raw[3];
  int16_t az = (raw[4] << 8) | raw[5];
  // raw[6..7] = temp (ignored)
  int16_t gx = (raw[8] << 8) | raw[9];
  int16_t gy = (raw[10] << 8) | raw[11];
  int16_t gz = (raw[12] << 8) | raw[13];

  s->ax = (float)ax / ACC_SENS_2G;                  // g
  s->ay = (float)ay / ACC_SENS_2G;
  s->az = (float)az / ACC_SENS_2G;
  // Convert gyro to rad/s
  float gx_dps = (float)gx / GYRO_SENS_250DPS;
  float gy_dps = (float)gy / GYRO_SENS_250DPS;
  float gz_dps = (float)gz / GYRO_SENS_250DPS;
  s->gx = gx_dps * DEG2RAD;
  s->gy = gy_dps * DEG2RAD;
  s->gz = gz_dps * DEG2RAD;

  return HAL_OK;
}

**3) Gyro bias (quick calibration)**

Average a few hundred samples at rest and subtract as constant offsets:

void calibrate_gyro(float *bx, float *by, float *bz) {
  imu_sample_t s; *bx = *by = *bz = 0.f;
  const int N = 500;
  for (int i=0; i<N; ++i) {
    mpu6050_read_sample(&s);
    *bx += s.gx; *by += s.gy; *bz += s.gz;
    HAL_Delay(2); // ~500 Hz loop → 1 s total
  }
  *bx /= N; *by /= N; *bz /= N;
}

**4) Complementary filter (6-DoF → Roll/Pitch)**

This fuses accelerometer (good for tilt) with gyro (good for dynamics). Yaw requires a magnetometer or other absolute reference.

typedef struct {
  float roll, pitch;   // rad
  float gbx, gby, gbz; // gyro biases (rad/s)
} attitude_t;

static void attitude_init(attitude_t *a) { a->roll = a->pitch = 0.f; }

static void attitude_update(attitude_t *a, const imu_sample_t *s, float dt)
{
  // 1) Accelerometer tilt (robust at rest)
  float roll_acc  = atan2f(s->ay, s->az);
  float pitch_acc = atan2f(-s->ax, sqrtf(s->ay*s->ay + s->az*s->az));

  // 2) Gyro integrate (subtract bias)
  float gx = s->gx - a->gbx;
  float gy = s->gy - a->gby;

  float roll_gyro  = a->roll  + gx * dt;
  float pitch_gyro = a->pitch + gy * dt;

  // 3) Blend (alpha ~ 0.98 @ ~100–200 Hz)
  const float alpha = 0.98f;
  a->roll  = alpha*roll_gyro  + (1.f - alpha)*roll_acc;
  a->pitch = alpha*pitch_gyro + (1.f - alpha)*pitch_acc;
}

**5) Main loop (200 Hz example)**

int main(void)
{
  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_I2C1_Init();

  if (mpu6050_init() != HAL_OK) Error_Handler();

  attitude_t att; attitude_init(&att);
  calibrate_gyro(&att.gbx, &att.gby, &att.gbz);

  uint32_t t_prev = HAL_GetTick(); // ms
  for (;;) {
    imu_sample_t s;
    if (mpu6050_read_sample(&s) == HAL_OK) {
      uint32_t t_now = HAL_GetTick();
      float dt = (t_now - t_prev) / 1000.0f; // seconds
      t_prev = t_now;

      // Guard against dt=0 (same tick)
      if (dt <= 0.f) dt = 0.005f; // ~200 Hz fallback

      attitude_update(&att, &s, dt);

      // Convert to degrees if needed
      float roll_deg  = att.roll  * 57.29578f;
      float pitch_deg = att.pitch * 57.29578f;

      // TODO: print via UART / send over USB / log
    }

    // Simple pacing for ~200 Hz if you didn't set SMPLRT_DIV accordingly
    HAL_Delay(5);
  }
}

For more accurate dt, use a hardware timer (TIMx) or enable the DWT cycle counter on Cortex-M (where available) to measure microseconds.

**6) Getting Yaw (heading) — two options**
**A) Add a magnetometer (recommended for absolute yaw)**

* Add HMC5883L (I²C) or use a 9-DoF IMU (e.g., ICM-20948, MPU-9250).
* Read and calibrate the magnetometer (hard/soft-iron).
* Run a quaternion AHRS (e.g., Madgwick or Mahony) with accel+gyro+mag.

* You’ll get a quaternion q → derive yaw (and keep roll/pitch consistent).
* Typical Madgwick parameter: beta around 0.04–0.2; tune for noise vs. responsiveness.

**B) Gyro-only yaw (not absolute)**

Integrate gz to get relative yaw—but it drifts over time without magnetometer or other references.

**7) Calibration & robustness checklist**

* Gyro bias at startup (as above). Re-calibrate if temperature drifts matter.
* Accel calibration (6-point) improves tilt accuracy (scale/offset).
* Magnetometer: always do hard/soft-iron calibration (figure-8s), reject bad readings (norm check).
* LPF: keep IMU DLPF on (e.g., CONFIG=0x03) to suppress high-freq noise.
* Mounting: define your [sensor](https://www.ampheoelec.de/c/sensors) axes; if your board’s IMU orientation differs, rotate the measured vector into your body frame before fusion.

**8) Stepping up to 9-DoF quickly (drop-in plan)**

* Swap to a 9-DoF IMU (ICM-20948/MPU-9250). Use SPI if you want higher throughput and fewer I²C pitfalls.
* Keep the same HAL structure: imu_init(), read_9dof(&ax,ay,az,&gx,gy,gz,&mx,my,mz).
* Add a Madgwick (or Mahony) update function:

// Pseudocode call (float units: g, rad/s, uT; dt in s)
MadgwickUpdate(ax, ay, az, gx, gy, gz, mx, my, mz, dt, &q0, &q1, &q2, &q3);
// Convert quaternion to Euler if desired

* Start with 200 Hz fusion rate; ensure consistent dt.

**Common issues (and fixes)**

* I²C NACKs: wrong address (remember HAL expects 8-bit addr <<1), no pull-ups, bus too fast → drop to 100 kHz for testing.
* No motion but angles wander: missing gyro bias subtraction; DLPF off; alpha too high/low.
* Yaw nonsense: magnetometer not calibrated or disturbed (motors, steel nearby).
* Jittery Euler: compute and log quaternions, convert to Euler only for display.