feat(filters): Update filters example, and expose filters to x-plat l…

…ibrary (#330)

* feat(filters): Expose lowpass filters to lib and update example

* update example to test more permutations of filters

* fix sa

* add missing cpp files to cmake

* readme: update

components/filters/CMakeLists.txt

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

components/filters/example/README.md

@@ -1,8 +1,8 @@
 # Filters Example
 
-This example shows how to use a `LowpassFilter` and a `ButterworthFilter` from
-the `filters` component to filter signals. This example simply operates on
-random perturbations of auto-generated data.
+This example shows how to use a `LowpassFilter`, `ButterworthFilter`, and
+`SimpleLowpassFilter` from the `filters` component to filter signals. This
+example simply operates on random perturbations of auto-generated data.
 
 ## How to use example
 
@@ -22,4 +22,4 @@ See the Getting Started Guide for full steps to configure and use ESP-IDF to bui
 
 ## Example Output
 
-![output](https://user-images.githubusercontent.com/213467/202814226-a0a41887-d5f0-4de8-a69b-df3e1a5ed316.png)
+![output](https://github.com/user-attachments/assets/091ca271-3923-448e-9d26-998fc4f97297)

components/filters/example/main/filters_example.cpp

@@ -5,6 +5,7 @@
 
 #include "butterworth_filter.hpp"
 #include "lowpass_filter.hpp"
+#include "simple_lowpass_filter.hpp"
 #include "task.hpp"
 
 using namespace std::chrono_literals;
@@ -29,21 +30,29 @@ extern "C" void app_main(void) {
         .normalized_cutoff_frequency = normalized_cutoff_frequency,
         .q_factor = 1.0f,
     });
-    static constexpr size_t ORDER = 2;
+    espp::SimpleLowpassFilter slpf({
+        .time_constant = normalized_cutoff_frequency,
+    });
+    espp::ButterworthFilter<1, espp::BiquadFilterDf1> bwf_df1_o1(
+        {.normalized_cutoff_frequency = normalized_cutoff_frequency});
+    espp::ButterworthFilter<2, espp::BiquadFilterDf1> bwf_df1_o2(
+        {.normalized_cutoff_frequency = normalized_cutoff_frequency});
     // NOTE: using the Df2 since it's hardware accelerated :)
-    espp::ButterworthFilter<ORDER, espp::BiquadFilterDf2> butterworth(
+    espp::ButterworthFilter<2, espp::BiquadFilterDf2> bwf_df2_o2(
+        {.normalized_cutoff_frequency = normalized_cutoff_frequency});
+    espp::ButterworthFilter<4, espp::BiquadFilterDf2> bwf_df2_o4(
         {.normalized_cutoff_frequency = normalized_cutoff_frequency});
-    fmt::print("{}\n", butterworth);
     static auto start = std::chrono::high_resolution_clock::now();
-    auto task_fn = [&lpf, &butterworth](std::mutex &m, std::condition_variable &cv) {
+    auto task_fn = [&](std::mutex &m, std::condition_variable &cv) {
       auto now = std::chrono::high_resolution_clock::now();
       float seconds = std::chrono::duration<float>(now - start).count();
       // use time to create a stairstep function
       constexpr float noise_scale = 0.2f;
       float input = floor(seconds) + get_random() * noise_scale;
-      float lpf_output = lpf.update(input);
-      float bwf_output = butterworth.update(input);
-      fmt::print("{:.03f}, {:.03f}, {:.03f}, {:.03f}\n", seconds, input, lpf_output, bwf_output);
+      fmt::print("{:.03f}, {:.03f}, {:.03f}, {:.03f}, "
+                 "{:.03f}, {:.03f}, {:.03f}, {:.03f}\n",
+                 seconds, input, slpf.update(input), lpf.update(input), bwf_df1_o1.update(input),
+                 bwf_df1_o2.update(input), bwf_df2_o2.update(input), bwf_df2_o4.update(input));
       // NOTE: sleeping in this way allows the sleep to exit early when the
       // task is being stopped / destroyed
       {
@@ -56,7 +65,9 @@ extern "C" void app_main(void) {
     auto task = espp::Task({.name = "Lowpass Filter",
                             .callback = task_fn,
                             .log_level = espp::Logger::Verbosity::INFO});
-    fmt::print("% time (s), input, lpf_output, bwf_output\n");
+    fmt::print("% time (s), input, simple_lpf_output, lpf_output, "
+               "bwf_df1_o1_output, bwf_df1_o2_output, bwf_df2_o2_output, "
+               "bwf_df2_o4_output\n");
     task.start();
     //! [filter example]
     std::this_thread::sleep_for(num_seconds_to_run * 1s);

components/filters/include/biquad_filter.hpp

@@ -3,7 +3,9 @@
 #include <array>
 #include <cmath>
 
+#if defined(ESP_PLATFORM)
 #include "esp_dsp.h"
+#endif
 
 #include "format.hpp"
 #include "transfer_function.hpp"
@@ -122,7 +124,13 @@ class BiquadFilterDf2 {
    * @param length Number of samples, should be >= length of input & output memory.
    */
   void update(const float *input, float *output, size_t length) {
+#if defined(ESP_PLATFORM)
     dsps_biquad_f32(input, output, length, coeffs_.data(), w_.data());
+#else
+    for (size_t i = 0; i < length; i++) {
+      output[i] = update(input[i]);
+    }
+#endif
   }
 
   /**
@@ -133,7 +141,13 @@ class BiquadFilterDf2 {
    */
   float update(const float input) {
     float result;
+#if defined(ESP_PLATFORM)
     dsps_biquad_f32(&input, &result, 1, coeffs_.data(), w_.data());
+#else
+    result = input * b_[0] + w_[0];
+    w_[0] = input * b_[1] - result * a_[0] + w_[1];
+    w_[1] = input * b_[2] - result * a_[1];
+#endif
     return result;
   }
 
@@ -148,28 +162,4 @@ class BiquadFilterDf2 {
 };
 } // namespace espp
 
-// for allowing easy serialization/printing of the
-// espp::BiquadFilterDf1
-template <> struct fmt::formatter<espp::BiquadFilterDf1> {
-  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
-    return ctx.begin();
-  }
-
-  template <typename FormatContext>
-  auto format(espp::BiquadFilterDf1 const &bqf, FormatContext &ctx) const {
-    return fmt::format_to(ctx.out(), "DF1 - B: {}, A: {}", bqf.b_, bqf.a_);
-  }
-};
-
-// for allowing easy serialization/printing of the
-// espp::BiquadFilterDf2
-template <> struct fmt::formatter<espp::BiquadFilterDf2> {
-  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
-    return ctx.begin();
-  }
-
-  template <typename FormatContext>
-  auto format(espp::BiquadFilterDf2 const &bqf, FormatContext &ctx) const {
-    return fmt::format_to(ctx.out(), "DF2 - B: {}, A: {}", bqf.b_, bqf.a_);
-  }
-};
+#include "biquad_filter_formatters.hpp"

components/filters/include/biquad_filter_formatters.hpp

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "format.hpp"
+
+// for allowing easy serialization/printing of the
+// espp::BiquadFilterDf1
+template <> struct fmt::formatter<espp::BiquadFilterDf1> {
+  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(espp::BiquadFilterDf1 const &bqf, FormatContext &ctx) const {
+    return fmt::format_to(ctx.out(), "DF1 - B: {}, A: {}", bqf.b_, bqf.a_);
+  }
+};
+
+// for allowing easy serialization/printing of the
+// espp::BiquadFilterDf2
+template <> struct fmt::formatter<espp::BiquadFilterDf2> {
+  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(espp::BiquadFilterDf2 const &bqf, FormatContext &ctx) const {
+    return fmt::format_to(ctx.out(), "DF2 - B: {}, A: {}", bqf.b_, bqf.a_);
+  }
+};

components/filters/include/butterworth_filter.hpp

@@ -2,6 +2,7 @@
 
 #include <cmath>
 
+#include "format.hpp"
 #include "sos_filter.hpp"
 
 namespace espp {
@@ -15,7 +16,7 @@ namespace espp {
  * @tparam ORDER The order of the filter.
  * @tparam Impl Which Biquad implementation form to use.
  */
-template <size_t ORDER, class Impl = BiquadFilterDf2>
+template <size_t ORDER, class Impl = BiquadFilterDf1>
 class ButterworthFilter : public SosFilter<(ORDER + 1) / 2, Impl> {
 public:
   /**
@@ -72,23 +73,4 @@ class ButterworthFilter : public SosFilter<(ORDER + 1) / 2, Impl> {
 };
 } // namespace espp
 
-// for allowing easy serialization/printing of the
-// espp::ButterworthFilter
-template <size_t ORDER, class Impl> struct fmt::formatter<espp::ButterworthFilter<ORDER, Impl>> {
-  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
-    return ctx.begin();
-  }
-
-  template <typename FormatContext>
-  auto format(espp::ButterworthFilter<ORDER, Impl> const &f, FormatContext &ctx) const {
-    auto &&out = ctx.out();
-    fmt::format_to(out, "Butterworth - [");
-    if constexpr (ORDER > 0) {
-      fmt::format_to(out, "[{}]", f.sections_[0]);
-    }
-    for (int i = 1; i < (ORDER + 1) / 2; i++) {
-      fmt::format_to(out, ", [{}]", f.sections_[i]);
-    }
-    return fmt::format_to(out, "]");
-  }
-};
+#include "butterworth_filter_formatters.hpp"

components/filters/include/butterworth_filter_formatters.hpp

@@ -0,0 +1,24 @@
+#pragma once
+
+#include "format.hpp"
+
+// for allowing easy serialization/printing of the
+// espp::ButterworthFilter
+template <size_t ORDER, class Impl> struct fmt::formatter<espp::ButterworthFilter<ORDER, Impl>> {
+  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(espp::ButterworthFilter<ORDER, Impl> const &f, FormatContext &ctx) const {
+    auto &&out = ctx.out();
+    fmt::format_to(out, "Butterworth - [");
+    if constexpr (ORDER > 0) {
+      fmt::format_to(out, "[{}]", f.sections_[0]);
+    }
+    for (int i = 1; i < (ORDER + 1) / 2; i++) {
+      fmt::format_to(out, ", [{}]", f.sections_[i]);
+    }
+    return fmt::format_to(out, "]");
+  }
+};

components/filters/include/lowpass_filter.hpp

@@ -2,7 +2,11 @@
 
 #include <stdlib.h>
 
+#if defined(ESP_PLATFORM)
 #include "esp_dsp.h"
+#endif
+
+#include "format.hpp"
 
 namespace espp {
 /**
@@ -21,11 +25,22 @@ class LowpassFilter {
         q_factor; /**< Quality (Q) factor of the filter. The higher the Q the better the filter. */
   };
 
+  /**
+   * @brief Default constructor.
+   */
+  LowpassFilter() = default;
+
   /**
    * @brief Initialize the lowpass filter coefficients based on the config.
    * @param config Configuration struct.
    */
-  explicit LowpassFilter(const Config &config) { init(config); }
+  explicit LowpassFilter(const Config &config);
+
+  /**
+   * @brief Set the filter coefficients based on the config.
+   * @param config Configuration struct.
+   */
+  void configure(const Config &config);
 
   /**
    * @brief Filter the input samples, updating internal state, and writing the
@@ -34,52 +49,41 @@ class LowpassFilter {
    * @param output Pointer to (floating point) array which will be filled with
    *        the filtered input.
    * @param length Number of samples, should be >= length of input & output memory.
+   * @note On ESP32, the input and output arrays must have
+   *       __attribute__((aligned(16))) to ensure proper alignment for the ESP32
+   *       DSP functions.
    */
-  void update(const float *input, float *output, size_t length) {
-    dsps_biquad_f32(input, output, length, coeffs_, state_);
-  }
+  void update(const float *input, float *output, size_t length);
 
   /**
    * @brief Filter the signal sampled by input, updating internal state, and
    *        returning the filtered output.
    * @param input New sample of the input data.
    * @return Filtered output based on input and history.
    */
-  float update(const float input) {
-    float output;
-    dsps_biquad_f32(&input, &output, 1, coeffs_, state_);
-    return output;
-  }
+  float update(const float input);
 
   /**
    * @brief Filter the signal sampled by input, updating internal state, and
    *        returning the filtered output.
    * @param input New sample of the input data.
    * @return Filtered output based on input and history.
    */
-  float operator()(float input) { return update(input); }
+  float operator()(float input);
+
+  /**
+   * @brief Reset the filter state to zero.
+   */
+  void reset();
 
   friend struct fmt::formatter<LowpassFilter>;
 
 protected:
-  void init(const Config &config) {
-    dsps_biquad_gen_lpf_f32(coeffs_, config.normalized_cutoff_frequency, config.q_factor);
-  }
+  void init(const Config &config);
 
-  float coeffs_[5];
-  float state_[2];
+  float coeffs_[5] = {0, 0, 0, 0, 0};
+  float state_[5] = {0, 0, 0, 0, 0};
 };
 } // namespace espp
 
-// for allowing easy serialization/printing of the
-// espp::LowpassFilter
-template <> struct fmt::formatter<espp::LowpassFilter> {
-  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
-    return ctx.begin();
-  }
-
-  template <typename FormatContext>
-  auto format(espp::LowpassFilter const &f, FormatContext &ctx) const {
-    return fmt::format_to(ctx.out(), "Lowpass - {}", f.coeffs_);
-  }
-};
+#include "lowpass_filter_formatters.hpp"

components/filters/include/lowpass_filter_formatters.hpp

@@ -0,0 +1,16 @@
+#pragma once
+
+#include "format.hpp"
+
+// for allowing easy serialization/printing of the
+// espp::LowpassFilter
+template <> struct fmt::formatter<espp::LowpassFilter> {
+  template <typename ParseContext> constexpr auto parse(ParseContext &ctx) const {
+    return ctx.begin();
+  }
+
+  template <typename FormatContext>
+  auto format(espp::LowpassFilter const &f, FormatContext &ctx) const {
+    return fmt::format_to(ctx.out(), "Lowpass - {}", f.coeffs_);
+  }
+};