Attitude Estimation· Live
Home / Chapter 2 / 2.5

STM32: a modular sensor driver

Lesson 2.5 · STM32 · free

Video coming soon

A driver is one header and one source file for our sensor: it knows the registers, but it never talks to the board directly. We add the two files to the project and see where each number comes from in the datasheet.

In this lesson · 14 steps
  1. Add two files
  2. ISM330DHCX (our sensor): ism330dhcx.h
  3. ISM330DHCX (our sensor): ism330dhcx.c
  4. Other IMU · LSM6DSOX: lsm6dsox.h
  5. Other IMU · LSM6DSOX: lsm6dsox.c
  6. Other IMU · MPU-6050: mpu6050.h
  7. Other IMU · MPU-6050: mpu6050.c
  8. Other IMU · MPU-9250 (+ AK8963 magnetometer): mpu9250.h
  9. Other IMU · MPU-9250 (+ AK8963 magnetometer): mpu9250.c
  10. Other IMU · ICM-20948 (+ AK09916 magnetometer): icm20948.h
  11. Other IMU · ICM-20948 (+ AK09916 magnetometer): icm20948.c
  12. Other IMU · ICM-42688-P: icm42688.h
  13. Other IMU · ICM-42688-P: icm42688.c
  14. Reference: what you call

1Add two files

  1. Core/Inc → right-click → New → Header File: ism330dhcx.h
  2. Core/Src → right-click → New → Source File: ism330dhcx.c
  3. Another IMU? Open its two files below (hidden by default) and use them instead. Same shape, same imu_t, same three board functions: only the include and the _init / _read names change in main.c

2ISM330DHCX (our sensor): ism330dhcx.h

Core/Inc/ism330dhcx.h · the whole file

The top part (imu_t and the three function types) is the link to the board: the same block is in every driver.

Show the code
/* ISM330DHCX (ST, industrial 6-axis IMU). Same register map as the ISM330DHCX / LSM6DSOX family. */
/* Datasheet: https://www.st.com/en/mems-and-sensors/ism330dhcx.html */
#ifndef ISM330DHCX_H
#define ISM330DHCX_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */

#ifdef __cplusplus
extern "C" {
#endif

#define ISM330DHCX_ADDR       0x6A   /* SDO pin to GND (0x6B if SDO to 3V3) */

/* ---- output data rate (top 4 bits of CTRL1_XL / CTRL2_G) ---- */
#define ISM330DHCX_ODR_12HZ   0x1
#define ISM330DHCX_ODR_26HZ   0x2
#define ISM330DHCX_ODR_52HZ   0x3
#define ISM330DHCX_ODR_104HZ  0x4
#define ISM330DHCX_ODR_208HZ  0x5
#define ISM330DHCX_ODR_416HZ  0x6
#define ISM330DHCX_ODR_833HZ  0x7

/* ---- accelerometer range (bits 3:2 of CTRL1_XL) ---- */
#define ISM330DHCX_ACC_2G     0x0   /* 0.061 mg per count */
#define ISM330DHCX_ACC_4G     0x2   /* 0.122 mg per count */
#define ISM330DHCX_ACC_8G     0x3   /* 0.244 mg per count */
#define ISM330DHCX_ACC_16G    0x1   /* 0.488 mg per count */

/* ---- gyroscope range (bits 3:2 of CTRL2_G) ---- */
#define ISM330DHCX_GYR_250DPS  0x0  /*  8.75 mdps per count */
#define ISM330DHCX_GYR_500DPS  0x1  /* 17.5  mdps per count */
#define ISM330DHCX_GYR_1000DPS 0x2  /* 35    mdps per count */
#define ISM330DHCX_GYR_2000DPS 0x3  /* 70    mdps per count */

/* ---- our choice: change these three lines ---- */
#define ISM330DHCX_ODR        ISM330DHCX_ODR_104HZ
#define ISM330DHCX_ACC_RANGE  ISM330DHCX_ACC_4G
#define ISM330DHCX_GYR_RANGE  ISM330DHCX_GYR_500DPS

bool ism330dhcx_init(imu_t *imu);                   /* WHO_AM_I, reset, rate + range  */
bool ism330dhcx_read(imu_t *imu, imu_raw_t *out);   /* gyro + accel in one read       */

#ifdef __cplusplus
}
#endif

#endif /* ISM330DHCX_H */
Why?
Change the rate and the two ranges in the three lines under "our choice".

3ISM330DHCX (our sensor): ism330dhcx.c

Core/Src/ism330dhcx.c · the whole file

Only this file knows the registers.

Show the code
#include "ism330dhcx.h"

/* registers (datasheet, register map) */
#define WHO_AM_I       0x0F
#define CTRL1_XL       0x10   /* accelerometer: rate + range */
#define CTRL2_G        0x11   /* gyroscope:     rate + range */
#define CTRL3_C        0x12   /* reset, auto-increment       */
#define OUTX_L_G       0x22   /* first data register         */

#define WHO_AM_I_VALUE 0x6B

bool ism330dhcx_init(imu_t *imu)
{
  uint8_t id = 0;

  /* 1. is it the right chip? */
  if (imu->read(imu->address, WHO_AM_I, &id, 1) != 0) return false;
  if (id != WHO_AM_I_VALUE) return false;

  /* 2. reset */
  if (imu->write(imu->address, CTRL3_C, 0x01) != 0) return false;
  imu->delay(10);

  /* 3. auto-increment on (for burst reads) + block data update */
  if (imu->write(imu->address, CTRL3_C, 0x44) != 0) return false;

  /* 4. rate + range: rate in the top 4 bits, range in bits 3:2 */
  if (imu->write(imu->address, CTRL1_XL, (ISM330DHCX_ODR << 4) | (ISM330DHCX_ACC_RANGE << 2)) != 0) return false;
  if (imu->write(imu->address, CTRL2_G,  (ISM330DHCX_ODR << 4) | (ISM330DHCX_GYR_RANGE << 2)) != 0) return false;

  return true;
}

bool ism330dhcx_read(imu_t *imu, imu_raw_t *out)
{
  uint8_t b[12];

  /* one read, 12 registers: gyro x, y, z, then accel x, y, z */
  if (imu->read(imu->address, OUTX_L_G, b, 12) != 0) return false;

  /* two bytes -> one number: low byte first, then high byte */
  out->gx = (int16_t)(b[1]  << 8 | b[0]);
  out->gy = (int16_t)(b[3]  << 8 | b[2]);
  out->gz = (int16_t)(b[5]  << 8 | b[4]);
  out->ax = (int16_t)(b[7]  << 8 | b[6]);
  out->ay = (int16_t)(b[9]  << 8 | b[8]);
  out->az = (int16_t)(b[11] << 8 | b[10]);
  return true;
}
Why?
It never calls HAL: only imu->read, imu->write and imu->delay, the three board functions.

4Other IMU · LSM6DSOX: lsm6dsox.h

Core/Inc/lsm6dsox.h · the whole file (instead of ism330dhcx.h)
Show the code
/* LSM6DSOX (ST, 6-axis IMU). Same register map as the ISM330DHCX / LSM6DSOX family. */
/* Datasheet: https://www.st.com/en/mems-and-sensors/lsm6dsox.html */
#ifndef LSM6DSOX_H
#define LSM6DSOX_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */

#ifdef __cplusplus
extern "C" {
#endif

#define LSM6DSOX_ADDR       0x6A   /* SDO pin to GND (0x6B if SDO to 3V3) */

/* ---- output data rate (top 4 bits of CTRL1_XL / CTRL2_G) ---- */
#define LSM6DSOX_ODR_12HZ   0x1
#define LSM6DSOX_ODR_26HZ   0x2
#define LSM6DSOX_ODR_52HZ   0x3
#define LSM6DSOX_ODR_104HZ  0x4
#define LSM6DSOX_ODR_208HZ  0x5
#define LSM6DSOX_ODR_416HZ  0x6
#define LSM6DSOX_ODR_833HZ  0x7

/* ---- accelerometer range (bits 3:2 of CTRL1_XL) ---- */
#define LSM6DSOX_ACC_2G     0x0   /* 0.061 mg per count */
#define LSM6DSOX_ACC_4G     0x2   /* 0.122 mg per count */
#define LSM6DSOX_ACC_8G     0x3   /* 0.244 mg per count */
#define LSM6DSOX_ACC_16G    0x1   /* 0.488 mg per count */

/* ---- gyroscope range (bits 3:2 of CTRL2_G) ---- */
#define LSM6DSOX_GYR_250DPS  0x0  /*  8.75 mdps per count */
#define LSM6DSOX_GYR_500DPS  0x1  /* 17.5  mdps per count */
#define LSM6DSOX_GYR_1000DPS 0x2  /* 35    mdps per count */
#define LSM6DSOX_GYR_2000DPS 0x3  /* 70    mdps per count */

/* ---- our choice: change these three lines ---- */
#define LSM6DSOX_ODR        LSM6DSOX_ODR_104HZ
#define LSM6DSOX_ACC_RANGE  LSM6DSOX_ACC_4G
#define LSM6DSOX_GYR_RANGE  LSM6DSOX_GYR_500DPS

bool lsm6dsox_init(imu_t *imu);                   /* WHO_AM_I, reset, rate + range  */
bool lsm6dsox_read(imu_t *imu, imu_raw_t *out);   /* gyro + accel in one read       */

#ifdef __cplusplus
}
#endif

#endif /* LSM6DSOX_H */

5Other IMU · LSM6DSOX: lsm6dsox.c

Core/Src/lsm6dsox.c · the whole file (instead of ism330dhcx.c)
Show the code
#include "lsm6dsox.h"

/* registers (datasheet, register map) */
#define WHO_AM_I       0x0F
#define CTRL1_XL       0x10   /* accelerometer: rate + range */
#define CTRL2_G        0x11   /* gyroscope:     rate + range */
#define CTRL3_C        0x12   /* reset, auto-increment       */
#define OUTX_L_G       0x22   /* first data register         */

#define WHO_AM_I_VALUE 0x6C

bool lsm6dsox_init(imu_t *imu)
{
  uint8_t id = 0;

  /* 1. is it the right chip? */
  if (imu->read(imu->address, WHO_AM_I, &id, 1) != 0) return false;
  if (id != WHO_AM_I_VALUE) return false;

  /* 2. reset */
  if (imu->write(imu->address, CTRL3_C, 0x01) != 0) return false;
  imu->delay(10);

  /* 3. auto-increment on (for burst reads) + block data update */
  if (imu->write(imu->address, CTRL3_C, 0x44) != 0) return false;

  /* 4. rate + range: rate in the top 4 bits, range in bits 3:2 */
  if (imu->write(imu->address, CTRL1_XL, (LSM6DSOX_ODR << 4) | (LSM6DSOX_ACC_RANGE << 2)) != 0) return false;
  if (imu->write(imu->address, CTRL2_G,  (LSM6DSOX_ODR << 4) | (LSM6DSOX_GYR_RANGE << 2)) != 0) return false;

  return true;
}

bool lsm6dsox_read(imu_t *imu, imu_raw_t *out)
{
  uint8_t b[12];

  /* one read, 12 registers: gyro x, y, z, then accel x, y, z */
  if (imu->read(imu->address, OUTX_L_G, b, 12) != 0) return false;

  /* two bytes -> one number: low byte first, then high byte */
  out->gx = (int16_t)(b[1]  << 8 | b[0]);
  out->gy = (int16_t)(b[3]  << 8 | b[2]);
  out->gz = (int16_t)(b[5]  << 8 | b[4]);
  out->ax = (int16_t)(b[7]  << 8 | b[6]);
  out->ay = (int16_t)(b[9]  << 8 | b[8]);
  out->az = (int16_t)(b[11] << 8 | b[10]);
  return true;
}

6Other IMU · MPU-6050: mpu6050.h

Core/Inc/mpu6050.h · the whole file (instead of ism330dhcx.h)
Show the code
/* MPU-6050 (InvenSense/TDK): the classic cheap 6-axis IMU (GY-521 boards). */
/* Datasheet: https://invensense.tdk.com/products/motion-tracking/6-axis/mpu-6050/  (register map: RM-MPU-6000A) */
#ifndef MPU6050_H
#define MPU6050_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */

#ifdef __cplusplus
extern "C" {
#endif

#define MPU6050_ADDR        0x68   /* AD0 pin to GND (0x69 if AD0 to 3V3) */

/* ---- output data rate: 1 kHz / (1 + divider) ---- */
#define MPU6050_ODR_50HZ    19
#define MPU6050_ODR_100HZ   9
#define MPU6050_ODR_200HZ   4
#define MPU6050_ODR_500HZ   1

/* ---- accelerometer range (bits 4:3 of ACCEL_CONFIG) ---- */
#define MPU6050_ACC_2G      0x0    /* 16384 counts per g */
#define MPU6050_ACC_4G      0x1    /*  8192 counts per g */
#define MPU6050_ACC_8G      0x2    /*  4096 counts per g */
#define MPU6050_ACC_16G     0x3    /*  2048 counts per g */

/* ---- gyroscope range (bits 4:3 of GYRO_CONFIG) ---- */
#define MPU6050_GYR_250DPS  0x0    /* 131  counts per dps */
#define MPU6050_GYR_500DPS  0x1    /* 65.5 counts per dps */
#define MPU6050_GYR_1000DPS 0x2    /* 32.8 counts per dps */
#define MPU6050_GYR_2000DPS 0x3    /* 16.4 counts per dps */

/* ---- our choice: change these three lines ---- */
#define MPU6050_ODR         MPU6050_ODR_200HZ
#define MPU6050_ACC_RANGE   MPU6050_ACC_4G
#define MPU6050_GYR_RANGE   MPU6050_GYR_500DPS

bool mpu6050_init(imu_t *imu);
bool mpu6050_read(imu_t *imu, imu_raw_t *out);

#ifdef __cplusplus
}
#endif

#endif /* MPU6050_H */

7Other IMU · MPU-6050: mpu6050.c

Core/Src/mpu6050.c · the whole file (instead of ism330dhcx.c)
Show the code
#include "mpu6050.h"

/* registers (datasheet, register map) */
#define SMPLRT_DIV     0x19   /* rate divider */
#define CONFIG         0x1A   /* low-pass filter */
#define GYRO_CONFIG    0x1B   /* gyroscope range */
#define ACCEL_CONFIG   0x1C   /* accelerometer range */
#define ACCEL_XOUT_H   0x3B   /* first data register */
#define PWR_MGMT_1     0x6B   /* reset, sleep */
#define WHO_AM_I       0x75

bool mpu6050_init(imu_t *imu)
{
  uint8_t id = 0;

  /* 1. is it the right chip? 0x68 = MPU-6050. Clones often answer 0x70, 0x72 or 0x98 (same registers). */
  if (imu->read(imu->address, WHO_AM_I, &id, 1) != 0) return false;
  if (id != 0x68 && id != 0x70 && id != 0x72 && id != 0x98) return false;

  /* 2. reset, then wake up (the chip starts asleep) */
  if (imu->write(imu->address, PWR_MGMT_1, 0x80) != 0) return false;
  imu->delay(100);
  if (imu->write(imu->address, PWR_MGMT_1, 0x01) != 0) return false;
  imu->delay(10);

  /* 3. rate + range */
  if (imu->write(imu->address, SMPLRT_DIV,   MPU6050_ODR) != 0)              return false;
  if (imu->write(imu->address, CONFIG,       0x03) != 0)                     return false; /* low-pass ~44 Hz */
  if (imu->write(imu->address, GYRO_CONFIG,  MPU6050_GYR_RANGE << 3) != 0)   return false;
  if (imu->write(imu->address, ACCEL_CONFIG, MPU6050_ACC_RANGE << 3) != 0)   return false;
  return true;
}

bool mpu6050_read(imu_t *imu, imu_raw_t *out)
{
  uint8_t b[14];

  /* one read, 14 registers: accel x, y, z, temperature, gyro x, y, z */
  if (imu->read(imu->address, ACCEL_XOUT_H, b, 14) != 0) return false;

  /* two bytes -> one number: HIGH byte first on this chip */
  out->ax = (int16_t)(b[0]  << 8 | b[1]);
  out->ay = (int16_t)(b[2]  << 8 | b[3]);
  out->az = (int16_t)(b[4]  << 8 | b[5]);
  /* b[6], b[7] = temperature */
  out->gx = (int16_t)(b[8]  << 8 | b[9]);
  out->gy = (int16_t)(b[10] << 8 | b[11]);
  out->gz = (int16_t)(b[12] << 8 | b[13]);
  return true;
}

8Other IMU · MPU-9250 (+ AK8963 magnetometer): mpu9250.h

Core/Inc/mpu9250.h · the whole file (instead of ism330dhcx.h)
Show the code
/* MPU-9250 / MPU-9255 (InvenSense/TDK): 9-axis = MPU-6500 IMU + AK8963 magnetometer in one package.
 * The IMU part is the MPU-6050 again. The magnetometer is a second chip inside, at address 0x0C.
 * We switch on "bypass" so the AK8963 sits directly on our I2C bus. */
/* Datasheet: https://invensense.tdk.com/?p=7057  (MPU-9250 product specification + register map) */
#ifndef MPU9250_H
#define MPU9250_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */
#ifdef __cplusplus
extern "C" {
#endif

#define MPU9250_ADDR      0x68
#define MPU9250_ADDR_ALT  0x69

/* ---- output data rate: 1 kHz / (1 + divider) ---- */
#define MPU9250_ODR_50HZ    19
#define MPU9250_ODR_100HZ   9
#define MPU9250_ODR_200HZ   4
#define MPU9250_ODR_500HZ   1
/* ---- accelerometer range ---- */
#define MPU9250_ACC_2G      0x0    /* 16384 counts per g */
#define MPU9250_ACC_4G      0x1    /*  8192 counts per g */
#define MPU9250_ACC_8G      0x2    /*  4096 counts per g */
#define MPU9250_ACC_16G     0x3    /*  2048 counts per g */
/* ---- gyroscope range ---- */
#define MPU9250_GYR_250DPS  0x0    /* 131  counts per dps */
#define MPU9250_GYR_500DPS  0x1    /* 65.5 counts per dps */
#define MPU9250_GYR_1000DPS 0x2    /* 32.8 counts per dps */
#define MPU9250_GYR_2000DPS 0x3    /* 16.4 counts per dps */

/* ---- our choice: change these three lines ---- */
#define MPU9250_ODR         MPU9250_ODR_200HZ
#define MPU9250_ACC_RANGE   MPU9250_ACC_4G
#define MPU9250_GYR_RANGE   MPU9250_GYR_500DPS

#define AK8963_ADDR       0x0C                 /* visible only after mpu9250_init() (bypass on) */
#define AK8963_MAG_SCALE  0.15f                /* uT per count, 16-bit mode */

bool mpu9250_init(imu_t *imu);
bool mpu9250_read(imu_t *imu, imu_raw_t *out);

bool ak8963_init(imu_t *imu);                /* call mpu9250_init() first */
bool ak8963_read(imu_t *imu, mag_raw_t *out);      /* signs as in stm32-simple-imu-reading */

#ifdef __cplusplus
}
#endif

#endif /* MPU9250_H */

9Other IMU · MPU-9250 (+ AK8963 magnetometer): mpu9250.c

Core/Src/mpu9250.c · the whole file (instead of ism330dhcx.c)
Show the code
#include "mpu9250.h"

/* ---- MPU-9250: same registers as the MPU-6050 ---- */
#define SMPLRT_DIV    0x19
#define CONFIG        0x1A
#define GYRO_CONFIG   0x1B
#define ACCEL_CONFIG  0x1C
#define ACCEL_CONFIG2 0x1D   /* accel low-pass filter (new on the 9250) */
#define INT_PIN_CFG   0x37   /* bit 1 = BYPASS_EN */
#define ACCEL_XOUT_H  0x3B   /* 14 bytes, high byte first */
#define USER_CTRL     0x6A   /* bit 5 = internal I2C master (must be off for bypass) */
#define PWR_MGMT_1    0x6B
#define WHO_AM_I      0x75   /* 0x71 = MPU-9250, 0x73 = MPU-9255 */

static bool answers(imu_t *imu)
{
    uint8_t id = 0;
    if (imu->read(imu->address, WHO_AM_I, &id, 1) != 0) return false;
    return id == 0x71 || id == 0x73;   /* 0x70 = MPU-6500: same IMU but NO magnetometer inside */
}

bool mpu9250_init(imu_t *imu)
{
    if (!answers(imu)) return false;

    if (imu->write(imu->address, PWR_MGMT_1, 0x80) != 0) return false;   /* reset */
    imu->delay(100);
    if (imu->write(imu->address, PWR_MGMT_1, 0x01) != 0) return false;   /* wake up, best clock */
    imu->delay(10);
    if (imu->write(imu->address, SMPLRT_DIV,    MPU9250_ODR) != 0) return false;
    if (imu->write(imu->address, CONFIG,        0x03) != 0) return false; /* gyro low-pass ~41 Hz  */
    if (imu->write(imu->address, ACCEL_CONFIG2, 0x03) != 0) return false; /* accel low-pass ~41 Hz */
    if (imu->write(imu->address, GYRO_CONFIG,   MPU9250_GYR_RANGE << 3) != 0) return false;
    if (imu->write(imu->address, ACCEL_CONFIG,  MPU9250_ACC_RANGE << 3) != 0) return false;
    if (imu->write(imu->address, USER_CTRL,     0x00) != 0) return false; /* internal I2C master off */
    if (imu->write(imu->address, INT_PIN_CFG,   0x02) != 0) return false; /* bypass on: AK8963 appears at 0x0C */
    imu->delay(10);
    return true;
}

bool mpu9250_read(imu_t *imu, imu_raw_t *out)
{
    uint8_t b[14];
    if (imu->read(imu->address, ACCEL_XOUT_H, b, 14) != 0) return false;
    out->ax = (int16_t)(b[0] << 8 | b[1]);
    out->ay = (int16_t)(b[2] << 8 | b[3]);
    out->az = (int16_t)(b[4] << 8 | b[5]);
    out->gx = (int16_t)(b[8] << 8 | b[9]);
    out->gy = (int16_t)(b[10] << 8 | b[11]); out->gz = (int16_t)(b[12] << 8 | b[13]);
    return true;
}

/* ---- AK8963 magnetometer (inside the MPU-9250) ---- */
#define AK_WIA      0x00   /* always 0x48 */
#define AK_HXL      0x03   /* 6 bytes, LOW byte first, then ST2 at 0x09 */
#define AK_CNTL1    0x0A   /* mode */
#define AK_CNTL2    0x0B   /* bit 0 = reset */
#define AK_ASAX     0x10   /* 3 factory trim bytes (read in "fuse ROM" mode) */

static uint8_t asa[3] = { 128, 128, 128 };

bool ak8963_init(imu_t *imu)
{
    uint8_t id = 0;
    if (imu->read(imu->address, AK_WIA, &id, 1) != 0 || id != 0x48) return false;

    if (imu->write(imu->address, AK_CNTL2, 0x01) != 0) return false;   /* reset */
    imu->delay(10);
    if (imu->write(imu->address, AK_CNTL1, 0x0F) != 0) return false;   /* fuse ROM mode */
    imu->delay(10);
    if (imu->read(imu->address, AK_ASAX, asa, 3) != 0) return false;  /* factory trim per axis */
    if (imu->write(imu->address, AK_CNTL1, 0x00) != 0) return false;   /* power down between modes */
    imu->delay(10);
    if (imu->write(imu->address, AK_CNTL1, 0x16) != 0) return false;   /* 16-bit, continuous 100 Hz */
    imu->delay(10);
    return true;
}

static int16_t trim(int16_t v, uint8_t a)            /* datasheet: v * ((ASA - 128) / 256 + 1) */
{
    return (int16_t)(((int32_t)v * (a + 128)) / 256);
}

bool ak8963_read(imu_t *imu, mag_raw_t *out)
{
    uint8_t b[7];                                     /* HXL..HZH + ST2 (reading ST2 frees the next sample) */
    if (imu->read(imu->address, AK_HXL, b, 7) != 0) return false;
    int16_t x = trim((int16_t)(b[1] << 8 | b[0]), asa[0]);
    int16_t y = trim((int16_t)(b[3] << 8 | b[2]), asa[1]);
    int16_t z = trim((int16_t)(b[5] << 8 | b[4]), asa[2]);
    /* signs as in stm32-simple-imu-reading (checked on hardware) */
    out->x =  y;
    out->y = -x;
    out->z =  z;
    return true;
}

10Other IMU · ICM-20948 (+ AK09916 magnetometer): icm20948.h

Core/Inc/icm20948.h · the whole file (instead of ism330dhcx.h)
Show the code
/* ICM-20948 (TDK): 9-axis = 6-axis IMU + AK09916 magnetometer in one package.
 * Successor of the MPU-9250. Its registers are split into 4 "banks".
 * We use bypass mode (like the MPU-9250), so the AK09916 sits on our I2C bus at 0x0C. */
/* Datasheet: https://invensense.tdk.com/?p=6969 */
#ifndef ICM20948_H
#define ICM20948_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */
#ifdef __cplusplus
extern "C" {
#endif

#define ICM20948_ADDR      0x69                 /* Adafruit + SparkFun boards (AD0 high) */
#define ICM20948_ADDR_ALT  0x68

/* ---- output data rate: 1125 Hz / (1 + divider) ---- */
#define ICM20948_ODR_56HZ   19
#define ICM20948_ODR_112HZ  9
#define ICM20948_ODR_225HZ  4
#define ICM20948_ODR_562HZ  1
/* ---- accelerometer range ---- */
#define ICM20948_ACC_2G      0x0    /* 16384 counts per g */
#define ICM20948_ACC_4G      0x1    /*  8192 counts per g */
#define ICM20948_ACC_8G      0x2    /*  4096 counts per g */
#define ICM20948_ACC_16G     0x3    /*  2048 counts per g */
/* ---- gyroscope range ---- */
#define ICM20948_GYR_250DPS  0x0    /* 131  counts per dps */
#define ICM20948_GYR_500DPS  0x1    /* 65.5 counts per dps */
#define ICM20948_GYR_1000DPS 0x2    /* 32.8 counts per dps */
#define ICM20948_GYR_2000DPS 0x3    /* 16.4 counts per dps */

/* ---- our choice: change these three lines ---- */
#define ICM20948_ODR         ICM20948_ODR_225HZ
#define ICM20948_ACC_RANGE   ICM20948_ACC_4G
#define ICM20948_GYR_RANGE   ICM20948_GYR_500DPS

#define AK09916_ADDR       0x0C                 /* visible only after icm20948_init() */
#define AK09916_MAG_SCALE  0.15f                /* uT per count */

bool icm20948_init(imu_t *imu);
bool icm20948_read(imu_t *imu, imu_raw_t *out);

bool ak09916_init(imu_t *imu);               /* call icm20948_init() first */
bool ak09916_read(imu_t *imu, mag_raw_t *out);     /* signs as in stm32-simple-imu-reading */

#ifdef __cplusplus
}
#endif

#endif /* ICM20948_H */

11Other IMU · ICM-20948 (+ AK09916 magnetometer): icm20948.c

Core/Src/icm20948.c · the whole file (instead of ism330dhcx.c)
Show the code
#include "icm20948.h"

/* Bank 0 */
#define WHO_AM_I       0x00   /* always 0xEA */
#define USER_CTRL      0x03   /* bit 5 = internal I2C master (off for bypass) */
#define PWR_MGMT_1     0x06   /* bit 7 = reset, bit 6 = sleep */
#define PWR_MGMT_2     0x07   /* 0 = all axes on */
#define INT_PIN_CFG    0x0F   /* bit 1 = BYPASS_EN */
#define ACCEL_XOUT_H   0x2D   /* 12 bytes: accel (6), gyro (6). High byte first. */
#define REG_BANK_SEL   0x7F   /* bank number in bits [5:4]; exists in every bank */
/* Bank 2 */
#define GYRO_SMPLRT_DIV  0x00 /* rate = 1125 Hz / (1 + value) */
#define GYRO_CONFIG_1    0x01 /* low-pass [5:3], range [2:1], low-pass on [0] */
#define ACCEL_SMPLRT_DIV 0x11 /* low byte of the accel divider */
#define ACCEL_CONFIG     0x14 /* low-pass [5:3], range [2:1], low-pass on [0] */

static bool bank(imu_t *imu, uint8_t n) { return imu->write(imu->address, REG_BANK_SEL, (uint8_t)(n << 4)) == 0; }

static bool answers(imu_t *imu)
{
    uint8_t id = 0;
    imu->write(imu->address, REG_BANK_SEL, 0x00);   /* make sure we are in bank 0 */
    return imu->read(imu->address, WHO_AM_I, &id, 1) == 0 && id == 0xEA;
}

bool icm20948_init(imu_t *imu)
{
    if (!answers(imu)) return false;

    if (imu->write(imu->address, PWR_MGMT_1, 0x80) != 0) return false;   /* reset (bank goes back to 0) */
    imu->delay(100);
    if (imu->write(imu->address, PWR_MGMT_1, 0x01) != 0) return false;   /* wake up, best clock */
    if (imu->write(imu->address, PWR_MGMT_2, 0x00) != 0) return false;   /* all axes on */
    imu->delay(10);

    if (!bank(imu, 2)) return false;
    if (imu->write(imu->address, GYRO_SMPLRT_DIV,  ICM20948_ODR) != 0) return false;
    if (imu->write(imu->address, GYRO_CONFIG_1,    0x19 | (ICM20948_GYR_RANGE << 1)) != 0) return false; /* low-pass on + range */
    if (imu->write(imu->address, ACCEL_SMPLRT_DIV, ICM20948_ODR) != 0) return false;
    if (imu->write(imu->address, ACCEL_CONFIG,     0x19 | (ICM20948_ACC_RANGE << 1)) != 0) return false; /* low-pass on + range */
    if (!bank(imu, 0)) return false;                                 /* stay in bank 0 for reading */

    if (imu->write(imu->address, USER_CTRL,   0x00) != 0) return false;      /* internal I2C master off */
    if (imu->write(imu->address, INT_PIN_CFG, 0x02) != 0) return false;      /* bypass on: AK09916 at 0x0C */
    imu->delay(10);
    return true;
}

bool icm20948_read(imu_t *imu, imu_raw_t *out)
{
    uint8_t b[12];
    if (imu->read(imu->address, ACCEL_XOUT_H, b, 12) != 0) return false;
    out->ax = (int16_t)(b[0] << 8 | b[1]);
    out->ay = (int16_t)(b[2] << 8 | b[3]);
    out->az = (int16_t)(b[4] << 8 | b[5]);
    out->gx = (int16_t)(b[6] << 8 | b[7]);
    out->gy = (int16_t)(b[8] << 8 | b[9]);
    out->gz = (int16_t)(b[10] << 8 | b[11]);
    return true;
}

/* ---- AK09916 magnetometer (inside the ICM-20948) ---- */
#define AK_WIA2   0x01   /* always 0x09 */
#define AK_HXL    0x11   /* 6 bytes, LOW byte first; then 0x17 (dummy) and ST2 at 0x18 */
#define AK_CNTL2  0x31   /* mode: 0x08 = continuous 100 Hz */
#define AK_CNTL3  0x32   /* bit 0 = reset */

bool ak09916_init(imu_t *imu)
{
    uint8_t id = 0;
    if (imu->read(imu->address, AK_WIA2, &id, 1) != 0 || id != 0x09) return false;
    if (imu->write(imu->address, AK_CNTL3, 0x01) != 0) return false;   /* reset */
    imu->delay(10);
    if (imu->write(imu->address, AK_CNTL2, 0x08) != 0) return false;   /* continuous 100 Hz */
    imu->delay(10);
    return true;
}

bool ak09916_read(imu_t *imu, mag_raw_t *out)
{
    uint8_t b[8];                                     /* HXL..HZH, dummy, ST2 (frees the next sample) */
    if (imu->read(imu->address, AK_HXL, b, 8) != 0) return false;
    /* no sign change, as in stm32-simple-imu-reading (checked on hardware) */
    out->x = (int16_t)(b[1] << 8 | b[0]);
    out->y = (int16_t)(b[3] << 8 | b[2]);
    out->z = (int16_t)(b[5] << 8 | b[4]);
    return true;
}

12Other IMU · ICM-42688-P: icm42688.h

Core/Inc/icm42688.h · the whole file (instead of ism330dhcx.h)
Show the code
/* ICM-42688-P (TDK): modern low-noise 6-axis IMU, common on drone flight controllers. */
/* Datasheet: https://invensense.tdk.com/?p=9240 */
#ifndef ICM42688_H
#define ICM42688_H

#include <stdint.h>
#include <stdbool.h>

/* ---- the link to your board (the same block is in every driver header) ---- */
#ifndef IMU_BOARD_TYPES
#define IMU_BOARD_TYPES   /* defined only once, even if you use two drivers */

/* the three functions your board provides (return 0 = OK) */
typedef int  (*imu_read_fn) (uint8_t address, uint8_t reg, uint8_t *data, uint16_t length);
typedef int  (*imu_write_fn)(uint8_t address, uint8_t reg, uint8_t value);
typedef void (*imu_delay_fn)(uint32_t ms);

/* one sensor: its I2C address + the three board functions */
typedef struct {
  uint8_t      address;   /* 7-bit I2C address */
  imu_read_fn  read;
  imu_write_fn write;
  imu_delay_fn delay;
} imu_t;

/* raw numbers from the sensor, in the chip's own axes */
typedef struct { int16_t ax, ay, az, gx, gy, gz; } imu_raw_t;
typedef struct { int16_t x, y, z; } mag_raw_t;

#endif /* IMU_BOARD_TYPES */
#ifdef __cplusplus
extern "C" {
#endif

#define ICM42688_ADDR      0x68                 /* AP_AD0 to GND */
#define ICM42688_ADDR_ALT  0x69                 /* AP_AD0 to 3V3 */

/* ---- output data rate (bits 3:0) ---- */
#define ICM42688_ODR_100HZ  0x8
#define ICM42688_ODR_200HZ  0x7
#define ICM42688_ODR_1KHZ   0x6
/* ---- accelerometer range ---- */
/* careful: on this chip a bigger code = a SMALLER range */
#define ICM42688_ACC_2G      0x3   /* 16384 counts per g */
#define ICM42688_ACC_4G      0x2
#define ICM42688_ACC_8G      0x1
#define ICM42688_ACC_16G     0x0
/* ---- gyroscope range ---- */
#define ICM42688_GYR_250DPS  0x3  /* 131  counts per dps */
#define ICM42688_GYR_500DPS  0x2  /* 65.5 counts per dps */
#define ICM42688_GYR_1000DPS 0x1
#define ICM42688_GYR_2000DPS 0x0

/* ---- our choice: change these three lines ---- */
#define ICM42688_ODR         ICM42688_ODR_200HZ
#define ICM42688_ACC_RANGE   ICM42688_ACC_4G
#define ICM42688_GYR_RANGE   ICM42688_GYR_500DPS

bool icm42688_init(imu_t *imu);
bool icm42688_read(imu_t *imu, imu_raw_t *out);

#ifdef __cplusplus
}
#endif

#endif /* ICM42688_H */

13Other IMU · ICM-42688-P: icm42688.c

Core/Src/icm42688.c · the whole file (instead of ism330dhcx.c)
Show the code
#include "icm42688.h"

#define DEVICE_CONFIG 0x11   /* bit 0 = software reset */
#define ACCEL_DATA_X1 0x1F   /* 12 bytes: accel (6) then gyro (6). High byte first. */
#define PWR_MGMT0     0x4E   /* sensors are OFF after reset */
#define GYRO_CONFIG0  0x4F   /* range [7:5], data rate [3:0] */
#define ACCEL_CONFIG0 0x50   /* range [7:5], data rate [3:0] */
#define WHO_AM_I      0x75
#define WHO_AM_I_VAL  0x47

static bool answers(imu_t *imu)
{
    uint8_t id = 0;
    return imu->read(imu->address, WHO_AM_I, &id, 1) == 0 && id == WHO_AM_I_VAL;
}

bool icm42688_init(imu_t *imu)
{
    if (!answers(imu)) return false;

    if (imu->write(imu->address, DEVICE_CONFIG, 0x01) != 0) return false;  /* reset */
    imu->delay(2);
    if (imu->write(imu->address, GYRO_CONFIG0,  (ICM42688_GYR_RANGE << 5) | ICM42688_ODR) != 0) return false;
    if (imu->write(imu->address, ACCEL_CONFIG0, (ICM42688_ACC_RANGE << 5) | ICM42688_ODR) != 0) return false;
    if (imu->write(imu->address, PWR_MGMT0,     0x0F) != 0) return false;  /* gyro + accel on, low-noise mode */
    imu->delay(50);                                             /* gyro start-up time */
    return true;
}

bool icm42688_read(imu_t *imu, imu_raw_t *out)
{
    uint8_t b[12];
    if (imu->read(imu->address, ACCEL_DATA_X1, b, 12) != 0) return false;
    out->ax = (int16_t)(b[0] << 8 | b[1]);
    out->ay = (int16_t)(b[2] << 8 | b[3]);
    out->az = (int16_t)(b[4] << 8 | b[5]);
    out->gx = (int16_t)(b[6] << 8 | b[7]);
    out->gy = (int16_t)(b[8] << 8 | b[9]);
    out->gz = (int16_t)(b[10] << 8 | b[11]);
    return true;
}

14Reference: what you call

  1. ism330dhcx_init(&imu) → true = OK. Checks WHO_AM_I, resets, sets rate and range. Called once: 2.6 (USER CODE BEGIN 2), 2.6b (setup()).
  2. ism330dhcx_read(&imu, &raw) → true = OK. Fills raw.ax … raw.gz (imu_raw_t). Called every tick.
  3. You change: ISM330DHCX_ODR, ISM330DHCX_ACC_RANGE, ISM330DHCX_GYR_RANGE in the header; ISM330DHCX_ADDR if your board uses 0x6B.
  4. Other IMU: the same two calls with its prefix, e.g. mpu6050_init / mpu6050_read.
Check & troubleshoot

What you should see

  • The project builds with 0 errors after adding the two files

If it doesn't work

  • Wrong sensor files: use the two files of your own IMU (they are hidden under 2.5, one pair per sensor)
  • Header in the wrong folder: .h goes into Core/Inc, .c into Core/Src

Good to know

  • imu_t holds the sensor address and three function pointers: read, write and delay. The board fills it in (lesson 2.6)
  • The three function types say what each board function looks like. Each returns 0 when it worked
  • Burst read: start at register OUTX_L_G (hex 22) and read 12 bytes: gyroscope x, y, z, then accelerometer x, y, z. Each value is two bytes, low byte first
  • Other IMUs: open only the two files for your sensor

Next: In lesson 2.6 we give the driver our three board functions and read live data.