MightyPork/tangara-fw
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Resampling *basically* working? Just cleanup and buffering issues

Browse Source
This commit is contained in:
jacqueline
2023-08-07 15:20:12 +10:00
parent 4118d880c3
commit a66c342806
7 changed files with 183 additions and 237 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/audio/i2s_audio_output.cpp
+4 -3
View File
@@ -117,18 +117,19 @@ auto I2SAudioOutput::AdjustVolumeDown() -> bool {
auto I2SAudioOutput::PrepareFormat(const StreamInfo::Pcm& orig)
-> StreamInfo::Pcm {
/*
return StreamInfo::Pcm{
.channels = std::min<uint8_t>(orig.channels, 2),
.bits_per_sample = std::clamp<uint8_t>(orig.bits_per_sample, 16, 32),
.sample_rate = std::clamp<uint32_t>(orig.sample_rate, 8000, 96000),
};
*/
/*
return StreamInfo::Pcm{
.channels = std::min<uint8_t>(orig.channels, 2),
.bits_per_sample = 16,
.sample_rate = 44100,
//.sample_rate = std::clamp<uint32_t>(orig.sample_rate, 8000, 96000),
.sample_rate = 32000,
};
*/
}
auto I2SAudioOutput::Configure(const StreamInfo::Pcm& pcm) -> void {
src/audio/include/audio_sink.hpp
-1
View File
@@ -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_;
src/audio/include/resample.hpp
+9 -6
View File
@@ -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<size_t,size_t>;
private:
auto ApplyDither(cpp::span<sample::Sample>) -> void;
auto Subsample(int channel) -> float;
auto ApplyFilter(cpp::span<float> filter, cpp::span<float> input) -> float;
uint32_t source_sample_rate_;
uint32_t target_sample_rate_;
uint32_t factor_;
float factor_;
uint8_t num_channels_;
std::vector<Channel> channels_;
std::vector<float*> channel_buffers_;
size_t channel_buffer_size_;
float output_offset_;
int32_t input_index_;
};
} // namespace audio
src/audio/resample.cpp
+141 -216
View File
@@ -4,6 +4,7 @@
#include <stdlib.h>
#include <string.h>
#include <algorithm>
#include <cmath>
#include <numeric>
#include "esp_log.h"
@@ -13,215 +14,42 @@
namespace audio {
static constexpr size_t kFilterSize = 1536;
static constexpr char kTag[] = "resample";
constexpr auto calc_deltas(const std::array<int32_t, kFilterSize>& filter)
-> std::array<int32_t, kFilterSize> {
std::array<int32_t, kFilterSize> 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<int32_t, kFilterSize> kFilter{
#include "fir.h"
};
typedef std::array<float, kTapsPerFilter> Filter;
static std::array<Filter, kNumFilters + 1> sFilters{};
static bool sFiltersInitialised = false;
static const std::array<int32_t, kFilterSize> 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<sample::Sample> out) -> size_t;
auto AddSample(sample::Sample, cpp::span<sample::Sample> 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<int32_t> 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;
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;
}
Channel::~Channel() {
delete sample_buffer_.data();
}
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<int64_t>(*--sample) * a;
}
// 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<int64_t>(*sample++) * a;
}
/* scale */
value >>= 2;
value *= unity_scale_;
value >>= 27;
return sample::Clip(value);
}
auto Channel::FlushSamples(cpp::span<sample::Sample> 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--;
}
return produced;
}
auto Channel::AddSample(sample::Sample in, cpp::span<sample::Sample> out)
-> size_t {
// Add the latest sample to our working buffer.
sample_buffer_[latest_sample_++] = in;
// 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;
}
// 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_;
}
// 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;
} else {
latest_sample_ = new_current_sample;
}
}
return samples_output;
}
static const size_t kChunkSizeSamples = 256;
auto InitFilter(int index) -> void;
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_() {
factor_(static_cast<double>(target_sample_rate) /
static_cast<double>(source_sample_rate)),
num_channels_(num_channels) {
channel_buffers_.resize(num_channels);
channel_buffer_size_ = kTapsPerFilter * 16;
for (int i = 0; i < num_channels; i++) {
channels_.emplace_back(source_sample_rate, target_sample_rate,
kChunkSizeSamples, num_channels);
channel_buffers_[i] =
static_cast<float*>(calloc(sizeof(float), channel_buffer_size_));
}
output_offset_ = kTapsPerFilter / 2.0f;
input_index_ = kTapsPerFilter;
if (!sFiltersInitialised) {
sFiltersInitialised = true;
for (int i = 0; i < kNumFilters + 1; i++) {
InitFilter(i);
}
}
}
@@ -231,31 +59,128 @@ auto Resampler::Process(cpp::span<const sample::Sample> input,
cpp::span<sample::Sample> output,
bool end_of_data) -> std::pair<size_t, size_t> {
size_t samples_used = 0;
std::vector<size_t> samples_produced = {};
samples_produced.resize(num_channels_, 0);
size_t total_samples_produced = 0;
size_t samples_produced = 0;
size_t slop = (factor_ >> kPhaseBits) + 1;
size_t input_frames = input.size() / num_channels_;
size_t output_frames = output.size() / num_channels_;
uint_fast8_t cur_channel = 0;
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));
}
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;
output_offset_ -= channel_buffer_size_ - kTapsPerFilter;
input_index_ -= channel_buffer_size_ - kTapsPerFilter;
}
// Generate the next samples
size_t new_samples = channels_[cur_channel].AddSample(
input[samples_used++], output.subspan(next_output_index));
for (i = 0; i < num_channels_; ++i) {
channel_buffers_[i][input_index_] =
sample::ToFloat(input[samples_used++]);
}
samples_produced[cur_channel] += new_samples;
total_samples_produced += new_samples;
input_index_++;
input_frames--;
} else
break;
} else {
for (i = 0; i < num_channels_; i++) {
output[samples_produced++] = sample::FromFloat(Subsample(i));
}
cur_channel = (cur_channel + 1) % num_channels_;
output_offset_ += (1.0f / factor_);
}
}
return {samples_used, total_samples_produced};
return {samples_used, samples_produced};
}
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;
double fraction =
static_cast<double>(index) / static_cast<double>(kNumFilters);
double filter_sum = 0.0;
// "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<double, kTapsPerFilter> 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);
// Blackman-Harris window
value *= a0 + a1 * cos(ratio) + a2 * cos(2 * ratio) + a3 * cos(3 * ratio);
} else {
value = 1.0;
}
working_buffer[i] = value;
filter_sum += value;
}
// filter should have unity DC gain
double scaler = 1.0 / filter_sum;
double error = 0.0;
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<double>(filter[i]) - working_buffer[i];
}
}
auto Resampler::Subsample(int channel) -> float {
float sum1, sum2;
cpp::span<float> source{channel_buffers_[channel], channel_buffer_size_};
int offset_integral = std::floor(output_offset_);
source = source.subspan(offset_integral);
float offset_fractional = output_offset_ - offset_integral;
int filter_index = offset_fractional * kNumFilters;
offset_fractional *= kNumFilters;
sum1 = ApplyFilter(sFilters[filter_index],
{source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter});
offset_fractional -= filter_index;
sum2 = ApplyFilter(sFilters[filter_index + 1],
{source.data() - kTapsPerFilter / 2 + 1, kTapsPerFilter});
return (sum2 * offset_fractional) + (sum1 * (1.0f - offset_fractional));
}
auto Resampler::ApplyFilter(cpp::span<float> filter, cpp::span<float> input)
-> float {
float sum = 0.0;
for (int i = 0; i < kTapsPerFilter; i++) {
sum += filter[i] * input[i];
}
return sum;
}
} // namespace audio
src/audio/sink_mixer.cpp
+12 -9
View File
@@ -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<tasks::Type::kMixer>([&]() { Main(); });
tasks::StartPersistent<tasks::Type::kMixer>(1, [&]() { Main(); });
}
SinkMixer::~SinkMixer() {
@@ -154,13 +157,13 @@ auto SinkMixer::HandleBytes() -> void {
cpp::span<sample::Sample> src =
output_source->data_as<sample::Sample>().first(
output_source->info().bytes_in_stream() / sizeof(sample::Sample));
cpp::span<int16_t> dest = output_source->data_as<int16_t>().first(
output_source->info().bytes_in_stream() / sizeof(int16_t));
cpp::span<int16_t> dest{reinterpret_cast<int16_t*>(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};
src/codecs/include/sample.hpp
+7 -1
View File
@@ -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<float>(INT32_MAX);
constexpr auto ToFloat(Sample src) -> float {
return src * kFactor;
}
} // namespace sample
src/tasks/tasks.hpp
+10 -1
View File
@@ -57,11 +57,20 @@ auto PersistentMain(void* fn) -> void;
template <Type t>
auto StartPersistent(const std::function<void(void)>& fn) -> void {
StaticTask_t* task_buffer = new StaticTask_t;
cpp::span<StackType_t> stack = AllocateStack<t>();
xTaskCreateStatic(&PersistentMain, Name<t>().c_str(),
stack.size(), new std::function<void(void)>(fn),
Priority<t>(), stack.data(), task_buffer);
}
template <Type t>
auto StartPersistent(BaseType_t core, const std::function<void(void)>& fn) -> void {
StaticTask_t* task_buffer = new StaticTask_t;
cpp::span<StackType_t> stack = AllocateStack<t>();
xTaskCreateStaticPinnedToCore(&PersistentMain, Name<t>().c_str(),
stack.size(), new std::function<void(void)>(fn),
Priority<t>(), stack.data(), task_buffer, 0);
Priority<t>(), stack.data(), task_buffer, core);
}
class Worker {