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
  32 ResamplingQueue::ResamplingQueue(unsigned card_num, unsigned freq_in, unsigned freq_out, unsigned num_channels)
  33         : card_num(card_num), freq_in(freq_in), freq_out(freq_out), num_channels(num_channels),
  34           ratio(double(freq_out) / double(freq_in))
  35 {
  36         vresampler.setup(ratio, num_channels, /*hlen=*/32);
  37
  38         // Prime the resampler so there's no more delay.
  39         vresampler.inp_count = vresampler.inpsize() / 2 - 1;
  40         vresampler.out_count = 1048576;
  41         vresampler.process ();
  42 }
  43
  44 void ResamplingQueue::add_input_samples(double pts, const float *samples, ssize_t num_samples)
  45 {
  46         if (num_samples == 0) {
  47                 return;
  48         }
  49         if (first_input) {
  50                 // Synthesize a fake length.
  51                 last_input_len = double(num_samples) / freq_in;
  52                 first_input = false;
  53         } else {
  54                 last_input_len = pts - last_input_pts;
  55         }
  56
  57         last_input_pts = pts;
  58
  59         k_a0 = k_a1;
  60         k_a1 += num_samples;
  61
  62         buffer.insert(buffer.end(), samples, samples + num_samples * num_channels);
  63 }
  64
  65 bool ResamplingQueue::get_output_samples(double pts, float *samples, ssize_t num_samples, ResamplingQueue::RateAdjustmentPolicy rate_adjustment_policy)
  66 {
  67         assert(num_samples > 0);
  68         if (first_input) {
  69                 // No data yet, just return zeros.
  70                 memset(samples, 0, num_samples * num_channels * sizeof(float));
  71                 return true;
  72         }
  73
  74         double rcorr = -1.0;
  75         if (rate_adjustment_policy == ADJUST_RATE) {
  76                 double last_output_len;
  77                 if (first_output) {
  78                         // Synthesize a fake length.
  79                         last_output_len = double(num_samples) / freq_out;
  80                 } else {
  81                         last_output_len = pts - last_output_pts;
  82                 }
  83                 last_output_pts = pts;
  84
  85                 // Using the time point since just before the last call to add_input_samples() as a base,
  86                 // estimate actual delay based on activity since then, measured in number of input samples:
  87                 double actual_delay = 0.0;
  88                 assert(last_input_len != 0);
  89                 actual_delay += (k_a1 - k_a0) * last_output_len / last_input_len;    // Inserted samples since k_a0, rescaled for the different time periods.
  90                 actual_delay += k_a0 - total_consumed_samples;                       // Samples inserted before k_a0 but not consumed yet.
  91                 actual_delay += vresampler.inpdist();                                // Delay in the resampler itself.
  92                 double err = actual_delay - expected_delay;
  93                 if (first_output && err < 0.0) {
  94                         // Before the very first block, insert artificial delay based on our initial estimate,
  95                         // so that we don't need a long period to stabilize at the beginning.
  96                         int delay_samples_to_add = lrintf(-err);
  97                         for (ssize_t i = 0; i < delay_samples_to_add * num_channels; ++i) {
  98                                 buffer.push_front(0.0f);
  99                         }
 100                         total_consumed_samples -= delay_samples_to_add;  // Equivalent to increasing k_a0 and k_a1.
 101                         err += delay_samples_to_add;
 102                 }
 103                 first_output = false;
 104
 105                 // Compute loop filter coefficients for the two filters. We need to compute them
 106                 // every time, since they depend on the number of samples the user asked for.
 107                 //
 108                 // The loop bandwidth is at 0.02 Hz; we trust the initial estimate quite well,
 109                 // and our jitter is pretty large since none of the threads involved run at
 110                 // real-time priority.
 111                 double loop_bandwidth_hz = 0.02;
 112
 113                 // Set filters. The first filter much wider than the first one (20x as wide).
 114                 double w = (2.0 * M_PI) * loop_bandwidth_hz * num_samples / freq_out;
 115                 double w0 = 1.0 - exp(-20.0 * w);
 116                 double w1 = w * 1.5 / num_samples / ratio;
 117                 double w2 = w / 1.5;
 118
 119                 // Filter <err> through the loop filter to find the correction ratio.
 120                 z1 += w0 * (w1 * err - z1);
 121                 z2 += w0 * (z1 - z2);
 122                 z3 += w2 * z2;
 123                 rcorr = 1.0 - z2 - z3;
 124                 if (rcorr > 1.05) rcorr = 1.05;
 125                 if (rcorr < 0.95) rcorr = 0.95;
 126                 assert(!isnan(rcorr));
 127                 vresampler.set_rratio(rcorr);
 128         } else {
 129                 assert(rate_adjustment_policy == DO_NOT_ADJUST_RATE);
 130         };
 131
 132         // Finally actually resample, consuming exactly <num_samples> output samples.
 133         vresampler.out_data = samples;
 134         vresampler.out_count = num_samples;
 135         while (vresampler.out_count > 0) {
 136                 if (buffer.empty()) {
 137                         // This should never happen unless delay is set way too low,
 138                         // or we're dropping a lot of data.
 139                         fprintf(stderr, "Card %u: PANIC: Out of input samples to resample, still need %d output samples! (correction factor is %f)\n",
 140                                 card_num, int(vresampler.out_count), rcorr);
 141                         memset(vresampler.out_data, 0, vresampler.out_count * num_channels * sizeof(float));
 142                         return false;
 143                 }
 144
 145                 float inbuf[1024];
 146                 size_t num_input_samples = sizeof(inbuf) / (sizeof(float) * num_channels);
 147                 if (num_input_samples * num_channels > buffer.size()) {
 148                         num_input_samples = buffer.size() / num_channels;
 149                 }
 150                 copy(buffer.begin(), buffer.begin() + num_input_samples * num_channels, inbuf);
 151
 152                 vresampler.inp_count = num_input_samples;
 153                 vresampler.inp_data = inbuf;
 154
 155                 int err = vresampler.process();
 156                 assert(err == 0);
 157
 158                 size_t consumed_samples = num_input_samples - vresampler.inp_count;
 159                 total_consumed_samples += consumed_samples;
 160                 buffer.erase(buffer.begin(), buffer.begin() + consumed_samples * num_channels);
 161         }
 162         return true;
 163 }