From a66c3428063017f2233b6b15d5ce6c920d5c9095 Mon Sep 17 00:00:00 2001 From: jacqueline Date: Mon, 7 Aug 2023 14:26:04 +1000 Subject: [PATCH] Resampling *basically* working? Just cleanup and buffering issues --- src/audio/i2s_audio_output.cpp | 7 +- src/audio/include/audio_sink.hpp | 1 - src/audio/include/resample.hpp | 15 +- src/audio/resample.cpp | 343 ++++++++++++------------------- src/audio/sink_mixer.cpp | 21 +- src/codecs/include/sample.hpp | 8 +- src/tasks/tasks.hpp | 11 +- 7 files changed, 176 insertions(+), 230 deletions(-) diff --git a/src/audio/i2s_audio_output.cpp b/src/audio/i2s_audio_output.cpp index 41640e7a..09dc1ce8 100644 --- a/src/audio/i2s_audio_output.cpp +++ b/src/audio/i2s_audio_output.cpp @@ -117,18 +117,19 @@ auto I2SAudioOutput::AdjustVolumeDown() -> bool { auto I2SAudioOutput::PrepareFormat(const StreamInfo::Pcm& orig) -> StreamInfo::Pcm { - /* return StreamInfo::Pcm{ .channels = std::min(orig.channels, 2), .bits_per_sample = std::clamp(orig.bits_per_sample, 16, 32), .sample_rate = std::clamp(orig.sample_rate, 8000, 96000), }; - */ + /* return StreamInfo::Pcm{ .channels = std::min(orig.channels, 2), .bits_per_sample = 16, - .sample_rate = 44100, + //.sample_rate = std::clamp(orig.sample_rate, 8000, 96000), + .sample_rate = 32000, }; + */ } auto I2SAudioOutput::Configure(const StreamInfo::Pcm& pcm) -> void { diff --git a/src/audio/include/audio_sink.hpp b/src/audio/include/audio_sink.hpp index 20bb6da6..28acdc31 100644 --- a/src/audio/include/audio_sink.hpp +++ b/src/audio/include/audio_sink.hpp @@ -17,7 +17,6 @@ namespace audio { class IAudioSink { private: - // TODO: tune. at least about 12KiB seems right for mp3 static const std::size_t kDrainBufferSize = 24 * 1024; StreamBufferHandle_t stream_; diff --git a/src/audio/include/resample.hpp b/src/audio/include/resample.hpp index d7933470..32b6fde8 100644 --- a/src/audio/include/resample.hpp +++ b/src/audio/include/resample.hpp @@ -9,8 +9,6 @@ namespace audio { -class Channel; - class Resampler { public: Resampler(uint32_t source_sample_rate, @@ -28,14 +26,19 @@ class Resampler { bool end_of_data) -> std::pair; private: - auto ApplyDither(cpp::span) -> void; + auto Subsample(int channel) -> float; + auto ApplyFilter(cpp::span filter, cpp::span input) -> float; uint32_t source_sample_rate_; uint32_t target_sample_rate_; - uint32_t factor_; - + float factor_; uint8_t num_channels_; - std::vector channels_; + + std::vector channel_buffers_; + size_t channel_buffer_size_; + + float output_offset_; + int32_t input_index_; }; } // namespace audio \ No newline at end of file diff --git a/src/audio/resample.cpp b/src/audio/resample.cpp index 6f7e670e..aa4c8f2a 100644 --- a/src/audio/resample.cpp +++ b/src/audio/resample.cpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include "esp_log.h" @@ -13,249 +14,173 @@ namespace audio { -static constexpr size_t kFilterSize = 1536; +static constexpr char kTag[] = "resample"; -constexpr auto calc_deltas(const std::array& filter) - -> std::array { - std::array deltas; - for (size_t n = 0; n < kFilterSize - 1; n++) - deltas[n] = filter[n + 1] - filter[n]; - return deltas; -} +static constexpr double kLowPassRatio = 0.5; +static constexpr size_t kNumFilters = 8; +static constexpr size_t kTapsPerFilter = 8; -static const std::array kFilter{ -#include "fir.h" -}; - -static const std::array kFilterDeltas = - calc_deltas(kFilter); - -class Channel { - public: - Channel(uint32_t src_rate, - uint32_t dest_rate, - size_t chunk_size, - size_t skip); - ~Channel(); - - auto output_chunk_size() -> size_t { return output_chunk_size_; } - - auto FlushSamples(cpp::span out) -> size_t; - auto AddSample(sample::Sample, cpp::span out) -> std::size_t; - auto ApplyFilter() -> sample::Sample; - - private: - size_t output_chunk_size_; - size_t skip_; - - uint32_t factor_; /* factor */ - - uint32_t time_; /* time */ - - uint32_t time_per_filter_iteration_; /* output step */ - uint32_t filter_step_; /* filter step */ - uint32_t filter_end_; /* filter end */ - - int32_t unity_scale_; /* unity scale */ - - int32_t samples_per_filter_wing_; /* extra samples */ - int32_t latest_sample_; /* buffer index */ - cpp::span sample_buffer_; /* the buffer */ -}; - -enum { - Nl = 8, /* 2^Nl samples per zero crossing in fir */ - Nη = 8, /* phase bits for filter interpolation */ - kPhaseBits = Nl + Nη, /* phase bits (fract of fixed point) */ - One = 1 << kPhaseBits, -}; - -Channel::Channel(uint32_t irate, uint32_t orate, size_t count, size_t skip) - : skip_(skip) { - factor_ = ((uint64_t)orate << kPhaseBits) / irate; - if (factor_ != One) { - time_per_filter_iteration_ = ((uint64_t)irate << kPhaseBits) / orate; - filter_step_ = 1 << (Nl + Nη); - filter_end_ = kFilterSize << Nη; - samples_per_filter_wing_ = 1 + (filter_end_ / filter_step_); - unity_scale_ = 13128; /* unity scale factor for fir */ - if (factor_ < One) { - unity_scale_ *= factor_; - unity_scale_ >>= kPhaseBits; - filter_step_ *= factor_; - filter_step_ >>= kPhaseBits; - samples_per_filter_wing_ *= time_per_filter_iteration_; - samples_per_filter_wing_ >>= kPhaseBits; - } - latest_sample_ = samples_per_filter_wing_; - time_ = latest_sample_ << kPhaseBits; +typedef std::array Filter; +static std::array sFilters{}; +static bool sFiltersInitialised = false; - size_t buf_size = samples_per_filter_wing_ * 2 + count; - int32_t* buf = new int32_t[buf_size]; - sample_buffer_ = {buf, buf_size}; - count += buf_size; /* account for buffer accumulation */ - } - output_chunk_size_ = ((uint64_t)count * factor_) >> kPhaseBits; -} +auto InitFilter(int index) -> void; -Channel::~Channel() { - delete sample_buffer_.data(); -} +Resampler::Resampler(uint32_t source_sample_rate, + uint32_t target_sample_rate, + uint8_t num_channels) + : source_sample_rate_(source_sample_rate), + target_sample_rate_(target_sample_rate), + factor_(static_cast(target_sample_rate) / + static_cast(source_sample_rate)), + num_channels_(num_channels) { + channel_buffers_.resize(num_channels); + channel_buffer_size_ = kTapsPerFilter * 16; -auto Channel::ApplyFilter() -> sample::Sample { - uint32_t iteration, p, i; - int32_t *sample, a; - - int64_t value = 0; - - // I did my best, but I'll be honest with you I've no idea about any of this - // maths stuff. - - // Left wing of the filter. - sample = &sample_buffer_[time_ >> kPhaseBits]; - p = time_ & ((1 << kPhaseBits) - 1); - iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p; - while (iteration < filter_end_) { - i = iteration >> Nη; - a = iteration & ((1 << Nη) - 1); - iteration += filter_step_; - a *= kFilterDeltas[i]; - a >>= Nη; - a += kFilter[i]; - value += static_cast(*--sample) * a; + for (int i = 0; i < num_channels; i++) { + channel_buffers_[i] = + static_cast(calloc(sizeof(float), channel_buffer_size_)); } - // Right wing of the filter. - sample = &sample_buffer_[time_ >> kPhaseBits]; - p = (One - p) & ((1 << kPhaseBits) - 1); - iteration = factor_ < One ? (factor_ * p) >> kPhaseBits : p; - if (p == 0) /* skip h[0] as it was already been summed above if p == 0 */ - iteration += filter_step_; - while (iteration < filter_end_) { - i = iteration >> Nη; - a = iteration & ((1 << Nη) - 1); - iteration += filter_step_; - a *= kFilterDeltas[i]; - a >>= Nη; - a += kFilter[i]; - value += static_cast(*sample++) * a; + output_offset_ = kTapsPerFilter / 2.0f; + input_index_ = kTapsPerFilter; + + if (!sFiltersInitialised) { + sFiltersInitialised = true; + for (int i = 0; i < kNumFilters + 1; i++) { + InitFilter(i); + } } +} - /* scale */ - value >>= 2; - value *= unity_scale_; - value >>= 27; +Resampler::~Resampler() {} - return sample::Clip(value); -} +auto Resampler::Process(cpp::span input, + cpp::span output, + bool end_of_data) -> std::pair { + size_t samples_used = 0; + size_t samples_produced = 0; + + size_t input_frames = input.size() / num_channels_; + size_t output_frames = output.size() / num_channels_; + + int half_taps = kTapsPerFilter / 2, i; + while (output_frames > 0) { + if (output_offset_ >= input_index_ - half_taps) { + if (input_frames > 0) { + if (input_index_ == channel_buffer_size_) { + for (i = 0; i < num_channels_; ++i) { + memmove(channel_buffers_[i], + channel_buffers_[i] + channel_buffer_size_ - kTapsPerFilter, + kTapsPerFilter * sizeof(float)); + } + + output_offset_ -= channel_buffer_size_ - kTapsPerFilter; + input_index_ -= channel_buffer_size_ - kTapsPerFilter; + } + + for (i = 0; i < num_channels_; ++i) { + channel_buffers_[i][input_index_] = + sample::ToFloat(input[samples_used++]); + } + + input_index_++; + input_frames--; + } else + break; + } else { + for (i = 0; i < num_channels_; i++) { + output[samples_produced++] = sample::FromFloat(Subsample(i)); + } -auto Channel::FlushSamples(cpp::span out) -> size_t { - size_t zeroes_needed = (2 * samples_per_filter_wing_) - latest_sample_; - size_t produced = 0; - while (zeroes_needed > 0) { - produced += AddSample(0, out.subspan(produced)); - zeroes_needed--; + output_offset_ += (1.0f / factor_); + } } - return produced; + + return {samples_used, samples_produced}; } -auto Channel::AddSample(sample::Sample in, cpp::span out) - -> size_t { - // Add the latest sample to our working buffer. - sample_buffer_[latest_sample_++] = in; +auto InitFilter(int index) -> void { + const double a0 = 0.35875; + const double a1 = 0.48829; + const double a2 = 0.14128; + const double a3 = 0.01168; - // If we don't have enough samples to run the filter, then bail out and wait - // for more. - if (latest_sample_ < 2 * samples_per_filter_wing_) { - return 0; - } + double fraction = + static_cast(index) / static_cast(kNumFilters); + double filter_sum = 0.0; - // Apply the filter to the buffered samples. First, we work out how long (in - // samples) we can run the filter for before running out. This isn't as - // trivial as it might look; e.g. depending on the resampling factor we might - // be doubling the number of samples, or halving them. - uint32_t max_time = (latest_sample_ - samples_per_filter_wing_) << kPhaseBits; - size_t samples_output = 0; - while (time_ < max_time) { - out[skip_ * samples_output++] = ApplyFilter(); - time_ += time_per_filter_iteration_; - } + // "dist" is the absolute distance from the sinc maximum to the filter tap to + // be calculated, in radians "ratio" is that distance divided by half the tap + // count such that it reaches π at the window extremes + + // Note that with this scaling, the odd terms of the Blackman-Harris + // calculation appear to be negated with respect to the reference formula + // version. + + Filter& filter = sFilters[index]; + std::array working_buffer{}; + for (int i = 0; i < kTapsPerFilter; ++i) { + double dist = fabs((kTapsPerFilter / 2.0 - 1.0) + fraction - i) * M_PI; + double ratio = dist / (kTapsPerFilter / 2.0); + double value; + + if (dist != 0.0) { + value = sin(dist * kLowPassRatio) / (dist * kLowPassRatio); - // If we are approaching the end of our buffer, we need to shift all the data - // in it down to the front to make room for more samples. - int32_t current_sample = time_ >> kPhaseBits; - if (current_sample >= (sample_buffer_.size() - samples_per_filter_wing_)) { - // NB: bit shifting back and forth means we're only modifying `time` by - // whole samples. - time_ -= current_sample << kPhaseBits; - time_ += samples_per_filter_wing_ << kPhaseBits; - - int32_t new_current_sample = time_ >> kPhaseBits; - new_current_sample -= samples_per_filter_wing_; - current_sample -= samples_per_filter_wing_; - - int32_t samples_to_move = latest_sample_ - current_sample; - if (samples_to_move > 0) { - auto samples = sample_buffer_.subspan(current_sample, samples_to_move); - std::copy_backward(samples.begin(), samples.end(), - sample_buffer_.first(new_current_sample).end()); - latest_sample_ = new_current_sample + samples_to_move; + // Blackman-Harris window + value *= a0 + a1 * cos(ratio) + a2 * cos(2 * ratio) + a3 * cos(3 * ratio); } else { - latest_sample_ = new_current_sample; + value = 1.0; } + + working_buffer[i] = value; + filter_sum += value; } - return samples_output; -} + // filter should have unity DC gain -static const size_t kChunkSizeSamples = 256; + double scaler = 1.0 / filter_sum; + double error = 0.0; -Resampler::Resampler(uint32_t source_sample_rate, - uint32_t target_sample_rate, - uint8_t num_channels) - : source_sample_rate_(source_sample_rate), - target_sample_rate_(target_sample_rate), - factor_(((uint64_t)target_sample_rate << kPhaseBits) / - source_sample_rate), - num_channels_(num_channels), - channels_() { - for (int i = 0; i < num_channels; i++) { - channels_.emplace_back(source_sample_rate, target_sample_rate, - kChunkSizeSamples, num_channels); + for (int i = kTapsPerFilter / 2; i < kTapsPerFilter; + i = kTapsPerFilter - i - (i >= kTapsPerFilter / 2)) { + working_buffer[i] *= scaler; + filter[i] = working_buffer[i] - error; + error += static_cast(filter[i]) - working_buffer[i]; } } -Resampler::~Resampler() {} +auto Resampler::Subsample(int channel) -> float { + float sum1, sum2; -auto Resampler::Process(cpp::span input, - cpp::span output, - bool end_of_data) -> std::pair { - size_t samples_used = 0; - std::vector samples_produced = {}; - samples_produced.resize(num_channels_, 0); - size_t total_samples_produced = 0; + cpp::span source{channel_buffers_[channel], channel_buffer_size_}; - size_t slop = (factor_ >> kPhaseBits) + 1; + int offset_integral = std::floor(output_offset_); + source = source.subspan(offset_integral); + float offset_fractional = output_offset_ - offset_integral; - uint_fast8_t cur_channel = 0; + int filter_index = offset_fractional * kNumFilters; + offset_fractional *= kNumFilters; - while (input.size() > samples_used && - output.size() > total_samples_produced + slop) { - // Work out where the next set of samples should be placed. - size_t next_output_index = - (samples_produced[cur_channel] * num_channels_) + cur_channel; + sum1 = ApplyFilter(sFilters[filter_index], + {source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter}); - // Generate the next samples - size_t new_samples = channels_[cur_channel].AddSample( - input[samples_used++], output.subspan(next_output_index)); + offset_fractional -= filter_index; - samples_produced[cur_channel] += new_samples; - total_samples_produced += new_samples; + sum2 = ApplyFilter(sFilters[filter_index + 1], + {source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter}); - cur_channel = (cur_channel + 1) % num_channels_; - } + return (sum2 * offset_fractional) + (sum1 * (1.0f - offset_fractional)); +} - return {samples_used, total_samples_produced}; +auto Resampler::ApplyFilter(cpp::span filter, cpp::span input) + -> float { + float sum = 0.0; + for (int i = 0; i < kTapsPerFilter; i++) { + sum += filter[i] * input[i]; + } + return sum; } } // namespace audio diff --git a/src/audio/sink_mixer.cpp b/src/audio/sink_mixer.cpp index 8a40fd63..176fb4a3 100644 --- a/src/audio/sink_mixer.cpp +++ b/src/audio/sink_mixer.cpp @@ -13,6 +13,7 @@ #include "esp_log.h" #include "freertos/portmacro.h" #include "freertos/projdefs.h" +#include "idf_additions.h" #include "resample.hpp" #include "sample.hpp" @@ -21,8 +22,8 @@ static constexpr char kTag[] = "mixer"; -static constexpr std::size_t kSourceBufferLength = 2 * 1024; -static constexpr std::size_t kSampleBufferLength = 2 * 1024; +static constexpr std::size_t kSourceBufferLength = 8 * 1024; +static constexpr std::size_t kSampleBufferLength = 240 * 2 * sizeof(int32_t); namespace audio { @@ -30,12 +31,14 @@ SinkMixer::SinkMixer(StreamBufferHandle_t dest) : commands_(xQueueCreate(1, sizeof(Args))), is_idle_(xSemaphoreCreateBinary()), resampler_(nullptr), - source_(xStreamBufferCreate(kSourceBufferLength, 1)), + source_(xStreamBufferCreateWithCaps(kSourceBufferLength, + 1, + MALLOC_CAP_SPIRAM)), sink_(dest) { - input_stream_.reset(new RawStream(kSampleBufferLength, MALLOC_CAP_SPIRAM)); - resampled_stream_.reset(new RawStream(kSampleBufferLength, MALLOC_CAP_SPIRAM)); + input_stream_.reset(new RawStream(kSampleBufferLength)); + resampled_stream_.reset(new RawStream(kSampleBufferLength)); - tasks::StartPersistent([&]() { Main(); }); + tasks::StartPersistent(1, [&]() { Main(); }); } SinkMixer::~SinkMixer() { @@ -154,13 +157,13 @@ auto SinkMixer::HandleBytes() -> void { cpp::span src = output_source->data_as().first( output_source->info().bytes_in_stream() / sizeof(sample::Sample)); - cpp::span dest = output_source->data_as().first( - output_source->info().bytes_in_stream() / sizeof(int16_t)); + cpp::span dest{reinterpret_cast(src.data()), + src.size()}; ApplyDither(src, 16); Downscale(src, dest); - output_source->info().bytes_in_stream() /= 2; + output_source->info().bytes_in_stream() = dest.size_bytes(); } InputStream output{output_source}; diff --git a/src/codecs/include/sample.hpp b/src/codecs/include/sample.hpp index 7209673b..f8e08cdc 100644 --- a/src/codecs/include/sample.hpp +++ b/src/codecs/include/sample.hpp @@ -52,8 +52,14 @@ constexpr auto FromMad(mad_fixed_t src) -> Sample { return FromSigned(src >> (MAD_F_FRACBITS + 1 - 24), 24); } -constexpr auto ToSigned16Bit(Sample src) -> uint16_t { +constexpr auto ToSigned16Bit(Sample src) -> int16_t { return src >> 16; } +static constexpr float kFactor = 1.0f / static_cast(INT32_MAX); + +constexpr auto ToFloat(Sample src) -> float { + return src * kFactor; +} + } // namespace sample diff --git a/src/tasks/tasks.hpp b/src/tasks/tasks.hpp index 1321aab8..fe65ffcc 100644 --- a/src/tasks/tasks.hpp +++ b/src/tasks/tasks.hpp @@ -57,11 +57,20 @@ auto PersistentMain(void* fn) -> void; template auto StartPersistent(const std::function& fn) -> void { + StaticTask_t* task_buffer = new StaticTask_t; + cpp::span stack = AllocateStack(); + xTaskCreateStatic(&PersistentMain, Name().c_str(), + stack.size(), new std::function(fn), + Priority(), stack.data(), task_buffer); +} + +template +auto StartPersistent(BaseType_t core, const std::function& fn) -> void { StaticTask_t* task_buffer = new StaticTask_t; cpp::span stack = AllocateStack(); xTaskCreateStaticPinnedToCore(&PersistentMain, Name().c_str(), stack.size(), new std::function(fn), - Priority(), stack.data(), task_buffer, 0); + Priority(), stack.data(), task_buffer, core); } class Worker {