Attitude Estimation· Live
Home / Chapter 2 / 2.6b

ESP32: the same driver, read the data

Lesson 2.6b · ESP32 · free

Video coming soon

The ESP32 now reads the IMU with exactly the same two driver files as the STM32. Only the three board functions change: they use Wire instead of HAL.

In this lesson · 16 steps
  1. New sketch + the driver 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. Include, rate
  15. The three board functions
  16. Start the sensor, read every tick

1New sketch + the driver files

  1. File → New Sketch, save it as ch2_imu
  2. The driver: ism330dhcx.h and ism330dhcx.c, exactly the same files as in 2.5 (they are below, hidden by default: click to open, then Copy)
  3. For each file: click ⋯ (right of the tabs) → New Tab, type the name (ism330dhcx.h, then ism330dhcx.c) and paste the code from the steps below
  4. Another IMU? Add its two files instead: only the include and the _init / _read names change

2ISM330DHCX (our sensor): ism330dhcx.h

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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

ch2_imu · new tab 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;
}

14Include, rate

ch2_imu.ino · top of the file
#include <Wire.h>
#include "ism330dhcx.h"

#define SAMPLE_RATE_HZ 50

const unsigned long SAMPLE_PERIOD_US = 1000000UL / SAMPLE_RATE_HZ;
unsigned long last_sample = 0;

15The three board functions

ch2_imu.ino · after the includes

The only ESP32-specific code: the same three functions as in 2.6, written with Wire.

/* read: start at register reg, read length bytes */
int esp32_read(uint8_t address, uint8_t reg, uint8_t *data, uint16_t length)
{
  Wire.beginTransmission(address);
  Wire.write(reg);
  if (Wire.endTransmission(false) != 0)
    return -1;
  if (Wire.requestFrom((int)address, (int)length) != length)
    return -1;
  for (uint16_t i = 0; i < length; i++)
    data[i] = Wire.read();
  return 0;
}

/* write: one value into register reg */
int esp32_write(uint8_t address, uint8_t reg, uint8_t value)
{
  Wire.beginTransmission(address);
  Wire.write(reg);
  Wire.write(value);
  if (Wire.endTransmission() != 0)
    return -1;
  return 0;
}

void esp32_delay(uint32_t ms)
{
  delay(ms);
}

/* our IMU: its address + the three board functions */
imu_t imu = {
  .address = ISM330DHCX_ADDR,   /* 0x6A */
  .read    = esp32_read,
  .write   = esp32_write,
  .delay   = esp32_delay,
};
Why?
esp32_write: beginTransmission, write(reg), write(value), endTransmission(), returns 0 = OK.
esp32_read: write(reg), then endTransmission(false) (keeps the bus for the read), requestFrom length bytes, returns 0 = OK.
esp32_delay: waits ms (delay).
No address << 1 here: Wire takes the 7-bit address as it is. See lesson 2.4 for the big picture.

16Start the sensor, read every tick

ch2_imu.ino · setup() and loop()

ism330dhcx_init and ism330dhcx_read are exactly the calls from 2.6.

void setup()
{
  Serial.begin(115200);
  while (!Serial)           /* wait until the serial plotter connects */
    delay(10);

  Wire.begin();             /* Nano ESP32: SDA = A4, SCL = A5 */
  Wire.setClock(400000);

  if (ism330dhcx_init(&imu))
    Serial.println("IMU ready");
  else
    Serial.println("IMU not found: check the wires, the address and the sensor type");

  last_sample = micros();
}

void loop()
{
  if (micros() - last_sample >= SAMPLE_PERIOD_US)
  {
    last_sample += SAMPLE_PERIOD_US;

    imu_raw_t raw;
    if (ism330dhcx_read(&imu, &raw))
      Serial.printf("ax:%d,ay:%d,az:%d,gx:%d,gy:%d,gz:%d\n",
                    raw.ax, raw.ay, raw.az, raw.gx, raw.gy, raw.gz);
  }
}
Why?
The loop is the fixed-rate loop from 1.5. while (!Serial) waits until the serial plotter connects, so we never miss the first message (no fixed delay like in 2.3b).
Check & troubleshoot

What you should see

  • IMU ready once, after you connect the plotter
  • Six lines (ax, ay, az, gx, gy, gz) that move when you tilt or turn the board

If it doesn't work

  • Nothing printed: the sketch waits until the plotter connects. Press Connect in our web serial plotter
  • IMU not found: check A4/A5, 3V3 and the address (hex 6A or 6B)

Good to know

  • The driver files are C, the sketch is C++. The driver headers have extern "C" guards, so they link in the Arduino sketch without changes

Next: Chapter 3: we add a second sensor, the magnetometer, so we can find the heading.