git.sesse.net Git - nageru/blob - resampling_queue.cpp

   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:
   4 //
   5 //  Copyright (C) 2012-2015 Fons Adriaensen <fons@linuxaudio.org>
   6 //
   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.
  11 //
  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.
  16 //
  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/>.
  19
  20 #include "resampling_queue.h"
  21
  22 #include <assert.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <string.h>
  26 #include <zita-resampler/vresampler.h>
  27 #include <algorithm>
  28 #include <cmath>
  29
  30 using namespace std;
  31 using namespace std::chrono;
  32
  33 ResamplingQueue::ResamplingQueue(unsigned card_num, unsigned freq_in, unsigned freq_out, unsigned num_channels, double expected_delay_seconds)
  34         : card_num(card_num), 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)
  37 {
  38         vresampler.setup(ratio, num_channels, /*hlen=*/32);
  39
  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 ();
  44 }
  45
  46 void ResamplingQueue::add_input_samples(steady_clock::time_point ts, const float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
  47 {
  48         if (num_samples == 0) {
  49                 return;
  50         }
  51
  52         assert(duration<double>(ts.time_since_epoch()).count() >= 0.0);
  53
  54         bool good_sample = (rate_adjustment_policy == ADJUST_RATE);
  55         if (good_sample && a1.good_sample) {
  56                 a0 = a1;
  57         }
  58         a1.ts = ts;
  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);
  64
  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);
  68         }
  69
  70         buffer.insert(buffer.end(), samples, samples + num_samples * num_channels);
  71 }
  72
  73 bool ResamplingQueue::get_output_samples(steady_clock::time_point ts, float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
  74 {
  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));
  79                 return true;
  80         }
  81
  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;
  85         }
  86
  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);
  94                 const double input_samples_received = base_point.input_samples_received +
  95                         current_estimated_freq_in * duration<double>(ts - base_point.ts).count();
  96                 assert(input_samples_received >= 0.0);
  97
  98                 // Estimate the number of input samples _consumed_ after we've run the resampler.
  99                 const double input_samples_consumed = total_consumed_samples +
 100                         num_samples / (ratio * rcorr);
 101
 102                 double actual_delay = input_samples_received - input_samples_consumed;
 103                 actual_delay += vresampler.inpdist();    // Delay in the resampler itself.
 104                 double err = actual_delay - expected_delay;
 105                 if (first_output) {
 106                         // Before the very first block, insert artificial delay based on our initial estimate,
 107                         // so that we don't need a long period to stabilize at the beginning.
 108                         if (err < 0.0) {
 109                                 int delay_samples_to_add = lrintf(-err);
 110                                 for (ssize_t i = 0; i < delay_samples_to_add * num_channels; ++i) {
 111                                         buffer.push_front(0.0f);
 112                                 }
 113                                 total_consumed_samples -= delay_samples_to_add;  // Equivalent to increasing input_samples_received on a0 and a1.
 114                                 err += delay_samples_to_add;
 115                         } else if (err > 0.0) {
 116                                 int delay_samples_to_remove = min<int>(lrintf(err), buffer.size() / num_channels);
 117                                 buffer.erase(buffer.begin(), buffer.begin() + delay_samples_to_remove * num_channels);
 118                                 total_consumed_samples += delay_samples_to_remove;
 119                                 err -= delay_samples_to_remove;
 120                         }
 121                 }
 122                 first_output = false;
 123
 124                 // Compute loop filter coefficients for the two filters. We need to compute them
 125                 // every time, since they depend on the number of samples the user asked for.
 126                 //
 127                 // The loop bandwidth is at 0.02 Hz; our jitter is pretty large
 128                 // since none of the threads involved run at real-time priority.
 129                 // However, the first four seconds, we use a larger loop bandwidth (2 Hz),
 130                 // because there's a lot going on during startup, and thus the
 131                 // initial estimate might be tainted by jitter during that phase,
 132                 // and we want to converge faster.
 133                 //
 134                 // NOTE: The above logic might only hold during Nageru startup
 135                 // (we start ResamplingQueues also when we e.g. switch sound sources),
 136                 // but in general, a little bit of increased timing jitter is acceptable
 137                 // right after a setup change like this.
 138                 double loop_bandwidth_hz = (total_consumed_samples < 4 * freq_in) ? 0.2 : 0.02;
 139
 140                 // Set filters. The first filter much wider than the first one (20x as wide).
 141                 double w = (2.0 * M_PI) * loop_bandwidth_hz * num_samples / freq_out;
 142                 double w0 = 1.0 - exp(-20.0 * w);
 143                 double w1 = w * 1.5 / num_samples / ratio;
 144                 double w2 = w / 1.5;
 145
 146                 // Filter <err> through the loop filter to find the correction ratio.
 147                 z1 += w0 * (w1 * err - z1);
 148                 z2 += w0 * (z1 - z2);
 149                 z3 += w2 * z2;
 150                 rcorr = 1.0 - z2 - z3;
 151                 if (rcorr > 1.05) rcorr = 1.05;
 152                 if (rcorr < 0.95) rcorr = 0.95;
 153                 assert(!isnan(rcorr));
 154                 vresampler.set_rratio(rcorr);
 155         }
 156
 157         // Finally actually resample, producing exactly <num_samples> output samples.
 158         vresampler.out_data = samples;
 159         vresampler.out_count = num_samples;
 160         while (vresampler.out_count > 0) {
 161                 if (buffer.empty()) {
 162                         // This should never happen unless delay is set way too low,
 163                         // or we're dropping a lot of data.
 164                         fprintf(stderr, "Card %u: PANIC: Out of input samples to resample, still need %d output samples! (correction factor is %f)\n",
 165                                 card_num, int(vresampler.out_count), rcorr);
 166                         memset(vresampler.out_data, 0, vresampler.out_count * num_channels * sizeof(float));
 167
 168                         // Reset the loop filter.
 169                         z1 = z2 = z3 = 0.0;
 170
 171                         return false;
 172                 }
 173
 174                 float inbuf[1024];
 175                 size_t num_input_samples = sizeof(inbuf) / (sizeof(float) * num_channels);
 176                 if (num_input_samples * num_channels > buffer.size()) {
 177                         num_input_samples = buffer.size() / num_channels;
 178                 }
 179                 copy(buffer.begin(), buffer.begin() + num_input_samples * num_channels, inbuf);
 180
 181                 vresampler.inp_count = num_input_samples;
 182                 vresampler.inp_data = inbuf;
 183
 184                 int err = vresampler.process();
 185                 assert(err == 0);
 186
 187                 size_t consumed_samples = num_input_samples - vresampler.inp_count;
 188                 total_consumed_samples += consumed_samples;
 189                 buffer.erase(buffer.begin(), buffer.begin() + consumed_samples * num_channels);
 190         }
 191         return true;
 192 }