Added exmaple code of lsm6do to quickly test imu

software/lsm6dso_example/CMakeLists.txt

@@ -0,0 +1,22 @@
+# The following lines of boilerplate have to be in your project's CMakeLists
+# in this exact order for cmake to work correctly
+cmake_minimum_required(VERSION 3.20)
+
+set(ENV{IDF_COMPONENT_MANAGER} "0")
+include($ENV{IDF_PATH}/tools/cmake/project.cmake)
+
+# add the component directories that we want to use
+set(EXTRA_COMPONENT_DIRS
+  "components/"
+)
+
+set(
+  COMPONENTS
+  "main esptool_py i2c lsm6dso filters"
+  CACHE STRING
+  "List of components to include"
+  )
+
+project(lsm6dso_example)
+
+set(CMAKE_CXX_STANDARD 20)

software/lsm6dso_example/README.md

@@ -0,0 +1,50 @@
+# LSM6DSO Example
+
+This example demonstrates how to use the espp LSM6DSO 6-axis IMU driver with the
+ESP-IDF. The example is modeled after the ICM42607 example and shows how to
+configure the IMU, read accelerometer and gyroscope data, and use orientation
+filtering (e.g., Madgwick filter).
+
+![CleanShot 2025-06-18 at 14 16 01](https://github.com/user-attachments/assets/10b15539-688b-4711-b1ce-e7bb33f7e343)
+
+## Features
+- I2C communication with the LSM6DSO
+- Configurable accelerometer and gyroscope range and output data rate
+- Periodic reading of accelerometer, gyroscope, and temperature data
+- Orientation filtering using Madgwick filter
+
+## Usage
+- Configure the I2C pins and address in `sdkconfig` or via Kconfig options
+- Build and flash the example to your ESP32/ESP-IDF target
+- The example will print IMU data and orientation to the serial console
+
+### Build and Flash
+
+Build the project and flash it to the board, then run monitor tool to view
+serial output:
+
+```
+idf.py -p PORT flash monitor
+```
+
+(Replace PORT with the name of the serial port to use.)
+
+(To exit the serial monitor, type ``Ctrl-]``.)
+
+See the Getting Started Guide for full steps to configure and use ESP-IDF to build projects.
+
+## Example Output
+
+![CleanShot 2025-06-18 at 14 15 13](https://github.com/user-attachments/assets/26b07fa0-4e4a-4025-a5a7-135322da8d3b)
+![CleanShot 2025-06-18 at 14 16 01](https://github.com/user-attachments/assets/10b15539-688b-4711-b1ce-e7bb33f7e343)
+
+## Example Code
+See `main/lsm6dso_example.cpp` for the full example source code.
+
+## Configuration
+- Default I2C address: 0x6A (can be changed in Kconfig or via config struct)
+- Example I2C pins: SDA = 21, SCL = 22
+
+## Documentation
+See the [documentation](https://esp-cpp.github.io/espp/imu/lsm6dso.html) for
+full API details.

software/lsm6dso_example/main/CMakeLists.txt

@@ -0,0 +1,2 @@
+idf_component_register(SRC_DIRS "."
+                       INCLUDE_DIRS ".")

software/lsm6dso_example/main/Kconfig.projbuild

@@ -0,0 +1,46 @@
+menu "Example Configuration"
+
+choice EXAMPLE_HARDWARE
+  prompt "Hardware"
+  default EXAMPLE_HARDWARE_QTPYPICO
+  help
+    Select the hardware to run this example on.
+
+config EXAMPLE_HARDWARE_QTPYPICO
+  depends on IDF_TARGET_ESP32
+  bool "Qt Py PICO"
+
+config EXAMPLE_HARDWARE_QTPYS3
+  depends on IDF_TARGET_ESP32S3
+  bool "Qt Py S3"
+
+config EXAMPLE_HARDWARE_CUSTOM
+  bool "Custom"
+endchoice
+
+config EXAMPLE_I2C_SCL_GPIO
+  int "SCL GPIO Num"
+  range 0 50
+  default 19 if EXAMPLE_HARDWARE_QTPYPICO
+  default 40 if EXAMPLE_HARDWARE_QTPYS3
+  default 19 if EXAMPLE_HARDWARE_CUSTOM
+  help
+    GPIO number for I2C Master clock line.
+
+config EXAMPLE_I2C_SDA_GPIO
+  int "SDA GPIO Num"
+  range 0 50
+  default 22 if EXAMPLE_HARDWARE_QTPYPICO
+  default 41 if EXAMPLE_HARDWARE_QTPYS3
+  default 22 if EXAMPLE_HARDWARE_CUSTOM
+  help
+    GPIO number for I2C Master data line.
+
+config EXAMPLE_I2C_CLOCK_SPEED_HZ
+  int "I2C Clock Speed"
+  range 100 1000000
+  default 400000
+  help
+    I2C clock speed in Hz.
+
+endmenu

software/lsm6dso_example/main/lsm6dso_example.cpp

@@ -0,0 +1,199 @@
+#include <chrono>
+#include <cmath>
+#include <vector>
+
+#include "i2c.hpp"
+#include "kalman_filter.hpp"
+#include "logger.hpp"
+#include "lsm6dso.hpp"
+#include "madgwick_filter.hpp"
+
+using namespace std::chrono_literals;
+
+extern "C" void app_main(void) {
+  espp::Logger logger({.tag = "LSM6DSO Example", .level = espp::Logger::Verbosity::INFO});
+  logger.info("Starting LSM6DSO example!");
+
+  //! [lsm6dso 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);
+  }
+  //! [lsm6dso example]
+}

software/lsm6dso_example/sdkconfig.defaults

@@ -0,0 +1,20 @@
+CONFIG_FREERTOS_HZ=1000
+
+# set compiler optimization level to -O2 (compile for performance)
+CONFIG_COMPILER_OPTIMIZATION_PERF=y
+
+# ESP32-specific
+#
+CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ_240=y
+CONFIG_ESP_DEFAULT_CPU_FREQ_MHZ=240
+
+# Common ESP-related
+#
+CONFIG_ESP_SYSTEM_EVENT_TASK_STACK_SIZE=4096
+CONFIG_ESP_MAIN_TASK_STACK_SIZE=16384
+
+# Set esp-timer task stack size to 6KB
+CONFIG_ESP_TIMER_TASK_STACK_SIZE=6144
+
+# set the functions into IRAM
+CONFIG_SPI_MASTER_IN_IRAM=y