AdaptiveResamplingBuffer.h

Go to the documentation of this file.

#pragma once
 
#include <atomic>
 
#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 {
 
class AdaptiveResamplingBuffer : public BaseBuffer<uint8_t>,
                                  public AudioInfoSupport {
 public:
  AdaptiveResamplingBuffer() = default;
 
  AdaptiveResamplingBuffer(BaseBuffer<uint8_t>& buffer,
                            float stepRangePercent = 5.0f) {
    setBuffer(buffer);
    setStepRangePercent(stepRangePercent);
  }
 
  void setStartFillPercent(float percent) { start_fill_percent = percent; }
 
  // resample_stream internally stores a pointer to the sibling
  // queue_stream member (via setStream()). A copy would leave the copy's
  // resample_stream pointing at the original's queue_stream, silently
  // sharing/dangling state - so copying (and, since neither QueueStream
  // nor ResampleStream defines a real move, moving) is disabled.
  AdaptiveResamplingBuffer(const AdaptiveResamplingBuffer&) = delete;
  AdaptiveResamplingBuffer& operator=(const AdaptiveResamplingBuffer&) = delete;
 
  void setBuffer(BaseBuffer<uint8_t>& buffer) { p_buffer = &buffer; }
 
  void setAudioInfo(AudioInfo info) override {
    audio_info = info;
    // if we are already running, apply the change immediately
    if (is_active) resample_stream.setAudioInfo(audio_info);
  }
 
  AudioInfo audioInfo() override { return audio_info; }
 
  bool begin() {
    if (p_buffer == nullptr) return false;
    if (resample_range <= 0.0f) {
      LOGW(
          "resample_range is 0: call setStepRangePercent() before begin() "
          "to enable adaptive resampling");
    }
    queue_stream.setBuffer(*p_buffer);
    queue_stream.begin();
    resample_stream.setStream(queue_stream);
    resample_stream.begin(audio_info);
    // This is a jitter buffer for an endless/live stream: a momentarily
    // empty backing buffer is a transient underflow, not end of stream.
    resample_stream.transformationReader().setEofOnZeroReads(false);
    // readArray()/available() are typically driven from the same loop that
    // also has to keep a downstream time-critical consumer fed (e.g. a
    // StreamCopy pumping an I2S output with only a few ms of DMA buffer).
    // The default zero-read handling blocks in delay() - up to
    // MAX_ZERO_READ_COUNT times - waiting for more input on every
    // transient underflow; on that calling pattern, blocking here is what
    // starves the downstream consumer, not the resampling math itself. So
    // give up on a transient underflow immediately instead of stalling.
    resample_stream.transformationReader().setZeroReadDelay(0);
    last_time_ms = 0;
    is_active = true;
    is_primed = false;
    resetDriftCounters();
    // pid.calculate() returns a correction in [-resample_range, +resample_range]
    // which is subtracted from the feedforward center in recalculate().
    return pid.begin(1.0, resample_range, -resample_range, p, i, d);
  }
 
  void end() {
    is_active = false;
    queue_stream.end();
    resample_stream.end();
    recalc_count = 0;
    last_time_ms = 0;
    has_peek = false;
  }
 
  bool write(uint8_t data) override { return writeArray(&data, 1) == 1; }
 
  int writeArray(const uint8_t data[], int len) override {
    if (p_buffer == nullptr) return 0;
    int result = p_buffer->writeArray(data, len);
    total_bytes_written += result;
    recalculate();
    return result;
  }
 
  bool read(uint8_t& result) override { return readArray(&result, 1) == 1; }
 
  int readArray(uint8_t data[], int len) override {
    if (p_buffer == nullptr || len <= 0 || !checkPrimed()) return 0;
    int written = 0;
    if (has_peek) {
      data[written++] = peek_byte;
      has_peek = false;
    }
    if (written < len) {
      written += resample_stream.readBytes(data + written, len - written);
    }
    total_bytes_read += written;
    return written;
  }
 
  bool peek(uint8_t& result) override {
    if (!checkPrimed()) return false;
    if (!has_peek) {
      if (resample_stream.readBytes(&peek_byte, 1) != 1) return false;
      has_peek = true;
    }
    result = peek_byte;
    return true;
  }
 
  void reset() override {
    if (p_buffer != nullptr) p_buffer->reset();
    has_peek = false;
    pid.reset();
    kalman_filter.end();
    step_size = 1.0f;
    level_percent_smoothed = 0.0f;
    last_time_ms = 0;
    is_primed = false;
    resetDriftCounters();
    if (is_active) {
      resample_stream.end();
      resample_stream.begin(audio_info);
      resample_stream.setStepSize(step_size);
    }
  }
 
  int available() override {
    if (p_buffer == nullptr || !checkPrimed()) return 0;
    int raw_available = p_buffer->available();
    float estimate = step_size > 0.0f ? raw_available / step_size : raw_available;
    return (has_peek ? 1 : 0) + (int)estimate;
  }
 
  int availableForWrite() override {
    if (p_buffer == nullptr) return 0;
    return p_buffer->availableForWrite();
  }
 
  uint8_t* address() override { return nullptr; }
 
  size_t size() override { return p_buffer == nullptr ? 0 : p_buffer->size(); }
 
  float levelPercent() override {
    if (p_buffer == nullptr) return 0.0f;
    return p_buffer->levelPercent();
  }
 
  float recalculate() {
    if (p_buffer == nullptr) return step_size;
 
    // Throttle: writeArray() calls us on every write (e.g. once per
    // received USB packet, possibly ~1ms apart). Actually redoing the
    // PID/Kalman math that often ties the PID's dt - and therefore its
    // derivative term - to the caller's write cadence rather than a
    // deliberate control-loop interval, so a single write-sized jump in
    // the buffer fill level can dominate/saturate the derivative term.
    // Skip the recomputation (and its float math) until enough real time
    // has passed; in between, callers keep getting the last computed
    // step size, which is already applied to resample_stream.
    uint32_t now_ms = millis();
    if (min_recalc_interval_ms > 0 && last_time_ms != 0 &&
        (uint32_t)(now_ms - last_time_ms) < min_recalc_interval_ms) {
      return step_size;
    }
 
    // determine the actual elapsed time so the PID's integral/derivative
    // terms don't depend on how often/how large the caller's writes are
    float dt = last_time_ms == 0 ? 1.0f : (now_ms - last_time_ms) / 1000.0f;
    if (dt <= 0.0f) dt = 0.001f;
    pid.setDt(dt);
    last_time_ms = now_ms;
 
    // calculate new resampling step size
    level_percent_smoothed = p_buffer->levelPercent();
    kalman_filter.addMeasurement(level_percent_smoothed);
    // A larger step size makes the resampler consume the buffered input
    // faster, so a low fill level (correction > 0) must reduce the step
    // size, and a high fill level (correction < 0) must increase it.
    float correction = pid.calculate(50.0, kalman_filter.calculate());
    // Feedforward + feedback: center on the long-run average step size
    // implied by cumulative bytes written vs. read (a low-noise estimate
    // of genuine producer/consumer clock drift) and let the PID's
    // correction only handle short-term jitter around that center. This
    // is far more stable than centering on a fixed 1.0 and relying solely
    // on the PID to chase both drift and jitter from a noisy instantaneous
    // fill level.
    float new_step_size = averageStepSize() - correction;
    // averageStepSize() is only sanity-clamped to a generous [0.5, 2.0] (see
    // its docs) since it must be able to represent genuine long-run clock
    // drift; it is NOT bounded by resample_range. Enforce the documented
    // setStepRangePercent() contract here, on the combined result, so a
    // skewed drift estimate (e.g. from a transient input underrun briefly
    // depressing total_bytes_read - see readArray()) can't push playback
    // speed - and therefore pitch - outside the range the caller asked for.
    float min_step = 1.0f - resample_range;
    float max_step = 1.0f + resample_range;
    if (new_step_size < min_step) new_step_size = min_step;
    if (new_step_size > max_step) new_step_size = max_step;
 
    // Slew-rate limit: bound how fast step_size is allowed to move per
    // second, regardless of how large the jump from the current value to
    // new_step_size is or how long dt happens to be. Without this, a
    // wide resample_range combined with an infrequent recalculation
    // cadence (setMinRecalculateInterval()) lets a single correction snap
    // to a far-off value and hold it for the whole interval - audible as
    // a sudden pitch jump/"crack" rather than a smooth bend. This makes
    // any correction, however large the target, ramp in gradually and
    // caps the worst-case audible glitch regardless of range/interval
    // settings.
    float current_step = step_size;
    float max_delta = max_step_change_per_sec * dt;
    float delta = new_step_size - current_step;
    if (delta > max_delta) delta = max_delta;
    if (delta < -max_delta) delta = -max_delta;
    step_size = current_step + delta;
 
    // log step size every 100th recalculation
    if (recalc_count++ % 100 == 0) {
      LOGI("step_size: %f", step_size.load());
    }
 
    resample_stream.setStepSize(step_size);
    return step_size;
  }
 
  float averageStepSize() {
    if (total_bytes_read < min_bytes_for_drift_estimate) return 1.0f;
    // Deliberately NOT netted against the buffer's current occupancy
    // (e.g. total_written - p_buffer->available()): that quantity is
    // itself the result of whatever step size the resampler already
    // applied, so feeding it back as the next step size's center creates
    // a circular, self-reinforcing loop - any startup transient gets
    // permanently amplified instead of forgotten. total_bytes_written and
    // total_bytes_read are each driven purely by the source/sink and
    // don't depend on our own past control output, so their ratio is the
    // step size that would have kept the buffer's fill level unchanged
    // over the window - a genuine, non-circular drift estimate. Residual
    // absolute fill-level error is exactly what the PID feedback term
    // already corrects for.
    float ratio = (float)total_bytes_written / (float)total_bytes_read;
    // safety net against a pathological/runaway estimate; real clock drift
    // is normally well under 1%, so this only guards against bugs/edge cases
    if (ratio < 0.5f) ratio = 0.5f;
    if (ratio > 2.0f) ratio = 2.0f;
    return ratio;
  }
 
  void setMinBytesForDriftEstimate(uint32_t bytes) {
    min_bytes_for_drift_estimate = bytes;
  }
 
  void setMinRecalculateInterval(uint32_t interval_ms) {
    min_recalc_interval_ms = interval_ms;
  }
 
  void setMaxStepChangeRate(float per_second) {
    max_step_change_per_sec = per_second;
  }
 
  uint64_t totalBytesWritten() { return total_bytes_written; }
 
  uint64_t totalBytesRead() { return total_bytes_read; }
 
  void setStepRangePercent(float rangePercent) {
    resample_range = rangePercent / 100.0;
  }
 
  float levelPercentSmoothed() { return level_percent_smoothed; }
 
  float stepSize() { return step_size; }
 
  bool isPrimed() { return checkPrimed(); }
 
  void setKalmanParameters(float process_noise, float measurement_noise) {
    kalman_filter.begin(process_noise, measurement_noise);
  }
 
  void setPIDParameters(float p_value, float i_value, float d_value) {
    p = p_value;
    i = i_value;
    d = d_value;
  }
 
 protected:
  void resetDriftCounters() {
    total_bytes_written = 0;
    total_bytes_read = 0;
  }
 
  bool checkPrimed() {
    if (is_primed) return true;
    if (start_fill_percent <= 0.0f) {
      is_primed = true;
    } else if (p_buffer != nullptr &&
               p_buffer->levelPercent() >= start_fill_percent) {
      is_primed = true;
    }
    if (is_primed) {
      // Priming accumulates writes with no matching reads, so the
      // resampler's first-ever read would have to work through that
      // whole backlog at once - permanently skewing the cumulative
      // written-vs-read ratio used by averageStepSize(). Start counting
      // only from the moment real (post-priming) traffic begins.
      resetDriftCounters();
    }
    return is_primed;
  }
 
  PIDController pid;                // PID controller for adaptive resampling step size
  QueueStream<uint8_t> queue_stream; // Internal queue stream for buffering audio data
  BaseBuffer<uint8_t>* p_buffer = nullptr; // Pointer to the user-provided raw buffer
  ResampleStream resample_stream;   // Resample stream for adjusting playback rate
  KalmanFilter kalman_filter{0.01f, 0.1f}; // Kalman filter for smoothing buffer fill level
  AudioInfo audio_info;              // Audio format of the buffered data
  std::atomic<float> step_size{1.0f}; // Current resampling step size (see class docs on thread-safety)
  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_smoothed = 0.0; // Last calculated (Kalman-smoothed) fill level (percent)
  uint32_t recalc_count = 0;        // recalculate() call counter (used to throttle logging)
  uint32_t last_time_ms = 0;        // Timestamp of the last actual recalculation, for PID dt
  uint32_t min_recalc_interval_ms = 0; // 0 = recalculate on every write (default/prior behavior)
  float max_step_change_per_sec = 0.2f; // max allowed |d(step_size)/dt|, in step units/sec
  bool is_active = false;           // true between begin() and end()
  uint8_t peek_byte = 0;            // one-byte lookahead cache for peek()
  bool has_peek = false;            // true if peek_byte holds an unconsumed byte
  float start_fill_percent = 50.0f; // required fill level before priming completes (0 = disabled)
  bool is_primed = false;           // true once the start fill level has been reached once
  uint64_t total_bytes_written = 0; // cumulative raw bytes written since counters were last reset
  uint64_t total_bytes_read = 0;    // cumulative resampled bytes delivered since counters were last reset
  uint32_t min_bytes_for_drift_estimate = 4096; // warm-up threshold for averageStepSize()
};
 
}  // namespace audio_tools