LSM6DSO 6-Axis IMU
The Lsm6dso component provides a driver for the LSM6DSO 6-Axis Inertial Measurement Unit (IMU) from STMicroelectronics.
API Reference
Header File
Classes
-
template<lsm6dso::Interface Interface = lsm6dso::Interface::I2C>
class Lsm6dso : public espp::BasePeripheral<uint8_t, lsm6dso::Interface::I2C == lsm6dso::Interface::I2C> Class for the LSM6DSO 6-axis IMU (Accel + Gyro)
The LSM6DSO is a 6-axis IMU with a 3-axis accelerometer and 3-axis gyroscope. It supports both I2C and SPI, and on-chip filtering.
For more information, see the LSM6DSO datasheet: https://www.st.com/resource/en/datasheet/lsm6dso.pdf
Example
using Imu = espp::Lsm6dso<espp::lsm6dso::Interface::I2C>; // I2C config (customize as needed) static constexpr auto i2c_port = I2C_NUM_0; static constexpr auto i2c_clock_speed = CONFIG_EXAMPLE_I2C_CLOCK_SPEED_HZ; // Set in sdkconfig static constexpr gpio_num_t i2c_sda = (gpio_num_t)CONFIG_EXAMPLE_I2C_SDA_GPIO; // Set in sdkconfig static constexpr gpio_num_t i2c_scl = (gpio_num_t)CONFIG_EXAMPLE_I2C_SCL_GPIO; // Set in sdkconfig 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); 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; }; // Madgwick filter for 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 { 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); Imu::Value orientation{}; orientation.roll = espp::deg_to_rad(roll); orientation.pitch = espp::deg_to_rad(pitch); orientation.yaw = espp::deg_to_rad(yaw); return orientation; }; // IMU config Imu::Config config{ .device_address = Imu::DEFAULT_I2C_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 = { .accel_range = Imu::AccelRange::RANGE_2G, .accel_odr = Imu::AccelODR::ODR_416_HZ, .gyro_range = Imu::GyroRange::DPS_2000, .gyro_odr = Imu::GyroODR::ODR_416_HZ, }, .orientation_filter = kalman_filter_fn, .auto_init = true, .log_level = espp::Logger::Verbosity::INFO, }; logger.info("Creating LSM6DSO IMU"); Imu imu(config); std::error_code ec; // set the accel / gyro on-chip filters static constexpr uint8_t accel_filter_bandwidth = 0b001; // ODR / 10 static constexpr uint8_t gyro_lpf_bandwidth = 0b001; // ODR / 3 static constexpr bool gyro_hpf_enabled = false; // disable high-pass filter on gyro static constexpr auto gyro_hpf_bandwidth = Imu::GyroHPF::HPF_0_26_HZ; // 0.26Hz if (!imu.set_accelerometer_filter(accel_filter_bandwidth, Imu::AccelFilter::LOWPASS, ec)) { logger.error("Failed to set accelerometer filter: {}", ec.message()); } // set the gyroscope filter to have lowpass if (!imu.set_gyroscope_filter(gyro_lpf_bandwidth, gyro_hpf_enabled, gyro_hpf_bandwidth, ec)) { logger.error("Failed to set gyroscope filter: {}", ec.message()); } // 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 { static auto start = std::chrono::steady_clock::now(); 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(); [[maybe_unused]] auto t2 = esp_timer_get_time(); // time in microseconds 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 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); // 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); // fmt::print("IMU update took {:.3f} ms\n", (t2 - t0) / 1000.0f); // sleep first in case we don't get IMU data and need to exit early { std::unique_lock<std::mutex> lock(m); cv.wait_until(lock, start + 10ms); } 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), Madgwick 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); }
Usage:
Construct with a Config struct
Call init() to initialize
Use update() to read and filter data
Use get_accelerometer(), get_gyroscope(), get_temperature() to access data
Use get_orientation(), get_gravity_vector() for orientation and gravity vector if filter is set
Note
This class is intended for use with the LSM6DSO and compatible variants.
- Template Parameters:
Interface – The interface type (I2C or SPI)
Public Types
-
using AccelRange = lsm6dso::AccelRange
Alias for accelerometer range.
-
using AccelODR = lsm6dso::AccelODR
Alias for accelerometer output data rate (ODR)
-
using AccelFilter = lsm6dso::AccelFilter
Alias for accelerometer filter type.
-
using GyroRange = lsm6dso::GyroRange
Alias for gyroscope range.
-
using GyroODR = lsm6dso::GyroODR
Alias for gyroscope output data rate (ODR)
-
using GyroHPF = lsm6dso::GyroHPF
Alias for gyroscope high-pass filter (HPF)
-
using RawValue = lsm6dso::RawValue
Alias for raw sensor values.
-
using Value = lsm6dso::Value
Alias for processed sensor values.
-
using ImuConfig = lsm6dso::ImuConfig
Alias for IMU configuration.
-
typedef std::function<Value(float, const Value&, const Value&)> filter_fn
Filter function for orientation filtering
- 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
-
bool init(std::error_code &ec)
Initialize the LSM6DSO
- Parameters:
ec – The error code to set if an error occurs
- Returns:
True if initialized successfully, false otherwise
-
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
-
bool set_config(const ImuConfig &config, std::error_code &ec)
Set the IMU configuration
- Parameters:
config – The configuration
ec – The error code to set if an error occurs
- Returns:
True if the configuration was set successfully, false otherwise
-
bool set_accelerometer_range(AccelRange range, std::error_code &ec)
Set the accelerometer range
- Parameters:
range – The range of the accelerometer
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_gyroscope_range(GyroRange range, std::error_code &ec)
Set the gyroscope range
- Parameters:
range – The range of the gyroscope
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_accelerometer_odr(AccelODR odr, std::error_code &ec)
Set the accelerometer output data rate (ODR)
- Parameters:
odr – The output data rate
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_gyroscope_odr(GyroODR odr, std::error_code &ec)
Set the gyroscope output data rate (ODR)
- Parameters:
odr – The output data rate
ec – The error code to set if an error occurs
- Returns:
True if successful
-
inline float get_accelerometer_sensitivity() const
Get the accelerometer sensitivity in g/LSB
- Returns:
The accelerometer sensitivity
-
inline float get_gyroscope_sensitivity() const
Get the gyroscope sensitivity in dps/LSB
- Returns:
The gyroscope sensitivity
-
float read_accelerometer_sensitivity(std::error_code &ec)
Read the accelerometer sensitivity in g/LSB
- Parameters:
ec – The error code to set if an error occurs
- Returns:
The accelerometer sensitivity
-
float read_gyroscope_sensitivity(std::error_code &ec)
Read the gyroscope sensitivity in dps/LSB
- Parameters:
ec – The error code to set if an error occurs
- Returns:
The gyroscope sensitivity
-
bool set_accelerometer_power_mode(bool enable, std::error_code &ec)
Set the accelerometer power mode
- Parameters:
enable – True to enable, false to power down
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_gyroscope_power_mode(bool enable, std::error_code &ec)
Set the gyroscope power mode
- Parameters:
enable – True to enable, false to power down
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_accelerometer_filter(uint8_t bw, AccelFilter accel_filter, std::error_code &ec)
Set the accelerometer filter bandwidth
- Parameters:
bw – The filter bandwidth
accel_filter – The accelerometer filter type
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool set_gyroscope_filter(uint8_t lpf1_bw, bool hpf_enabled, GyroHPF hpf_bw, std::error_code &ec)
Set the gyroscope filter bandwidth
- Parameters:
lpf1_bw – The low-pass filter 1 bandwidth. Bandwidth differs depending on ODR setting.
hpf_enabled – True to enable high-pass filter, false to disable
hpf_bw – The high-pass filter bandwidth
ec – The error code to set if an error occurs
- Returns:
True if successful
-
bool update(float dt, std::error_code &ec)
Update the sensor values (read accel, gyro, temp, run filter)
- Parameters:
dt – The time step in seconds
ec – The error code to set if an error occurs
- Returns:
True if successful
-
Value read_accelerometer(std::error_code &ec)
Read the accelerometer values (g)
- Parameters:
ec – The error code to set if an error occurs
- Returns:
The accelerometer values
-
Value read_gyroscope(std::error_code &ec)
Read the gyroscope values (dps)
- Parameters:
ec – The error code to set if an error occurs
- Returns:
The gyroscope values
-
float get_temperature() const
Get the temperature (deg C)
- Returns:
The temperature
-
float read_temperature(std::error_code &ec)
Read the temperature (deg C)
- Parameters:
ec – The error code to set if an error occurs
- Returns:
The temperature
-
inline bool probe(std::error_code &ec) const
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 Config &config() const
Get the configuration for the peripheral
- Returns:
The configuration for the peripheral
-
inline uint8_t address() const
Get the address of the peripheral
- Returns:
The address of 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
See also
See also
- Returns:
The verbosity level of the logger
-
inline void set_log_level(espp::Logger::Verbosity level)
Set the log level for the logger
See also
See also
- 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
See also
See also
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
See also
See also
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
See also
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_I2C_ADDRESS = 0x6A
Default I2C address (can be 0x6B)
-
struct Config
Configuration struct for the LSM6DSO.
Public Members
-
uint8_t device_address = DEFAULT_I2C_ADDRESS
I2C address (if I2C)
-
BasePeripheral<uint8_t, Interface == lsm6dso::Interface::I2C>::write_fn write = nullptr
Write function.
-
BasePeripheral<uint8_t, Interface == lsm6dso::Interface::I2C>::read_fn read = nullptr
Read function.
-
bool auto_init = {true}
Automatically initialize on construction.
-
uint8_t device_address = DEFAULT_I2C_ADDRESS
Header File
Unions
- espp::lsm6dso::RawValue.__unnamed81__
Public Members
- struct espp::lsm6dso::RawValue
-
uint8_t raw[6]
Raw byte array for direct access.
-
uint16_t values[3]
Array for direct access to raw values.
- espp::lsm6dso::Value.__unnamed85__
Public Members
- struct espp::lsm6dso::Value
- struct espp::lsm6dso::Value
-
float values[3]