AdaptiveResamplingStream DRAFT

Author: pschatzmann

src/AudioTools/CoreAudio/AdaptiveResamplingStream.h

@@ -0,0 +1,177 @@
+#pragma once
+
+#include "AudioTools/CoreAudio/AudioBasic/KalmanFilter.h"
+#include "AudioTools/CoreAudio/AudioBasic/PIDController.h"
+#include "AudioTools/CoreAudio/AudioStreams.h"
+#include "AudioTools/CoreAudio/ResampleStream.h"
+
+namespace audio_tools {
+
+/**
+ * @brief An Audio Stream backed by a buffer (queue) which tries to correct
+ * jitter and automatically adjusts for the slightly different clock rates
+ * between an audio source and audio target. Use separate tasks to write and
+ * read the data. Also make sure that you protect the access with a mutex or
+ * provide a thread-safe buffer!
+ *
+ * The resamping step size is calculated with the help of a PID controller.
+ * The fill level is smoothed using a Kalman filter.
+ *
+ * @ingroup buffers
+ * @author Phil Schatzmann
+ */
+class AdaptiveResamplingStream : public AudioStream {
+ public:
+  /**
+   * @brief Construct a new AdaptiveResamplingStream object
+   *
+   * @param buffer Reference to the buffer used for audio data
+   * @param stepRangePercent Allowed resampling range in percent (default: 0.05)
+   */
+  AdaptiveResamplingStream(BaseBuffer<uint8_t>& buffer,
+                           float stepRangePercent = 0.05) {
+    p_buffer = &buffer;
+    setStepRangePercent(stepRangePercent);
+    addNotifyAudioChange(resample_stream);
+  }
+
+  /**
+   * @brief Initialize the stream and internal components.
+   *
+   * @return true if initialization was successful, false otherwise
+   */
+  bool begin() {
+    if (p_buffer == nullptr) return false;
+    queue_stream.setBuffer(*p_buffer);
+    queue_stream.begin();
+    resample_stream.setAudioInfo(audioInfo());
+    resample_stream.setStream(queue_stream);
+    resample_stream.begin(audioInfo());
+    float from_step = 1.0 - resample_range;
+    float to_step = 1.0 + resample_range;
+    return pid.begin(1.0, from_step, to_step, p, i, d);
+  }
+
+  /**
+   * @brief Write audio data to the buffer.
+   *
+   * @param data Pointer to the data to write
+   * @param len Number of bytes to write
+   * @return size_t Number of bytes actually written
+   */
+  size_t write(const uint8_t* data, size_t len) override {
+    if (p_buffer == 0) return 0;
+    size_t result = p_buffer->writeArray(data, len);
+    recalculate();
+    return result;
+  }
+
+  /**
+   * @brief End the stream and release resources.
+   */
+  void end() {
+    queue_stream.end();
+    resample_stream.end();
+  }
+
+  /**
+   * @brief Read resampled audio data from the buffer.
+   *
+   * @param data Pointer to the buffer to fill
+   * @param len Number of bytes to read
+   * @return size_t Number of bytes actually read
+   */
+  size_t readBytes(uint8_t* data, size_t len) override {
+    if (p_buffer->available() == 0) return 0;
+
+    return resample_stream.readBytes(data, len);
+  }
+
+  /**
+   * @brief Recalculate the resampling step size based on buffer fill level.
+   *
+   * @return float The new step size
+   */
+  float recalculate() {
+    if (p_buffer == nullptr) return step_size;
+
+    // calculate new resampling step size
+    level_percent = p_buffer->levelPercent();
+    kalman_filter.addMeasurement(level_percent);
+    step_size = pid.calculate(50.0, kalman_filter.calculate());
+
+    // log step size every 10th read
+    if (read_count++ % 10 == 0) {
+      LOGI("step_size: %f", step_size);
+    }
+
+    // return resampled result
+    resample_stream.setStepSize(step_size);
+    return step_size;
+  }
+
+  /**
+   * @brief Set the allowed resampling range as a percent.
+   *
+   * @param rangePercent Allowed range in percent (e.g., 0.05 for ±0.05%)
+   */
+  void setStepRangePercent(float rangePercent) {
+    resample_range = rangePercent / 100.0;
+  }
+
+  /**
+   * @brief Get the current actual buffer fill level in percent.
+   *
+   * @return float Current fill level (0-100)
+   */
+  float levelPercentActual() {
+    if (p_buffer == nullptr) return 0.0f;
+    return p_buffer->levelPercent();
+  }
+
+  /**
+   * @brief Get the fill level at the last calculation in percent.
+   *
+   * @return float Last calculated fill level (0-100)
+   */
+  float levelPercent() { return level_percent; }
+
+  /**
+   * @brief Set the Kalman filter parameters.
+   *
+   * @param process_noise Process noise covariance (Q)
+   * @param measurement_noise Measurement noise covariance (R)
+   */
+  void setKalmanParameters(float process_noise, float measurement_noise) {
+    kalman_filter.begin(process_noise, measurement_noise);
+  }
+
+  /**
+   * @brief Set the PID controller parameters.
+   *
+   * @param p_value Proportional gain
+   * @param i_value Integral gain
+   * @param d_value Derivative gain
+   */
+  void setPIDParameters(float p_value, float i_value, float d_value) {
+    p = p_value;
+    i = i_value;
+    d = d_value;
+  }
+
+protected:
+  PIDController pid;                // PID controller for adaptive resampling step size (p=0.005, i=0.00005, d=0.0001)
+  QueueStream<uint8_t> queue_stream; // Internal queue stream for buffering audio data
+  BaseBuffer<uint8_t>* p_buffer = nullptr; // Pointer to the user-provided buffer
+  ResampleStream resample_stream;   // Resample stream for adjusting playback rate
+  KalmanFilter kalman_filter{0.01f, 0.1f}; // Kalman filter for smoothing buffer fill level
+  float step_size = 1.0;            // Current resampling step size
+  float resample_range = 0;         // Allowed resampling range (fraction)
+  float p = 0.005;                  // PID proportional gain
+  float i = 0.00005;                // PID integral gain
+  float d = 0.0001;                 // PID derivative gain
+  float level_percent = 0.0;        // Last calculated buffer fill level (percent)
+  uint32_t read_count = 0;          // Read operation counter
+};
+
+}  // namespace audio_tools

src/AudioTools/CoreAudio/AudioBasic/KalmanFilter.h

@@ -0,0 +1,127 @@
+#pragma once
+
+namespace audio_tools {
+
+/**
+ * @brief Simple 1D Kalman Filter for smoothing measurements.
+ *
+ * This class implements a basic one-dimensional Kalman filter for smoothing
+ * noisy measurements. It is useful for sensor data fusion and signal processing
+ * applications where noise reduction is required.
+ *
+ * The Kalman filter uses two main parameters:
+ *   - @b process_noise (Q): Represents the expected process variance. A typical
+ * default is 0.01.
+ *   - @b measurement_noise (R): Represents the expected measurement variance. A
+ * typical default is 1.0.
+ *
+ * Lower values for Q make the filter trust the model more (less responsive to
+ * changes), while higher values for R make the filter trust the measurements
+ * less (more smoothing).
+ *
+ * @note Reasonable defaults for many sensor applications are Q = 0.01 and R
+ * = 1.0, but these should be tuned for your specific use case.
+ * @ingroup filters
+ * @author Phil Schatzmann
+ */
+class KalmanFilter {
+ public:
+  /**
+   * @brief Construct a new Kalman Filter object
+   *
+   * @param process_noise The process noise covariance (Q). Default is 0.01.
+   * @param measurement_noise The measurement noise covariance (R). Default
+   * is 1.0.
+   */
+  KalmanFilter(float process_noise = 0.01f, float measurement_noise = 1.0f) {
+    begin(process_noise, measurement_noise);
+  }
+
+  /**
+   * @brief reset the filter with new parameters
+   *
+   * @param process_noise The process noise covariance (Q). Default is 0.01.
+   * @param measurement_noise The measurement noise covariance (R). Default
+   * is 1.0.
+   */
+
+  /**
+   * @brief Initialize or reset the filter with new parameters.
+   *
+   * @param process_noise The process noise covariance (Q). Default is 0.01.
+   * @param measurement_noise The measurement noise covariance (R). Default
+   * is 1.0.
+   * @return true Always returns true.
+   */
+  bool begin(float process_noise, float measurement_noise) {
+    Q = process_noise;
+    R = measurement_noise;
+    P = 1.0;
+    X = 0.0;
+    return true;
+  }
+
+  /**
+   * @brief Reset the filter state to zero, keeping the current noise
+   * parameters.
+   *
+   * @return true Always returns true.
+   */
+  bool begin() {
+    X = 0;
+    return true;
+  }
+
+  /**
+   * @brief End or clear the filter (sets the estimate to zero).
+   */
+  void end() { X = 0; }
+
+  /**
+   * @brief Updates the filter with a new measurement and returns the filtered
+   * value.
+   *
+   * @param measurement The new measurement to be filtered
+   * @return float The updated (filtered) estimate
+   */
+  void addMeasurement(float measurement) {
+    // Prediction update
+    P = P + Q;
+
+    // Measurement update
+    K = P / (P + R);
+    X = X + K * (measurement - X);
+    P = (1 - K) * P;
+  }
+
+  /**
+   * @brief Returns the current estimated value.
+   *
+   * @return float The current estimate
+   */
+  float calculate() { return X; }
+
+ protected:
+  /**
+   * @brief Process noise covariance (Q)
+   */
+  float Q;
+  /**
+   * @brief Measurement noise covariance (R)
+   */
+  float R;
+  /**
+   * @brief Estimation error covariance (P)
+   */
+  float P;
+  /**
+   * @brief Estimated state (X)
+   */
+  float X;
+  /**
+   * @brief Kalman gain (K)
+   */
+  float K;
+};
+
+}  // namespace audio_tools

src/AudioTools/CoreAudio/AudioBasic/MovingAverage.h

@@ -17,14 +17,14 @@ class MovingAverage {
     setSize(size);
   }
 
-  void add(N value) {
+  void addMeasurement(N value) {
     if (this->values.size() == this->size) {
       this->values.pop_front();
     }
     this->values.push_back(value);
   }
 
-  float average() {
+  float calculate() {
     float sum = 0;
     for (int i = 0; i < this->values.size(); i++) {
       sum += this->values[i];