feat(icm42670): Add ICM42607 / ICM42670 IMU component (#392)

* feat(icm42670): Add ICM42607 / ICM42670 IMU component
* Add new component for IMU
* Add IMU to esp-box component and test it in the example

IMUs are nice, and exposing the IMU via the hardware component / bsp (esp-box in this case) is even better. Testing / showcasing the IMU in the bsp example is _even_ better.

Build and run `esp-box/example` on ESP-BOX and ensure the IMU works by looking at the on-screen-display while moving the box around various orientations.

* fleshing out some more

* fix sa for imu

* fleshing out examples and adding filters for computing pitch/roll from 6-axis imu

* fix sa and update icm example some more

* update example to support kconfig for custom hardware

* udpate fast_inv_sqrt and affected examples / code

* fix lib build

* update filter

* WIP updating example to work with madgwick filter as well

* working madgwick filter

* update comment

* fix sa

* only add compile options if not msvc

* readme: update

* update icm example to use madgwick filter as well

* reimplement kalman filter to support n-dimensional state; update examples to use both madgwick and kalman filters and show both (text and line)

* simplify code and update example

* update to use std::tie

* update docs

Author: finger563

.github/workflows/build.yml

@@ -81,6 +81,8 @@ jobs:
           target: esp32s3
         - path: 'components/i2c/example'
           target: esp32
+        - path: 'components/icm42607/example'
+          target: esp32s3
         - path: 'components/interrupt/example'
           target: esp32s3
         - path: 'components/joystick/example'

components/base_peripheral/include/base_peripheral.hpp

@@ -636,6 +636,28 @@ class BasePeripheral : public BaseComponent {
     write_u8_to_register(register_address, data, ec);
   }
 
+  /// Set bits in a register on the peripheral by mask
+  /// \param register_address The address of the register to modify
+  /// \param mask The mask to use when setting the bits. All bits not in the
+  ///             mask will be unmodified, while all bits within the mask will
+  ///             be set to the value of the corresponding bit in the value
+  /// \param value The value to set. Bits in the value should correspond to the
+  ///             bits in the mask
+  /// \param ec The error code to set if there is an error
+  void set_bits_in_register_by_mask(RegisterAddressType register_address, uint8_t mask,
+                                    uint8_t value, std::error_code &ec) {
+    logger_.debug("set_bits_in_register_by_mask 0x{:x} with mask 0x{:x} and value 0x{:x}",
+                  register_address, mask, value);
+    std::lock_guard<std::recursive_mutex> lock(base_mutex_);
+    uint8_t data = read_u8_from_register(register_address, ec);
+    if (ec) {
+      return;
+    }
+    data &= ~mask;
+    data |= value & mask;
+    write_u8_to_register(register_address, data, ec);
+  }
+
   /// Clear bits in a register on the peripheral
   /// \param register_address The address of the register to modify
   /// \param mask The mask to clear

components/bldc_motor/include/bldc_motor.hpp

@@ -407,8 +407,9 @@ class BldcMotor : public BaseComponent {
       float Uout;
       // a bit of optitmisation
       if (ud) { // only if ud and uq set
-        // fast_sqrt is an approx of sqrt (3-4% error)
-        Uout = fast_sqrt(ud * ud + uq * uq) / driver_->get_voltage_limit();
+        // fast_inv_sqrt is an approx of invsqrt (3-4% error), so we need to
+        // invert it to get the correct value (1/inv_sqrt = sqrt)
+        Uout = 1.0f / (fast_inv_sqrt(ud * ud + uq * uq) * driver_->get_voltage_limit());
         // angle normalisation in between 0 and 2pi
         // only necessary if using fast_sin and fast_cos - approximation functions
         el_angle = normalize_angle(el_angle + atan2(uq, ud));

components/esp-box/CMakeLists.txt

@@ -1,6 +1,6 @@
 idf_component_register(
   INCLUDE_DIRS "include"
   SRC_DIRS "src"
-  REQUIRES driver base_component codec display display_drivers i2c input_drivers interrupt gt911 task tt21100
+  REQUIRES driver base_component codec display display_drivers i2c input_drivers interrupt gt911 task tt21100 icm42607
   REQUIRED_IDF_TARGETS "esp32s3"
   )

components/esp-box/example/CMakeLists.txt

@@ -11,7 +11,7 @@ set(EXTRA_COMPONENT_DIRS
 
 set(
   COMPONENTS
-  "main esptool_py esp-box"
+  "main esptool_py esp-box filters"
   CACHE STRING
   "List of components to include"
   )

components/esp-box/example/README.md

@@ -9,10 +9,13 @@ state and each time you touch the scren it 1) uses LVGL to draw a circle where
 you touch, and 2) play a click sound (wav file bundled with the firmware). If
 you press the home button on the display, it will clear the circles.
 
+https://github.com/user-attachments/assets/6fc9ce02-fdae-4f36-9ee7-3a6c347ca1c4
+
 https://github.com/esp-cpp/espp/assets/213467/d5379983-9bc2-4d56-a9fc-9e37f54af15e
 
 ![image](https://github.com/esp-cpp/espp/assets/213467/bfb45218-0d1f-4a07-ba30-c0c74a1657b9)
 ![image](https://github.com/esp-cpp/espp/assets/213467/c7216cfd-330d-4610-baf2-30001c98ff42)
+![image](https://github.com/user-attachments/assets/6d9901f1-f4fe-433a-b6d5-c8c82abd14b7)
 
 ## How to use example
 
@@ -42,3 +45,16 @@ BOX3:
 
 BOX:
 ![CleanShot 2024-07-01 at 09 56 23](https://github.com/esp-cpp/espp/assets/213467/2f758ff5-a82e-4620-896e-99223010f013)
+
+Slow rotation video:
+
+https://github.com/user-attachments/assets/6fc9ce02-fdae-4f36-9ee7-3a6c347ca1c4
+
+Fast rotation video:
+
+https://github.com/user-attachments/assets/64fd1d71-d6b2-4c4f-8538-4097d4d162e9
+
+Pictures showing the new gravity vector pointing down from the center of the screen:
+![image](https://github.com/user-attachments/assets/0a971b19-95b9-468c-8d5b-e99f2fbf76b9)
+![image](https://github.com/user-attachments/assets/6d9901f1-f4fe-433a-b6d5-c8c82abd14b7)
+

components/esp-box/example/main/esp_box_example.cpp

@@ -5,6 +5,9 @@
 
 #include "esp-box.hpp"
 
+#include "kalman_filter.hpp"
+#include "madgwick_filter.hpp"
+
 using namespace std::chrono_literals;
 
 static constexpr size_t MAX_CIRCLES = 100;
@@ -80,16 +83,50 @@ extern "C" void app_main(void) {
     return;
   }
 
+  // initialize the IMU
+  if (!box.initialize_imu()) {
+    logger.error("Failed to initialize IMU!");
+    return;
+  }
+
   // set the background color to black
   lv_obj_t *bg = lv_obj_create(lv_screen_active());
   lv_obj_set_size(bg, box.lcd_width(), box.lcd_height());
   lv_obj_set_style_bg_color(bg, lv_color_make(0, 0, 0), 0);
 
   // add text in the center of the screen
   lv_obj_t *label = lv_label_create(lv_screen_active());
-  lv_label_set_text(label, "Touch the screen!\nPress the home button to clear circles.");
-  lv_obj_align(label, LV_ALIGN_CENTER, 0, 0);
-  lv_obj_set_style_text_align(label, LV_TEXT_ALIGN_CENTER, 0);
+  static std::string label_text =
+      "\n\n\n\nTouch the screen!\nPress the home button to clear circles.";
+  lv_label_set_text(label, label_text.c_str());
+  lv_obj_align(label, LV_ALIGN_TOP_LEFT, 0, 0);
+  lv_obj_set_style_text_align(label, LV_TEXT_ALIGN_LEFT, 0);
+
+  /*Create style*/
+  static lv_style_t style_line0;
+  lv_style_init(&style_line0);
+  lv_style_set_line_width(&style_line0, 8);
+  lv_style_set_line_color(&style_line0, lv_palette_main(LV_PALETTE_BLUE));
+  lv_style_set_line_rounded(&style_line0, true);
+
+  // make a line for showing the direction of "down"
+  lv_obj_t *line0 = lv_line_create(lv_screen_active());
+  static lv_point_precise_t line_points0[] = {{0, 0}, {box.lcd_width(), box.lcd_height()}};
+  lv_line_set_points(line0, line_points0, 2);
+  lv_obj_add_style(line0, &style_line0, 0);
+
+  /*Create style*/
+  static lv_style_t style_line1;
+  lv_style_init(&style_line1);
+  lv_style_set_line_width(&style_line1, 8);
+  lv_style_set_line_color(&style_line1, lv_palette_main(LV_PALETTE_RED));
+  lv_style_set_line_rounded(&style_line1, true);
+
+  // make a line for showing the direction of "down"
+  lv_obj_t *line1 = lv_line_create(lv_screen_active());
+  static lv_point_precise_t line_points1[] = {{0, 0}, {box.lcd_width(), box.lcd_height()}};
+  lv_line_set_points(line1, line_points1, 2);
+  lv_obj_add_style(line1, &style_line1, 0);
 
   // add a button in the top left which (when pressed) will rotate the display
   // through 0, 90, 180, 270 degrees
@@ -129,6 +166,7 @@ extern "C" void app_main(void) {
                       },
                       .task_config = {
                           .name = "lv_task",
+                          .stack_size_bytes = 6 * 1024,
                       }});
   lv_task.start();
 
@@ -143,6 +181,112 @@ extern "C" void app_main(void) {
   // set the display brightness to be 75%
   box.brightness(75.0f);
 
+  // make a task to read out the IMU data and print it to console
+  espp::Task imu_task(
+      {.callback = [&label, &line0, &line1](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);
+         }
+         static auto &box = espp::EspBox::get();
+         static auto imu = box.imu();
+
+         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;
+         // get accel
+         auto accel = imu->get_accelerometer(ec);
+         if (ec) {
+           return false;
+         }
+         auto gyro = imu->get_gyroscope(ec);
+         if (ec) {
+           return false;
+         }
+         auto temp = imu->get_temperature(ec);
+         if (ec) {
+           return false;
+         }
+
+         // with only the accelerometer + gyroscope, we can't get yaw :(
+         float roll = 0, pitch = 0;
+         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);
+
+         f.update(dt, accel.x, accel.y, accel.z, gyro.x * M_PI / 180.0f, gyro.y * M_PI / 180.0f,
+                  gyro.z * M_PI / 180.0f);
+         float yaw; // ignore / unused since we only have 6-axis
+         f.get_euler(roll, pitch, yaw);
+         pitch *= M_PI / 180.0f;
+         roll *= M_PI / 180.0f;
+
+         std::string text = fmt::format("{}\n\n\n\n\n", label_text);
+         text += fmt::format("Accel: {:02.2f} {:02.2f} {:02.2f}\n", accel.x, accel.y, accel.z);
+         text += fmt::format("Gyro: {:03.2f} {:03.2f} {:03.2f}\n", gyro.x * M_PI / 180.0f,
+                             gyro.y * M_PI / 180.0f, gyro.z * M_PI / 180.0f);
+         text +=
+             fmt::format("Angle: {:03.2f} {:03.2f}\n", roll * 180.0f / M_PI, pitch * 180.0f / M_PI);
+         text += fmt::format("Temp: {:02.1f} C\n", temp);
+
+         // use the pitch to to draw a line on the screen indiating the
+         // direction from the center of the screen to "down"
+         int x0 = box.lcd_width() / 2;
+         int y0 = box.lcd_height() / 2;
+
+         float vx = sin(pitch);
+         float vy = -cos(pitch) * sin(roll);
+         float vz = -cos(pitch) * cos(roll);
+
+         int x1 = x0 + 50 * vx;
+         int y1 = y0 + 50 * vy;
+
+         static lv_point_precise_t line_points0[] = {{x0, y0}, {x1, y1}};
+         line_points0[1].x = x1;
+         line_points0[1].y = y1;
+
+         // Apply Kalman filter
+         float accelPitch = atan2(-accel.x, sqrt(accel.y * accel.y + accel.z * accel.z));
+         float accelRoll = atan2(accel.y, accel.z);
+         kf.predict({float(gyro.x * M_PI / 180.0f), float(gyro.y * M_PI / 180.0f)}, dt);
+         kf.update({accelPitch, accelRoll});
+         std::tie(pitch, roll) = kf.get_state();
+
+         vx = sin(pitch);
+         vy = -cos(pitch) * sin(roll);
+         vz = -cos(pitch) * cos(roll);
+
+         x1 = x0 + 50 * vx;
+         y1 = y0 + 50 * vy;
+
+         static lv_point_precise_t line_points1[] = {{x0, y0}, {x1, y1}};
+         line_points1[1].x = x1;
+         line_points1[1].y = y1;
+
+         std::lock_guard<std::recursive_mutex> lock(lvgl_mutex);
+         lv_label_set_text(label, text.c_str());
+         lv_line_set_points(line0, line_points0, 2);
+         lv_line_set_points(line1, line_points1, 2);
+
+         return false;
+       },
+       .task_config = {
+           .name = "IMU",
+           .stack_size_bytes = 6 * 1024,
+           .priority = 10,
+           .core_id = 0,
+       }});
+  imu_task.start();
+
   // loop forever
   while (true) {
     std::this_thread::sleep_for(1s);

components/esp-box/include/esp-box.hpp

@@ -19,6 +19,7 @@
 #include "es8311.hpp"
 #include "gt911.hpp"
 #include "i2c.hpp"
+#include "icm42607.hpp"
 #include "interrupt.hpp"
 #include "st7789.hpp"
 #include "touchpad_input.hpp"
@@ -32,6 +33,9 @@ namespace espp {
 /// - Touchpad
 /// - Display
 /// - Audio
+/// - Interrupts
+/// - I2C
+/// - IMU (Inertial Measurement Unit)
 ///
 /// The class is a singleton and can be accessed using the get() method.
 ///
@@ -44,8 +48,13 @@ class EspBox : public BaseComponent {
 
   /// Alias for the display driver used by the ESP-Box display
   using DisplayDriver = espp::St7789;
+
+  /// Alias for the touchpad data used by the ESP-Box touchpad
   using TouchpadData = espp::TouchpadData;
 
+  /// Alias the IMU used by the ESP-Box
+  using Imu = espp::Icm42607<icm42607::Interface::I2C>;
+
   /// The type of the box
   enum class BoxType {
     UNKNOWN, ///< unknown box
@@ -286,6 +295,18 @@ class EspBox : public BaseComponent {
   /// \param num_bytes The number of bytes to play
   void play_audio(const uint8_t *data, uint32_t num_bytes);
 
+  /////////////////////////////////////////////////////////////////////////////
+  // IMU
+  /////////////////////////////////////////////////////////////////////////////
+
+  /// Initialize the IMU
+  /// \return true if the IMU was successfully initialized, false otherwise
+  bool initialize_imu();
+
+  /// Get the IMU
+  /// \return A shared pointer to the IMU
+  std::shared_ptr<Imu> imu() const;
+
 protected:
   EspBox();
   void detect();
@@ -440,6 +461,9 @@ class EspBox : public BaseComponent {
   i2s_std_config_t audio_std_cfg;
   i2s_event_callbacks_t audio_tx_callbacks_;
   std::atomic<bool> has_sound{false};
+
+  // IMU
+  std::shared_ptr<Imu> imu_;
 }; // class EspBox
 } // namespace espp