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"
26 #include <zita-resampler/vresampler.h>
31 using namespace std::chrono;
33 ResamplingQueue::ResamplingQueue(DeviceSpec device_spec, unsigned freq_in, unsigned freq_out, unsigned num_channels, double expected_delay_seconds)
34 : device_spec(device_spec), freq_in(freq_in), freq_out(freq_out), num_channels(num_channels),
35 current_estimated_freq_in(freq_in),
36 ratio(double(freq_out) / double(freq_in)), expected_delay(expected_delay_seconds * OUTPUT_FREQUENCY)
38 vresampler.setup(ratio, num_channels, /*hlen=*/32);
40 // Prime the resampler so there's no more delay.
41 vresampler.inp_count = vresampler.inpsize() / 2 - 1;
42 vresampler.out_count = 1048576;
43 vresampler.process ();
46 void ResamplingQueue::add_input_samples(steady_clock::time_point ts, const float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
48 if (num_samples == 0) {
52 assert(duration<double>(ts.time_since_epoch()).count() >= 0.0);
54 bool good_sample = (rate_adjustment_policy == ADJUST_RATE);
55 if (good_sample && a1.good_sample) {
59 a1.input_samples_received += num_samples;
60 a1.good_sample = good_sample;
61 if (a0.good_sample && a1.good_sample) {
62 current_estimated_freq_in = (a1.input_samples_received - a0.input_samples_received) / duration<double>(a1.ts - a0.ts).count();
63 assert(current_estimated_freq_in >= 0.0);
65 // Bound the frequency, so that a single wild result won't throw the filter off guard.
66 current_estimated_freq_in = min(current_estimated_freq_in, 1.2 * freq_in);
67 current_estimated_freq_in = max(current_estimated_freq_in, 0.8 * freq_in);
70 buffer.insert(buffer.end(), samples, samples + num_samples * num_channels);
73 bool ResamplingQueue::get_output_samples(steady_clock::time_point ts, float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
75 assert(num_samples > 0);
76 if (a1.input_samples_received == 0) {
77 // No data yet, just return zeros.
78 memset(samples, 0, num_samples * num_channels * sizeof(float));
82 // This can happen when we get dropped frames on the master card.
83 if (duration<double>(ts.time_since_epoch()).count() <= 0.0) {
84 rate_adjustment_policy = DO_NOT_ADJUST_RATE;
87 if (rate_adjustment_policy == ADJUST_RATE && (a0.good_sample || a1.good_sample)) {
88 // Estimate the current number of input samples produced at
89 // this instant in time, by extrapolating from the last known
90 // good point. Note that we could be extrapolating backward or
91 // forward, depending on the timing of the calls.
92 const InputPoint &base_point = a1.good_sample ? a1 : a0;
93 assert(duration<double>(base_point.ts.time_since_epoch()).count() >= 0.0);
95 // NOTE: Due to extrapolation, input_samples_received can
96 // actually go negative here the few first calls (ie., we asked
97 // about a timestamp where we hadn't actually started producing
98 // samples yet), but that is harmless.
99 const double input_samples_received = base_point.input_samples_received +
100 current_estimated_freq_in * duration<double>(ts - base_point.ts).count();
102 // Estimate the number of input samples _consumed_ after we've run the resampler.
103 const double input_samples_consumed = total_consumed_samples +
104 num_samples / (ratio * rcorr);
106 double actual_delay = input_samples_received - input_samples_consumed;
107 actual_delay += vresampler.inpdist(); // Delay in the resampler itself.
108 double err = actual_delay - expected_delay;
110 // Before the very first block, insert artificial delay based on our initial estimate,
111 // so that we don't need a long period to stabilize at the beginning.
113 int delay_samples_to_add = lrintf(-err);
114 for (ssize_t i = 0; i < delay_samples_to_add * num_channels; ++i) {
115 buffer.push_front(0.0f);
117 total_consumed_samples -= delay_samples_to_add; // Equivalent to increasing input_samples_received on a0 and a1.
118 err += delay_samples_to_add;
119 } else if (err > 0.0) {
120 int delay_samples_to_remove = min<int>(lrintf(err), buffer.size() / num_channels);
121 buffer.erase(buffer.begin(), buffer.begin() + delay_samples_to_remove * num_channels);
122 total_consumed_samples += delay_samples_to_remove;
123 err -= delay_samples_to_remove;
126 first_output = false;
128 // Compute loop filter coefficients for the two filters. We need to compute them
129 // every time, since they depend on the number of samples the user asked for.
131 // The loop bandwidth is at 0.02 Hz; our jitter is pretty large
132 // since none of the threads involved run at real-time priority.
133 // However, the first four seconds, we use a larger loop bandwidth (2 Hz),
134 // because there's a lot going on during startup, and thus the
135 // initial estimate might be tainted by jitter during that phase,
136 // and we want to converge faster.
138 // NOTE: The above logic might only hold during Nageru startup
139 // (we start ResamplingQueues also when we e.g. switch sound sources),
140 // but in general, a little bit of increased timing jitter is acceptable
141 // right after a setup change like this.
142 double loop_bandwidth_hz = (total_consumed_samples < 4 * freq_in) ? 0.2 : 0.02;
144 // Set filters. The first filter much wider than the first one (20x as wide).
145 double w = (2.0 * M_PI) * loop_bandwidth_hz * num_samples / freq_out;
146 double w0 = 1.0 - exp(-20.0 * w);
147 double w1 = w * 1.5 / num_samples / ratio;
150 // Filter <err> through the loop filter to find the correction ratio.
151 z1 += w0 * (w1 * err - z1);
152 z2 += w0 * (z1 - z2);
154 rcorr = 1.0 - z2 - z3;
155 if (rcorr > 1.05) rcorr = 1.05;
156 if (rcorr < 0.95) rcorr = 0.95;
157 assert(!isnan(rcorr));
158 vresampler.set_rratio(rcorr);
161 // Finally actually resample, producing exactly <num_samples> output samples.
162 vresampler.out_data = samples;
163 vresampler.out_count = num_samples;
164 while (vresampler.out_count > 0) {
165 if (buffer.empty()) {
166 // This should never happen unless delay is set way too low,
167 // or we're dropping a lot of data.
168 fprintf(stderr, "%s: PANIC: Out of input samples to resample, still need %d output samples! (correction factor is %f)\n",
169 spec_to_string(device_spec).c_str(), int(vresampler.out_count), rcorr);
170 memset(vresampler.out_data, 0, vresampler.out_count * num_channels * sizeof(float));
172 // Reset the loop filter.
179 size_t num_input_samples = sizeof(inbuf) / (sizeof(float) * num_channels);
180 if (num_input_samples * num_channels > buffer.size()) {
181 num_input_samples = buffer.size() / num_channels;
183 copy(buffer.begin(), buffer.begin() + num_input_samples * num_channels, inbuf);
185 vresampler.inp_count = num_input_samples;
186 vresampler.inp_data = inbuf;
188 int err = vresampler.process();
191 size_t consumed_samples = num_input_samples - vresampler.inp_count;
192 total_consumed_samples += consumed_samples;
193 buffer.erase(buffer.begin(), buffer.begin() + consumed_samples * num_channels);