Sandbox: DynamicResamplingQueueStream

Author: pschatzmann

src/AudioTools/AudioLibs/PIDController.h

@@ -13,28 +13,27 @@ namespace audio_tools {
  */
 
 class PIDController {
-public:
+ public:
   // dt -  loop interval time
   // max - maximum value of manipulated variable
   // min - minimum value of manipulated variable
   // kp -  proportional gain
   // ki -  Integral gain
   // kd -  derivative gain
-  void begin(float dt, float max, float min, float kp, float kd,
-                float ki) {
+  bool begin(float dt, float max, float min, float kp, float ki, float kd) {
     this->dt = dt;
     this->max = max;
     this->min = min;
     this->kp = kp;
     this->kd = kd;
     this->ki = ki;
+    return true;
   }
 
-
-  // setpoint = desired process value
-  // pv = current process value: return new process value
+  // target = desired process value
+  // measured = current process value:
+  // returns new process value
   float calculate(float target, float measured) {
-
     // Calculate errori
     float error = target - measured;
 
@@ -46,8 +45,8 @@ class PIDController {
     float Iout = ki * integral;
 
     // Derivative term
-    assert(dt!=0.0);
-    
+    assert(dt != 0.0);
+
     float derivative = (error - preerror) / dt;
     float dout = kd * derivative;
 
@@ -66,7 +65,7 @@ class PIDController {
     return output;
   }
 
-protected:
+ protected:
   float dt = 1.0f;
   float max = 0.0f;
   float min = 0.0f;
@@ -76,6 +75,6 @@ class PIDController {
   float preerror = 0.0f;
   float integral = 0.0f;
 
-}; // namespace audiotools
+};  // namespace audiotools
 
-} // namespace audiotools
+}  // namespace audio_tools

src/AudioTools/CoreAudio/AudioBasic/MovingAverage.h

@@ -13,13 +13,11 @@ namespace audio_tools {
 template <class N>
 class MovingAverage {
  public:
-  MovingAverage(int size) {
-    this->size = size;
-    this->values = List<float>();
-    this->values.reserve(size);
+  MovingAverage(size_t size) {
+    setSize(size);
   }
 
-  void add(float value) {
+  void add(N value) {
     if (this->values.size() == this->size) {
       this->values.pop_front();
     }
@@ -34,9 +32,14 @@ class MovingAverage {
     return sum / this->values.size();
   }
 
+  /// Defines the number of values
+  void setSize(size_t size) {
+    this->size = size;
+  }
+
  protected:
   List<N> values;
-  int size;
+  size_t size = 0;;
 };
 
 }  // namespace audio_tools

src/AudioTools/CoreAudio/BaseStream.h

@@ -307,6 +307,8 @@ class NullStream : public BaseStream {
 template <class T>
 class QueueStream : public BaseStream {
  public:
+  /// Empty Constructor: call setBuffer() to set the buffer
+  QueueStream() = default;
   /// Default constructor
   QueueStream(int bufferSize, int bufferCount,
               bool autoRemoveOldestDataIfFull = false) {
@@ -316,8 +318,7 @@ class QueueStream : public BaseStream {
   }
   /// Create stream from any BaseBuffer subclass
   QueueStream(BaseBuffer<T> &buffer) {
-    owns_buffer = false;
-    callback_buffer_ptr = &buffer;
+    setBuffer(buffer);
   }
 
   virtual ~QueueStream() {
@@ -326,6 +327,11 @@ class QueueStream : public BaseStream {
     }
   }
 
+  void setBuffer(BaseBuffer<T> &buffer){
+    owns_buffer = false;
+    callback_buffer_ptr = &buffer;
+  }
+
   /// Activates the output
   virtual bool begin() override {
     TRACED();

src/AudioTools/Sandbox/DynamicResamplingQueueStream.h

@@ -0,0 +1,87 @@
+#pragma once
+
+#include "AudioTools/AudioLibs/PIDController.h"
+#include "AudioTools/CoreAudio/AudioBasic/MovingAverage.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 task to write and read
+ * the data. Also make sure that you protect the access with a mutex or provide
+ * a thread save buffer!
+ * @ingroup buffers
+ * @author Phil Schatzmann
+ */
+class DynamicResamplingQueueStream : public AudioStream {
+ public:
+  DynamicResamplingQueueStream(BaseBuffer<uint8_t>& buffer,
+                               float stepRangePercent = 0.05) {
+    p_buffer = &buffer;
+    setStepRangePercent(stepRangePercent);
+    addNotifyAudioChange(resample_stream);
+  }
+
+  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);
+  }
+
+  /// Fill the buffer
+  size_t write(const uint8_t* data, size_t len) override {
+    if (p_buffer == 0) return 0;
+    return p_buffer->writeArray(data, len);
+  }
+
+  void end() {
+    queue_stream.end();
+    resample_stream.end();
+  }
+
+  /// Read resampled data from the buffer
+  size_t readBytes(uint8_t* data, size_t len) override {
+    if (p_buffer->available() == 0) return 0;
+
+    // calculate new resampling step size
+    moving_average_level_percent.add(p_buffer->levelPercent());
+    step_size = pid.calculate(50.0, moving_average_level_percent.average());
+
+    // return resampled result
+    resample_stream.setStepSize(step_size);
+    return resample_stream.readBytes(data, len);
+  }
+
+  void setMovingAvgSize(int size) {
+    moving_average_level_percent.setSize(size);
+  }
+
+  /// e.g. a value of 0.0005 means that we allow to resample for 44100 by +- 22.05 from 44077.95 to 44122.05
+  void setStepRangePercent(float rangePercent){
+    resample_range = rangePercent / 100.0;
+  }
+
+ protected:
+  PIDController pid;  // p=0.005, i=0.00005, d=0.0001
+  QueueStream<uint8_t> queue_stream;
+  BaseBuffer<uint8_t>* p_buffer = nullptr;
+  MovingAverage<float> moving_average_level_percent{50};
+  ResampleStream resample_stream;
+  float step_size = 1.0;
+  float resample_range = 0;
+  int copy_size = 80;
+  int buffer_size = 0;
+  float p = 0.005;
+  float i = 0.00005;
+  float d = 0.0001;
+};
+
+}  // namespace audio_tools