BMI270 6-Axis IMU

The Bmi270 component provides an interface to the BMI270 6-axis IMU sensor. It supports both SPI and I2C communication protocols and can be used to read accelerometer and gyroscope data. It also includes features for sensor configuration and data processing.

API Reference

Header File

Classes

template<bmi270::Interface Interface = bmi270::Interface::I2C>
class Bmi270 : public espp::BasePeripheral<uint8_t, bmi270::Interface::I2C == bmi270::Interface::I2C>

Class for the BMI270 6-axis motion sensor.

Key features from the datasheet:

  • 16-bit digital, triaxial accelerometer with ±2g/±4g/±8g/±16g range

  • 16-bit digital, triaxial gyroscope with ±125°/s to ±2000°/s range

  • Output data rates: 0.78Hz to 1.6kHz (accelerometer), 25Hz to 6.4kHz (gyroscope)

  • Low current consumption: typ. 685µA in full operation

  • Built-in power management unit (PMU) for advanced power management

  • 2KB on-chip FIFO buffer

  • Advanced features: step counter, activity recognition, wrist gestures, etc.

  • Temperature sensor with 0.5°C resolution

For more information, see the datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmi270-ds000.pdf

Example

  using Imu = espp::Bmi270<espp::bmi270::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,
                 .timeout_ms = 200, // need to be long enough for writing config file (8kb)
                 .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);
  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;
  };

  static constexpr float beta = 0.1f;
  static espp::MadgwickFilter madgwick(beta);
  auto madgwick_filter_fn = [](float dt, const Imu::Value &accel,
                               const Imu::Value &gyro) -> Imu::Value {
    // Apply Madgwick filter
    madgwick.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;
    madgwick.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;
  };

  // use the i2c to ping both of the possible BMI270 addresses and use the one that responds
  // This is necessary because the BMI270 can be configured to use either address
  // SDO pulled high or low.
  uint8_t bmi270_address = Imu::DEFAULT_ADDRESS;
  std::vector<uint8_t> addresses = {Imu::DEFAULT_ADDRESS, Imu::DEFAULT_ADDRESS_SDO_HIGH};
  for (auto address : addresses) {
    if (i2c.probe_device(address)) {
      logger.info("Found BMI270 at address: 0x{:02X}", address);
      bmi270_address = address;
      break;
    } else {
      logger.warn("No BMI270 found at address: 0x{:02X}", address);
    }
  }

  // make the IMU config
  Imu::Config config{
      .device_address = bmi270_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_4G,
              .accelerometer_odr = Imu::AccelerometerODR::ODR_100_HZ,
              .accelerometer_bandwidth = Imu::AccelerometerBandwidth::NORMAL_AVG4,
              .gyroscope_range = Imu::GyroscopeRange::RANGE_1000DPS,
              .gyroscope_odr = Imu::GyroscopeODR::ODR_100_HZ,
              .gyroscope_bandwidth = Imu::GyroscopeBandwidth::NORMAL_MODE,
              .gyroscope_performance_mode = Imu::GyroscopePerformanceMode::PERFORMANCE_OPTIMIZED,
          },
      .orientation_filter = kalman_filter_fn,
      .auto_init = true,
      .log_level = espp::Logger::Verbosity::INFO,
  };

  // create the IMU
  Imu imu(config);

  // make a task to read out the IMU data and print it to console
  auto task_fn = [&](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;
  };

  espp::Task task({.callback = task_fn,
                   .task_config = {
                       .name = "BMI270",
                       .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 (g), Accel Y (g), Accel Z (g), "
             "Gyro X (°/s), Gyro Y (°/s), Gyro Z (°/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), Madgwick Yaw (rad), "
             "Madgwick Gravity X, Madgwick Gravity Y, Madgwick Gravity Z\n");

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

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

Note

The BMI270 is a highly integrated, low power inertial measurement unit (IMU) that combines precise acceleration and angular rate measurement with intelligent on-chip motion-triggered interrupt features.

Template Parameters:

Interface – The interface type of the BMI270 (I2C or SPI)

Public Types

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

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 Bmi270(const Config &config)

Constructor

Parameters:

config – The configuration

inline bool init(std::error_code &ec)

Initialize the BMI270

Parameters:

ec – The error code to set if an error occurs

Returns:

True if the BMI270 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 soft_reset(std::error_code &ec)

Perform soft reset

Parameters:

ec – The error code to set if an error occurs

Returns:

True if reset was successful, false otherwise

inline bool set_low_power_mode(bool enabled, std::error_code &ec)

Set whether to enable or disable low power mode

Parameters:
  • enabled – True to enable low power mode, false to disable

  • ec – The error code to set if an error occurs

Returns:

True if power mode was set successfully, false otherwise

inline bool enable_temperature_sensor(bool enable, std::error_code &ec)

Enable/disable temperature sensor

Parameters:
  • enable – True to enable, false to disable

  • ec – The error code to set if an error occurs

Returns:

True if successful, false otherwise

inline bool enable_accelerometer(bool enable, std::error_code &ec)

Enable/disable accelerometer

Parameters:
  • enable – True to enable, false to disable

  • ec – The error code to set if an error occurs

Returns:

True if successful, false otherwise

inline bool enable_gyroscope(bool enable, std::error_code &ec)

Enable/disable gyroscope

Parameters:
  • enable – True to enable, false to disable

  • ec – The error code to set if an error occurs

Returns:

True if successful, false otherwise

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 has_new_data(std::error_code &ec)

Check if new data is available

Parameters:

ec – The error code to set if an error occurs

Returns:

True if new data is available, false otherwise

inline bool has_new_data() const

Check if new data is available (cached version)

Returns:

True if new data is available, false otherwise

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

Update all sensor data

Parameters:
  • dt – The time step in seconds

  • ec – The error code to set if an error occurs

Returns:

True if data was updated successfully, false otherwise

inline Value get_accelerometer() const

Get accelerometer data

Returns:

The accelerometer data in g

inline Value read_accelerometer(std::error_code &ec)

Read accelerometer data

Parameters:

ec – The error code to set if an error occurs

Returns:

The accelerometer data in g

inline Value get_gyroscope() const

Get gyroscope data

Returns:

The gyroscope data in °/s

inline Value read_gyroscope(std::error_code &ec)

Read gyroscope data

Parameters:

ec – The error code to set if an error occurs

Returns:

The gyroscope data in °/s

inline float get_temperature() const

Get temperature

Returns:

The temperature in °C

inline float read_temperature(std::error_code &ec)

Read temperature

Parameters:

ec – The error code to set if an error occurs

Returns:

The temperature in °C

inline Value get_orientation() const

Get orientation data

Returns:

The orientation data in radians

inline Value get_gravity_vector() const

Get gravity vector

Returns:

The gravity vector

inline float get_accelerometer_sensitivity() const

Get accelerometer sensitivity

Returns:

The accelerometer sensitivity in LSB/g

inline float get_gyroscope_sensitivity() const

Get gyroscope sensitivity

Returns:

The gyroscope sensitivity in LSB/(°/s)

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

Configure interrupts

Parameters:
  • config – The interrupt configuration

  • ec – The error code to set if an error occurs

Returns:

True if interrupts were configured successfully, 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.

Parameters:

config – The configuration for the peripheral

inline const std::string &get_name() const

Get the name of the component

Note

This is the tag of the logger

Returns:

A const reference to the name of the component

inline void set_log_tag(const std::string_view &tag)

Set the tag for the logger

Parameters:

tag – The tag to use for the logger

inline espp::Logger::Verbosity get_log_level() const

Get the log level for the logger

Returns:

The verbosity level of the logger

inline void set_log_level(espp::Logger::Verbosity level)

Set the log level for the logger

Parameters:

level – The verbosity level to use for the logger

inline void set_log_verbosity(espp::Logger::Verbosity level)

Set the log verbosity for the logger

See also

set_log_level

Note

This is a convenience method that calls set_log_level

Parameters:

level – The verbosity level to use for the logger

inline espp::Logger::Verbosity get_log_verbosity() const

Get the log verbosity for the logger

See also

get_log_level

Note

This is a convenience method that calls get_log_level

Returns:

The verbosity level of the logger

inline void set_log_rate_limit(std::chrono::duration<float> rate_limit)

Set the rate limit for the logger

Note

Only calls to the logger that have _rate_limit suffix will be rate limited

Parameters:

rate_limit – The rate limit to use for the logger

Public Static Attributes

static constexpr uint8_t DEFAULT_ADDRESS = 0x68

Default I2C address with SDO low.

static constexpr uint8_t DEFAULT_ADDRESS_SDO_HIGH = 0x69

I2C address with SDO high.

struct Config

Configuration struct for the BMI270.

Public Members

uint8_t device_address = DEFAULT_ADDRESS

I2C address of the BMI270.

BasePeripheral<uint8_t, Interface == bmi270::Interface::I2C>::write_fn write = nullptr

Write function.

BasePeripheral<uint8_t, Interface == bmi270::Interface::I2C>::read_fn read = nullptr

Read function.

ImuConfig imu_config

IMU configuration.

filter_fn orientation_filter = nullptr

Filter function for orientation.

bool auto_init = {true}

Automatically initialize the BMI270.

Logger::Verbosity log_level = {Logger::Verbosity::WARN}

Log level.

Header File

Unions

espp::bmi270::Value.__unnamed3__

Public Members

struct espp::bmi270::Value
struct espp::bmi270::Value
float values[3]

Array access.