ICM42607 6-Axis IMU

The Icm42607 component provides a driver for the ICM42607 and ICM42670 6-Axis Inertial Measurement Units (IMUs).

ICM42607 Example
- How to use example
  - Hardware Required
  - Build and Flash
- Example Output

API Reference

Header File

components/icm42607/include/icm42607.hpp

Classes

template<icm42607::Interface Interface = icm42607::Interface::I2C> class Icm42607 : public espp::BasePeripheral<uint8_t, icm42607::Interface::I2C == icm42607::Interface::I2C>

Class for the ICM42607 6-axis motion sensor.

The ICM42607 has a 3-axis gyroscope and a 3-axis accelerometer. The ICM42607 has a 16-bit ADC for the gyroscope and a 16-bit ADC for the accelerometer. The gyroscope has a full-scale range of up to ±2000°/s and the accelerometer has a full-scale range of up to ±16g.

The ICM42607 has a FIFO buffer that can store up to 2.25 kilobytes of data. The FIFO buffer can be configured to store gyroscope data, accelerometer data, or both. The FIFO buffer can be configured to store data in either FIFO mode or stream mode. In FIFO mode, the FIFO buffer stores data until the buffer is full, at which point the FIFO buffer stops storing data. In stream mode, the FIFO buffer stores data continuously, overwriting the oldest data when the buffer is full.

The ICM42607 has a Digital Motion Processor (DMP) that can be used to process the data from the gyroscope and accelerometer. The DMP can be used to calculate the orientation of the ICM42607, the linear acceleration of the ICM42607, and the angular velocity of the ICM42607. The DMP can also be used to detect motion events, such as tap events, shake events, and orientation events.

The ICM42607 has a built-in temperature sensor that can be used to measure the temperature of the ICM42607. The temperature sensor has a resolution of 0.5°C and an accuracy of ±2°C.

It supports low-noise 6-axis measurement with a current consumption at only 0.55 mA and a sleep mode current consumption of only 3.5 µA.

For more information see, the datasheet: https://invensense.tdk.com/wp-content/uploads/2021/07/ds-000451_icm-42670-p-datasheet.pdf

Example

  using Imu = espp::Icm42607<espp::icm42607::Interface::I2C>;

  // make the i2c we'll use to communicate
  static constexpr auto i2c_port = I2C_NUM_0;
  static constexpr auto i2c_clock_speed = CONFIG_EXAMPLE_I2C_CLOCK_SPEED_HZ;
  static constexpr gpio_num_t i2c_sda = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO;
  static constexpr gpio_num_t i2c_scl = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO;
  espp::I2c i2c({.port = i2c_port,
                 .sda_io_num = i2c_sda,
                 .scl_io_num = i2c_scl,
                 .sda_pullup_en = GPIO_PULLUP_ENABLE,
                 .scl_pullup_en = GPIO_PULLUP_ENABLE,
                 .clk_speed = i2c_clock_speed});

  // make the orientation filter to compute orientation from accel + gyro
  static constexpr float angle_noise = 0.001f;
  static constexpr float rate_noise = 0.1f;
  static espp::KalmanFilter<2> kf;
  kf.set_process_noise(rate_noise);
  kf.set_measurement_noise(angle_noise);
  static constexpr float beta = 0.1f; // higher = more accelerometer, lower = more gyro
  static espp::MadgwickFilter f(beta);

  auto kalman_filter_fn = [](float dt, const Imu::Value &accel,
                             const Imu::Value &gyro) -> Imu::Value {
    // Apply Kalman filter
    float accelRoll = atan2(accel.y, accel.z);
    float accelPitch = atan2(-accel.x, sqrt(accel.y * accel.y + accel.z * accel.z));
    kf.predict({espp::deg_to_rad(gyro.x), espp::deg_to_rad(gyro.y)}, dt);
    kf.update({accelRoll, accelPitch});
    float roll, pitch;
    std::tie(roll, pitch) = kf.get_state();
    // return the computed orientation
    Imu::Value orientation{};
    orientation.roll = roll;
    orientation.pitch = pitch;
    orientation.yaw = 0.0f;
    return orientation;
  };

  auto madgwick_filter_fn = [](float dt, const Imu::Value &accel,
                               const Imu::Value &gyro) -> Imu::Value {
    // Apply Madgwick filter
    f.update(dt, accel.x, accel.y, accel.z, espp::deg_to_rad(gyro.x), espp::deg_to_rad(gyro.y),
             espp::deg_to_rad(gyro.z));
    float roll, pitch, yaw;
    f.get_euler(roll, pitch, yaw);
    // return the computed orientation
    Imu::Value orientation{};
    orientation.pitch = espp::deg_to_rad(pitch);
    orientation.roll = espp::deg_to_rad(roll);
    orientation.yaw = espp::deg_to_rad(yaw);
    return orientation;
  };

  // make the IMU config
  Imu::Config config{
      .device_address = Imu::DEFAULT_ADDRESS,
      .write = std::bind(&espp::I2c::write, &i2c, std::placeholders::_1, std::placeholders::_2,
                         std::placeholders::_3),
      .read = std::bind(&espp::I2c::read, &i2c, std::placeholders::_1, std::placeholders::_2,
                        std::placeholders::_3),
      .imu_config =
          {
              .accelerometer_range = Imu::AccelerometerRange::RANGE_2G,
              .accelerometer_odr = Imu::AccelerometerODR::ODR_400_HZ,
              .gyroscope_range = Imu::GyroscopeRange::RANGE_2000DPS,
              .gyroscope_odr = Imu::GyroscopeODR::ODR_400_HZ,
          },
      .orientation_filter = kalman_filter_fn,
      .auto_init = true,
  };

  // create the IMU
  logger.info("Creating IMU");
  Imu imu(config);
  std::error_code ec;

  // turn on DMP
  logger.info("Turning on DMP");
  if (!imu.set_dmp_power_save(false, ec)) {
    logger.error("Failed to set DMP power save mode: {}", ec.message());
    return;
  }

  // initialize the DMP
  logger.info("Initializing DMP");
  if (!imu.dmp_initialize(ec)) {
    logger.error("Failed to initialize DMP: {}", ec.message());
    return;
  }

  // set the DMP output data rate
  logger.info("Setting DMP output data rate (ODR)");
  if (!imu.set_dmp_odr(espp::icm42607::DmpODR::ODR_25_HZ, ec)) {
    logger.error("Failed to set DMP ODR: {}", ec.message());
    return;
  }

  // set filters for the accel / gyro
  logger.info("Setting accel and gyro filters");
  static constexpr auto filter_bw = espp::icm42607::SensorFilterBandwidth::BW_16_HZ;
  if (!imu.set_accelerometer_filter(filter_bw, ec)) {
    logger.error("Failed to set accel filter: {}", ec.message());
    return;
  }
  if (!imu.set_gyroscope_filter(filter_bw, ec)) {
    logger.error("Failed to set gyro filter: {}", ec.message());
    return;
  }

  // make a task to read out the IMU data and print it to console
  espp::Task imu_task({.callback = [&](std::mutex &m, std::condition_variable &cv) -> bool {
                         // sleep first in case we don't get IMU data and need to exit early
                         {
                           std::unique_lock<std::mutex> lock(m);
                           cv.wait_for(lock, 10ms);
                         }

                         auto now = esp_timer_get_time(); // time in microseconds
                         static auto t0 = now;
                         auto t1 = now;
                         float dt = (t1 - t0) / 1'000'000.0f; // convert us to s
                         t0 = t1;

                         std::error_code ec;
                         // update the imu data
                         if (!imu.update(dt, ec)) {
                           return false;
                         }

                         // get accel
                         auto accel = imu.get_accelerometer();
                         auto gyro = imu.get_gyroscope();
                         auto temp = imu.get_temperature();
                         auto orientation = imu.get_orientation();
                         auto gravity_vector = imu.get_gravity_vector();

                         // print time and raw IMU data
                         std::string text = "";
                         text += fmt::format("{:.3f},", now / 1'000'000.0f);
                         text += fmt::format("{:02.3f},{:02.3f},{:02.3f},", (float)accel.x,
                                             (float)accel.y, (float)accel.z);
                         text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gyro.x,
                                             (float)gyro.y, (float)gyro.z);
                         text += fmt::format("{:02.1f},", temp);
                         // print kalman filter outputs
                         text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)orientation.x,
                                             (float)orientation.y, (float)orientation.z);
                         text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", (float)gravity_vector.x,
                                             (float)gravity_vector.y, (float)gravity_vector.z);

                         auto madgwick_orientation = madgwick_filter_fn(dt, accel, gyro);
                         float roll = madgwick_orientation.roll;
                         float pitch = madgwick_orientation.pitch;
                         float yaw = madgwick_orientation.yaw;
                         float vx = sin(pitch);
                         float vy = -cos(pitch) * sin(roll);
                         float vz = -cos(pitch) * cos(roll);

                         // print madgwick filter outputs
                         text += fmt::format("{:03.3f},{:03.3f},{:03.3f},", roll, pitch, yaw);
                         text += fmt::format("{:03.3f},{:03.3f},{:03.3f}", vx, vy, vz);

                         fmt::print("{}\n", text);

                         return false;
                       },
                       .task_config = {
                           .name = "IMU",
                           .stack_size_bytes = 6 * 1024,
                           .priority = 10,
                           .core_id = 0,
                       }});

  // print the header for the IMU data (for plotting)
  fmt::print("% Time (s), "
             // raw IMU data (accel, gyro, temp)
             "Accel X (m/s^2), Accel Y (m/s^2), Accel Z (m/s^2), "
             "Gyro X (rad/s), Gyro Y (rad/s), Gyro Z (rad/s), "
             "Temp (C), "
             // kalman filter outputs
             "Kalman Roll (rad), Kalman Pitch (rad), Kalman Yaw (rad), "
             "Kalman Gravity X, Kalman Gravity Y, Kalman Gravity Z, "
             // madgwick filter outputs
             "Madgwick Roll (rad), Madgwick Pitch (rad), Magwick Yaw (rad), "
             "Madgwick Gravity X, Madgwick Gravity Y, Madgwick Gravity Z\n");

  logger.info("Starting IMU task");
  imu_task.start();

  // loop forever
  while (true) {
    std::this_thread::sleep_for(1s);
  }

Note

The ICM42607 is a 6-axis motion sensor that can be interfaced with either I2C or SPI.

Note

The Icm42607 can work with both the Icm42607 and Icm42670, as they are the same sensor.

Template Parameters: Interface – The interface type of the ICM42607

Public Types

using FifoMode = icm42607::FifoMode: FIFO mode.

using AccelerometerRange = icm42607::AccelerometerRange: Accelerometer range.

using AccelerometerPowerMode = icm42607::AccelerometerPowerMode: Accelerometer power mode.

using AccelerometerODR = icm42607::AccelerometerODR: Accelerometer output data rate.

using GyroscopeRange = icm42607::GyroscopeRange: Gyroscope range.

using GyroscopePowerMode = icm42607::GyroscopePowerMode: Gyroscope power mode.

using GyroscopeODR = icm42607::GyroscopeODR: Gyroscope output data rate.

using DmpODR = icm42607::DmpODR: DMP output data rate.

using TemperatureFilterBandwidth = icm42607::TemperatureFilterBandwidth: Temperature filter bandwidth.

using SensorFilterBandwidth = icm42607::SensorFilterBandwidth: Sensor filter bandwidth.

using ImuConfig = icm42607::ImuConfig: IMU configuration.

using RawValue = icm42607::RawValue: Raw IMU data.

using Value = icm42607::Value: IMU data.

using InterruptDriveMode = icm42607::InterruptDriveMode: Interrupt drive mode.

using InterruptPolarity = icm42607::InterruptPolarity: Interrupt polarity.

using InterruptMode = icm42607::InterruptMode: Interrupt mode.

using InterruptConfig = icm42607::InterruptConfig: Interrupt configuration.

typedef std::function<Value(float, const Value&, const Value&)> filter_fn

Filter function.

Filter function for filtering 6-axis data into 3-axis orientation data

Param dt: The time step in seconds
Param accel: The accelerometer data
Param gyro: The gyroscope data
Return: The filtered orientation data in radians

typedef std::function<bool(uint8_t)> probe_fn

Function to probe the peripheral

Param address: The address to probe
Return: True if the peripheral is found at the given address

Public Functions

inline explicit Icm42607(const Config &config)

Constructor

Parameters: config – The configuration

inline bool init(std::error_code &ec)

Initialize the ICM42607

Parameters: ec – The error code to set if an error occurs
Returns: True if the ICM42607 was initialized successfully, false otherwise

inline uint8_t get_device_id(std::error_code &ec)

Get the device ID

Parameters: ec – The error code to set if an error occurs
Returns: The device ID

inline bool set_config(const ImuConfig &imu_config, std::error_code &ec)

Set the IMU configuration

Parameters

imu_config – The IMU configuration
ec – The error code to set if an error occurs

Returns

True if the configuration was set successfully, false otherwise

inline ImuConfig get_config() const

Get the IMU configuration

Returns: The IMU configuration

inline bool set_accelerometer_power_mode(const AccelerometerPowerMode &power_mode, std::error_code &ec)

Set the Accelerometer power mode

Parameters

power_mode – The power mode
ec – The error code to set if an error occurs

Returns

True if the power mode was set successfully, false otherwise

inline bool set_gyroscope_power_mode(const GyroscopePowerMode &power_mode, std::error_code &ec)

Set the Gyroscope power mode

Parameters

power_mode – The power mode
ec – The error code to set if an error occurs

Returns

True if the power mode was set successfully, false otherwise

inline bool set_accelerometer_filter(const SensorFilterBandwidth &bw, std::error_code &ec)

Set the Accelerometer filter bandwidth

Parameters

bw – The filter bandwidth
ec – The error code to set if an error occurs

Returns

True if the filter bandwidth was set successfully, false otherwise

inline bool set_gyroscope_filter(const SensorFilterBandwidth &bw, std::error_code &ec)

Set the Gyroscope filter bandwidth

Parameters

bw – The filter bandwidth
ec – The error code to set if an error occurs

Returns

True if the filter bandwidth was set successfully, false otherwise

inline bool dmp_initialize(std::error_code &ec)

Initialize the DMP

Parameters: ec – The error code to set if an error occurs
Returns: True if the DMP was initialized successfully, false otherwise

inline bool set_dmp_power_save(bool enable, std::error_code &ec)

Set the DMP power save mode

Parameters

enable – True to enable DMP power save mode, false to disable
ec – The error code to set if an error occurs

Returns

True if the DMP power save mode was set successfully, false otherwise

inline bool set_dmp_odr(const DmpODR &odr, std::error_code &ec)

Set the DMP output data rate

Parameters

odr – The DMP output data rate
ec – The error code to set if an error occurs

Returns

True if the DMP output data rate was set successfully, false otherwise

inline float get_accelerometer_sensitivity()

Get the accelerometer sensitivity from memory

Returns: The accelerometer sensitivity in g/LSB

inline float read_accelerometer_sensitivity(std::error_code &ec)

Read the accelerometer sensitivity

Parameters: ec – The error code to set if an error occurs
Returns: The accelerometer sensitivity in g/LSB

inline float get_gyroscope_sensitivity()

Get the gyroscope sensitivity from memory

Returns: The gyroscope sensitivity in °/s/LSB

inline float read_gyroscope_sensitivity(std::error_code &ec)

Read the gyroscope sensitivity

Parameters: ec – The error code to set if an error occurs
Returns: The gyroscope sensitivity in °/s/LSB

inline bool update(float dt, std::error_code &ec)

Update the accelerometer and gyroscope, and read the temperature

Note

The values can be retrieved with get_accelerometer_values and get_gyroscope_values, and the temperature can be retrieved with get_temperature

Parameters

dt – The time step in seconds
ec – The error code to set if an error occurs

Returns

True if the values were updated successfully, false otherwise

inline Value get_accelerometer() const

Get the accelerometer values

Note

The values are in g and are updated when read_accelerometer is called or update_values is called

Returns: The accelerometer values

inline Value read_accelerometer(std::error_code &ec)

Read the accelerometer data

Parameters: ec – The error code to set if an error occurs
Returns: The accelerometer data

inline Value get_gyroscope() const

Get the gyroscope values

Note

The values are in °/s and are updated when read_gyroscope is called or update_values is called

Returns: The gyroscope values

inline Value read_gyroscope(std::error_code &ec)

Read the gyroscope data

Parameters: ec – The error code to set if an error occurs
Returns: The gyroscope data

inline float get_temperature() const

Get the temperature

Note

The temperature is updated when read_temperature is called or update_values is called

Returns: The temperature in °C

inline float read_temperature(std::error_code &ec)

Read the temperature

Parameters: ec – The error code to set if an error occurs
Returns: The temperature in °C

inline Value get_orientation() const

Get the most recently calculated orientation, if any.

Note

The orientation is updated when update_values is called

Note

The orientation is calculated using the orientation filter function. If no filter function is set, the orientation will not be updated.

Returns: The orientation angles in radians - pitch, roll, yaw

inline Value get_gravity_vector() const

Get the most recently calculated gravity vector, if any.

Note

The gravity vector is updated when update_values is called

Note

The gravity vector is calculated using the orientation filter function. If no filter function is set, the gravity vector will not be calculated.

Returns: The gravity vector

inline bool configure_fifo(const FifoMode &mode, bool bypassed, std::error_code &ec)

Configure the FIFO buffer

Parameters

mode – The FIFO mode
bypassed – True if the FIFO is bypassed, false otherwise
ec – The error code to set if an error occurs

Returns

True if the FIFO buffer was configured successfully, false otherwise

inline bool fifo_flush(std::error_code &ec)

Flush the FIFO buffer

Parameters: ec – The error code to set if an error occurs
Returns: True if the FIFO buffer was flushed successfully, false otherwise

inline uint16_t fifo_count(std::error_code &ec)

Get the FIFO count

See also

set_fifo_count_format

Note

The count will be either the number of bytes (if FIFO_COUNT_FORMAT is 0) or the number of records (if FIFO_COUNT_FORMAT is 1). A record is 16 bytes for header + gyro + accel + temp sensor data + time stamp, OR 8 bytes for header + gyro/accel + temp sensor data.

Parameters: ec – The error code to set if an error occurs
Returns: The FIFO count.

inline bool set_fifo_count_bytes(std::error_code &ec)

Set the FIFO count format to count bytes

See also

fifo_count

Parameters: ec – The error code to set if an error occurs
Returns: True if the FIFO count format was set successfully, false otherwise

inline bool set_fifo_count_records(std::error_code &ec)

Set the FIFO count format to count records

See also

fifo_count

Parameters: ec – The error code to set if an error occurs
Returns: True if the FIFO count format was set successfully, false otherwise

inline std::vector<uint8_t> fifo_data(std::error_code &ec)

Get the FIFO data

Parameters: ec – The error code to set if an error occurs
Returns: The FIFO data

inline bool configure_interrupt_1(const InterruptConfig &config, std::error_code &ec)

Configure interrupt 1

Parameters

config – The interrupt configuration
ec – The error code to set if an error occurs

Returns

True if the interrupt was configured successfully, false otherwise

inline bool configure_interrupt_2(const InterruptConfig &config, std::error_code &ec)

Configure interrupt 2

Parameters

config – The interrupt configuration
ec – The error code to set if an error occurs

Returns

True if the interrupt was configured successfully, false otherwise

inline bool is_data_ready(std::error_code &ec)

Read whether the data is ready

Note

This will clear the data ready interrupt bit, which is automatically set to 1 when a Data Ready interrupt is generated.

Parameters: ec – The error code to set if an error occurs
Returns: True if the data is ready, false otherwise

inline bool probe(std::error_code &ec)

Probe the peripheral

Note

This function is thread safe

Note

If the probe function is not set, this function will return false and set the error code to operation_not_supported

Note

This function is only available if UseAddress is true

Parameters: ec – The error code to set if there is an error
Returns: True if the peripheral is found

inline void set_address(uint8_t address)

Set the address of the peripheral

Note

This function is thread safe

Note

This function is only available if UseAddress is true

Parameters: address – The address of the peripheral

inline void set_probe(const probe_fn &probe)

Set the probe function

Note

This function is thread safe

Note

This should rarely be used, as the probe function is usually set in the constructor. If you need to change the probe function, consider using the set_config function instead.

Note

This function is only available if UseAddress is true

Parameters: probe – The probe function

inline void set_write(const write_fn &write)

Set the write function

Note

This function is thread safe

Note

This should rarely be used, as the write function is usually set in the constructor. If you need to change the write function, consider using the set_config function instead.

Parameters: write – The write function

inline void set_read(const read_fn &read)

Set the read function

Note

This function is thread safe

Note

This should rarely be used, as the read function is usually set in the constructor. If you need to change the read function, consider using the set_config function instead.

Parameters: read – The read function

inline void set_read_register(const read_register_fn &read_register)

Set the read register function

Note

This function is thread safe

Note

This should rarely be used, as the read register function is usually set in the constructor. If you need to change the read register function, consider using the set_config function instead.

Parameters: read_register – The read register function

inline void set_write_then_read(const write_then_read_fn &write_then_read)

Set the write then read function

Note

This function is thread safe

Note

This should rarely be used, as the write then read function is usually set in the constructor. If you need to change the write then

Parameters: write_then_read – The write then read function

inline void set_separate_write_then_read_delay(const std::chrono::milliseconds &delay)

Set the delay between the write and read operations in write_then_read

Note

This function is thread safe

Note

This should rarely be used, as the delay is usually set in the constructor. If you need to change the delay, consider using the set_config function instead.

Note

This delay is only used if the write_then_read function is not set to a custom function and the write and read functions are separate functions.

Parameters: delay – The delay between the write and read operations in write_then_read

inline void set_config(const Config &config)

Set the configuration for the peripheral

Note

This function is thread safe

Note

The configuration should normally be set in the constructor, but this function can be used to change the configuration after the peripheral has been created - for instance if the peripheral could be found on different communications buses.

Parameters: config – The configuration for the peripheral

inline void set_config(Config &&config)

Set the configuration for the peripheral

Note

This function is thread safe

Note

The configuration should normally be set in the constructor, but this function can be used to change the configuration after the peripheral has been created - for instance if the peripheral could be found on different communications buses.