1 // Parts of the code is adapted from Adriaensen's project Zita-ajbridge
2 // (as of November 2015), although it has been heavily reworked for this use
3 // case. Original copyright follows:
5 // Copyright (C) 2012-2015 Fons Adriaensen <fons@linuxaudio.org>
7 // This program is free software; you can redistribute it and/or modify
8 // it under the terms of the GNU General Public License as published by
9 // the Free Software Foundation; either version 3 of the License, or
10 // (at your option) any later version.
12 // This program is distributed in the hope that it will be useful,
13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15 // GNU General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program. If not, see <http://www.gnu.org/licenses/>.
20 #include "resampling_queue.h"
27 #include <zita-resampler/vresampler.h>
29 ResamplingQueue::ResamplingQueue(unsigned card_num, unsigned freq_in, unsigned freq_out, unsigned num_channels)
30 : card_num(card_num), freq_in(freq_in), freq_out(freq_out), num_channels(num_channels),
31 ratio(double(freq_out) / double(freq_in))
33 vresampler.setup(ratio, num_channels, /*hlen=*/32);
35 // Prime the resampler so there's no more delay.
36 vresampler.inp_count = vresampler.inpsize() / 2 - 1;
37 vresampler.out_count = 1048576;
38 vresampler.process ();
41 void ResamplingQueue::add_input_samples(double pts, const float *samples, ssize_t num_samples)
43 if (num_samples == 0) {
47 // Synthesize a fake length.
48 last_input_len = double(num_samples) / freq_in;
51 last_input_len = pts - last_input_pts;
59 buffer.insert(buffer.end(), samples, samples + num_samples * num_channels);
62 bool ResamplingQueue::get_output_samples(double pts, float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
64 assert(num_samples > 0);
66 // No data yet, just return zeros.
67 memset(samples, 0, num_samples * num_channels * sizeof(float));
72 if (rate_adjustment_policy == ADJUST_RATE) {
73 double last_output_len;
75 // Synthesize a fake length.
76 last_output_len = double(num_samples) / freq_out;
78 last_output_len = pts - last_output_pts;
80 last_output_pts = pts;
82 // Using the time point since just before the last call to add_input_samples() as a base,
83 // estimate actual delay based on activity since then, measured in number of input samples:
84 double actual_delay = 0.0;
85 assert(last_input_len != 0);
86 actual_delay += (k_a1 - k_a0) * last_output_len / last_input_len; // Inserted samples since k_a0, rescaled for the different time periods.
87 actual_delay += k_a0 - total_consumed_samples; // Samples inserted before k_a0 but not consumed yet.
88 actual_delay += vresampler.inpdist(); // Delay in the resampler itself.
89 double err = actual_delay - expected_delay;
90 if (first_output && err < 0.0) {
91 // Before the very first block, insert artificial delay based on our initial estimate,
92 // so that we don't need a long period to stabilize at the beginning.
93 int delay_samples_to_add = lrintf(-err);
94 for (ssize_t i = 0; i < delay_samples_to_add * num_channels; ++i) {
95 buffer.push_front(0.0f);
97 total_consumed_samples -= delay_samples_to_add; // Equivalent to increasing k_a0 and k_a1.
98 err += delay_samples_to_add;
100 first_output = false;
102 // Compute loop filter coefficients for the two filters. We need to compute them
103 // every time, since they depend on the number of samples the user asked for.
105 // The loop bandwidth is at 0.02 Hz; we trust the initial estimate quite well,
106 // and our jitter is pretty large since none of the threads involved run at
107 // real-time priority.
108 double loop_bandwidth_hz = 0.02;
110 // Set filters. The first filter much wider than the first one (20x as wide).
111 double w = (2.0 * M_PI) * loop_bandwidth_hz * num_samples / freq_out;
112 double w0 = 1.0 - exp(-20.0 * w);
113 double w1 = w * 1.5 / num_samples / ratio;
116 // Filter <err> through the loop filter to find the correction ratio.
117 z1 += w0 * (w1 * err - z1);
118 z2 += w0 * (z1 - z2);
120 rcorr = 1.0 - z2 - z3;
121 if (rcorr > 1.05) rcorr = 1.05;
122 if (rcorr < 0.95) rcorr = 0.95;
123 assert(!isnan(rcorr));
124 vresampler.set_rratio(rcorr);
126 assert(rate_adjustment_policy == DO_NOT_ADJUST_RATE);
129 // Finally actually resample, consuming exactly <num_samples> output samples.
130 vresampler.out_data = samples;
131 vresampler.out_count = num_samples;
132 while (vresampler.out_count > 0) {
133 if (buffer.empty()) {
134 // This should never happen unless delay is set way too low,
135 // or we're dropping a lot of data.
136 fprintf(stderr, "Card %u: PANIC: Out of input samples to resample, still need %d output samples! (correction factor is %f)\n",
137 card_num, int(vresampler.out_count), rcorr);
138 memset(vresampler.out_data, 0, vresampler.out_count * num_channels * sizeof(float));
143 size_t num_input_samples = sizeof(inbuf) / (sizeof(float) * num_channels);
144 if (num_input_samples * num_channels > buffer.size()) {
145 num_input_samples = buffer.size() / num_channels;
147 copy(buffer.begin(), buffer.begin() + num_input_samples * num_channels, inbuf);
149 vresampler.inp_count = num_input_samples;
150 vresampler.inp_data = inbuf;
152 int err = vresampler.process();
155 size_t consumed_samples = num_input_samples - vresampler.inp_count;
156 total_consumed_samples += consumed_samples;
157 buffer.erase(buffer.begin(), buffer.begin() + consumed_samples * num_channels);