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

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/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/main/esp_box_example.cpp

@@ -5,6 +5,8 @@
 
 #include "esp-box.hpp"
 
+#include "kalman_filter.hpp"
+
 using namespace std::chrono_literals;
 
 static constexpr size_t MAX_CIRCLES = 100;
@@ -93,9 +95,24 @@ extern "C" void app_main(void) {
 
   // 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\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_line;
+  lv_style_init(&style_line);
+  lv_style_set_line_width(&style_line, 8);
+  lv_style_set_line_color(&style_line, lv_palette_main(LV_PALETTE_BLUE));
+  lv_style_set_line_rounded(&style_line, true);
+
+  // make a line for showing the direction of "down"
+  lv_obj_t *line = lv_line_create(lv_screen_active());
+  static lv_point_precise_t line_points[] = {{0, 0}, {box.lcd_width(), box.lcd_height()}};
+  lv_line_set_points(line, line_points, 2);
+  lv_obj_add_style(line, &style_line, 0);
 
   // add a button in the top left which (when pressed) will rotate the display
   // through 0, 90, 180, 270 degrees
@@ -135,6 +152,7 @@ extern "C" void app_main(void) {
                       },
                       .task_config = {
                           .name = "lv_task",
+                          .stack_size_bytes = 6 * 1024,
                       }});
   lv_task.start();
 
@@ -151,19 +169,19 @@ extern "C" void app_main(void) {
 
   // make a task to read out the IMU data and print it to console
   espp::Task imu_task(
-      {.callback = [&label](std::mutex &m, std::condition_variable &cv) -> bool {
+      {.callback = [&label, &line](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, 100ms);
+           cv.wait_for(lock, 10ms);
          }
          static auto &box = espp::EspBox::get();
          static auto imu = box.imu();
 
-         auto now = std::chrono::steady_clock::now();
+         auto now = esp_timer_get_time(); // time in microseconds
          static auto t0 = now;
          auto t1 = now;
-         float dt = std::chrono::duration<float>(t1 - t0).count();
+         float dt = (t1 - t0) / 1'000'000.0f; // convert us to s
          t0 = t1;
 
          std::error_code ec;
@@ -181,17 +199,51 @@ extern "C" void app_main(void) {
            return false;
          }
 
-         static espp::icm42607::ComplimentaryAngle angle{};
-         angle = imu->complimentary_filter(dt, angle, accel, gyro);
+         float roll = 0, pitch = 0;
+
+         // with only the accelerometer + gyroscope, we can't get yaw :(
+         static espp::KalmanFilter kalmanPitch;
+         static espp::KalmanFilter kalmanRoll;
+
+         // Compute pitch and roll from accelerometer
+         float accelPitch =
+             atan2(accel.y, sqrt(accel.x * accel.x + accel.z * accel.z)) * 180.0f / M_PI;
+         float accelRoll = atan2(-accel.x, accel.z) * 180.0f / M_PI;
 
-         std::string text = "Touch the screen!\nPress the home button to clear circles.\n";
+         // Apply Kalman filter
+         pitch = kalmanPitch.update(accelPitch, gyro.y, dt);
+         roll = kalmanRoll.update(accelRoll, gyro.x, dt);
+
+         std::string text = fmt::format("{}\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, gyro.y, gyro.z);
-         text += fmt::format("Angle: {:03.2f} {:03.2f}\n", angle.roll, angle.pitch);
+         text += fmt::format("Angle: {:03.2f} {:03.2f}\n", roll, pitch);
          text += fmt::format("Temp: {:02.1f} C\n", temp);
 
+         float rollRad = roll * M_PI / 180.0f;
+         float pitchRad = pitch * M_PI / 180.0f;
+
+         // 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;
+         int x1 = x0 - 50 * cos(-pitchRad);
+         int y1 = y0 + 50 * sin(-pitchRad);
+
+         float vx = sin(pitchRad);
+         float vy = -cos(pitchRad) * sin(rollRad);
+         float vz = -cos(pitchRad) * cos(rollRad);
+
+         x1 = x0 - 50 * vy;
+         y1 = y0 - 50 * vx;
+
+         static lv_point_precise_t line_points[] = {{x0, y0}, {x1, y1}};
+         line_points[1].x = x1;
+         line_points[1].y = y1;
+
          std::lock_guard<std::recursive_mutex> lock(lvgl_mutex);
          lv_label_set_text(label, text.c_str());
+         lv_line_set_points(line, line_points, 2);
 
          return false;
        },

components/filters/CMakeLists.txt

@@ -1,4 +1,4 @@
 idf_component_register(
   INCLUDE_DIRS "include"
   SRC_DIRS "src"
-  REQUIRES esp_timer format esp-dsp)
+  REQUIRES esp_timer format esp-dsp math)

components/filters/include/complementary_filter.hpp

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <cmath>
+
+#include "fast_math.hpp"
+
+namespace espp {
+/// Complementary filter for estimating pitch and roll angles from
+/// accelerometer and gyroscope readings.
+/// The filter is defined by the following equation:
+///  angle = alpha * (angle + gyro * dt) + (1 - alpha) * accel
+///  where:
+///  - angle is the estimated angle,
+///  - alpha is the filter coefficient (0 < alpha < 1),
+///  - gyro is the gyroscope reading,
+///  - accel is the accelerometer reading,
+///  - dt is the time step.
+class ComplementaryFilter {
+public:
+  /// Constructor
+  /// \param alpha Filter coefficient (0 < alpha < 1)
+  ComplementaryFilter(float alpha = 0.98)
+      : alpha(alpha)
+      , pitch(0.0f)
+      , roll(0.0f) {}
+
+  /// Update the filter with accelerometer and gyroscope readings.
+  /// \param accel Accelerometer readings (m/s^2)
+  /// \param gyro Gyroscope readings (degrees/s)
+  /// \param dt Time step (s)
+  /// \note The accelerometer and gyroscope readings must be in the
+  /// same coordinate system.
+  void update(const std::span<const float, 3> &accel, const std::span<const float, 3> &gyro,
+              float dt) {
+    update(accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], dt);
+  }
+
+  /// Update the filter with accelerometer and gyroscope readings.
+  /// \param ax Accelerometer x-axis reading (m/s^2)
+  /// \param ay Accelerometer y-axis reading (m/s^2)
+  /// \param az Accelerometer z-axis reading (m/s^2)
+  /// \param gx Gyroscope x-axis reading (degrees/s)
+  /// \param gy Gyroscope y-axis reading (degrees/s)
+  /// \param gz Gyroscope z-axis reading (degrees/s)
+  /// \param dt Time step (s)
+  /// \note The accelerometer and gyroscope readings must be in the
+  /// same coordinate system.
+  void update(float ax, float ay, float az, float gx, float gy, float gz, float dt) {
+    // Convert gyroscope readings from degrees/s to radians/s
+    gx = gx * M_PI / 180.0f;
+    gy = gy * M_PI / 180.0f;
+
+    // Compute pitch and roll from accelerometer (degrees)
+    float accelPitch = atan2(ay, sqrt(ax * ax + az * az)) * 180.0f / M_PI;
+    float accelRoll = atan2(-ax, az) * 180.0f / M_PI;
+
+    // Integrate gyroscope readings
+    float gyroPitch = pitch + gy * dt;
+    float gyroRoll = roll + gx * dt;
+
+    // Apply complementary filter
+    pitch = alpha * gyroPitch + (1 - alpha) * accelPitch;
+    roll = alpha * gyroRoll + (1 - alpha) * accelRoll;
+  }
+
+  /// Get the estimated pitch angle (degrees)
+  /// \return The estimated pitch angle
+  float get_pitch() const { return pitch; }
+
+  /// Get the estimated roll angle (degrees)
+  /// \return The estimated roll angle
+  float get_roll() const { return roll; }
+
+protected:
+  float alpha;
+  float pitch, roll;
+}; // class ComplimentaryFilter
+} // namespace espp

components/filters/include/kalman_filter.hpp

@@ -0,0 +1,84 @@
+#pragma once
+
+#include <cmath>
+
+#include "fast_math.hpp"
+
+namespace espp {
+/// Kalman Filter for angle estimation
+///
+/// This class implements a simple Kalman filter for estimating the angle of a
+/// system based on accelerometer and gyroscope measurements. The filter is
+/// based on the following state-space model:
+///  x(k+1) = A * x(k) + B * u(k) + w(k)
+///  z(k) = H * x(k) + v(k)
+///  where:
+///  - x(k) is the state vector at time k
+///  - u(k) is the control input at time k
+///  - z(k) is the measurement at time k
+///  - w(k) is the process noise at time k
+///  - v(k) is the measurement noise at time k
+///  - A, B, and H are matrices
+///  - A is the state transition matrix
+///  - B is the control input matrix
+///  - H is the measurement matrix
+///  The Kalman filter estimates the state x(k) based on the measurements z(k).
+///  The filter is implemented in two steps: prediction and correction.
+///  The prediction step estimates the state at the next time step based on the
+///  current state and the control input. The correction step updates the state
+///  estimate based on the measurement.
+class KalmanFilter {
+public:
+  /// Constructor
+  KalmanFilter() {
+    angle = 0.0f;
+    bias = 0.0f;
+    P[0][0] = 1.0f;
+    P[0][1] = 0.0f;
+    P[1][0] = 0.0f;
+    P[1][1] = 1.0f;
+
+    Q_angle = 0.001f;  // Process noise variance for accelerometer
+    Q_bias = 0.003f;   // Process noise variance for gyro bias
+    R_measure = 0.03f; // Measurement noise variance
+  }
+
+  /// Update the filter with a new measurement
+  /// \param newAngle The new angle measurement
+  /// \param newRate The new rate measurement
+  /// \param dt The time step
+  /// \return The updated angle estimate
+  float update(float newAngle, float newRate, float dt) {
+    // Prediction step
+    rate = newRate - bias;
+    angle += dt * rate;
+
+    P[0][0] += dt * (dt * P[1][1] - P[0][1] - P[1][0] + Q_angle);
+    P[0][1] -= dt * P[1][1];
+    P[1][0] -= dt * P[1][1];
+    P[1][1] += Q_bias * dt;
+
+    // Correction step
+    float S = P[0][0] + R_measure;
+    float K[2] = {P[0][0] / S, P[1][0] / S};
+
+    float y = newAngle - angle;
+    angle += K[0] * y;
+    bias += K[1] * y;
+
+    // Update error covariance matrix
+    float P00_temp = P[0][0], P01_temp = P[0][1];
+    P[0][0] -= K[0] * P00_temp;
+    P[0][1] -= K[0] * P01_temp;
+    P[1][0] -= K[1] * P00_temp;
+    P[1][1] -= K[1] * P01_temp;
+
+    return angle;
+  }
+
+protected:
+  float angle, bias, rate;
+  float P[2][2]; // Error covariance matrix
+  float Q_angle, Q_bias, R_measure;
+}; // class KalmanFilter
+} // namespace espp

components/filters/include/madgwick_filter.hpp

@@ -0,0 +1,83 @@
+#pragma once
+
+#include <cmath>
+
+#include "fast_math.hpp"
+
+namespace espp {
+class MadgwickFilter {
+public:
+  MadgwickFilter(float beta = 0.1f, float sampleFreq = 100.0f)
+      : beta(beta)
+      , sampleFreq(sampleFreq)
+      , q0(1.0f)
+      , q1(0.0f)
+      , q2(0.0f)
+      , q3(0.0f) {}
+
+  void update(float gx, float gy, float gz, float ax, float ay, float az) {
+    float recipNorm;
+    float s0, s1, s2, s3;
+    float qDot1, qDot2, qDot3, qDot4;
+
+    // Convert gyroscope from degrees/s to radians/s
+    gx *= M_PI / 180.0f;
+    gy *= M_PI / 180.0f;
+    gz *= M_PI / 180.0f;
+
+    // Compute quaternion derivatives
+    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
+    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
+    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
+    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
+
+    // Normalize accelerometer measurement
+    recipNorm = 1.0f / sqrt(ax * ax + ay * ay + az * az);
+    ax *= recipNorm;
+    ay *= recipNorm;
+    az *= recipNorm;
+
+    // Compute feedback correction terms
+    s0 = 2.0f * (q2 * q3 - q0 * q1) - ax;
+    s1 = 2.0f * (q0 * q2 + q1 * q3) - ay;
+    s2 = 1.0f - 2.0f * (q1 * q1 + q2 * q2) - az;
+    s3 = 2.0f * (q1 * q2 + q0 * q3) - az; // Corrected: Missing s3 term
+
+    // Normalize correction
+    recipNorm = 1.0f / sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+    s0 *= recipNorm;
+    s1 *= recipNorm;
+    s2 *= recipNorm;
+    s3 *= recipNorm;
+
+    // Compute feedback
+    qDot1 -= beta * s0;
+    qDot2 -= beta * s1;
+    qDot3 -= beta * s2;
+    qDot4 -= beta * s3; // Corrected: s3 was missing before
+
+    // Integrate quaternion
+    q0 += qDot1 * (1.0f / sampleFreq);
+    q1 += qDot2 * (1.0f / sampleFreq);
+    q2 += qDot3 * (1.0f / sampleFreq);
+    q3 += qDot4 * (1.0f / sampleFreq);
+
+    // Normalize quaternion
+    recipNorm = 1.0f / sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
+    q0 *= recipNorm;
+    q1 *= recipNorm;
+    q2 *= recipNorm;
+    q3 *= recipNorm;
+  }
+
+  void get_euler(float &pitch, float &roll, float &yaw) {
+    pitch = atan2(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) * 180.0f / M_PI;
+    roll = asin(2.0f * (q0 * q2 - q3 * q1)) * 180.0f / M_PI;
+    yaw = atan2(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) * 180.0f / M_PI;
+  }
+
+protected:
+  float beta, sampleFreq;
+  float q0, q1, q2, q3; // Quaternion values
+};
+} // namespace espp