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

   1 // Parts of the code is adapted from Adriaensen's project Zita-ajbridge,
   2 // although it has been heavily reworked for this use case. Original copyright follows:
   3 //
   4 //  Copyright (C) 2012-2015 Fons Adriaensen <fons@linuxaudio.org>
   5 //
   6 //  This program is free software; you can redistribute it and/or modify
   7 //  it under the terms of the GNU General Public License as published by
   8 //  the Free Software Foundation; either version 3 of the License, or
   9 //  (at your option) any later version.
  10 //
  11 //  This program is distributed in the hope that it will be useful,
  12 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 //  GNU General Public License for more details.
  15 //
  16 //  You should have received a copy of the GNU General Public License
  17 //  along with this program.  If not, see <http://www.gnu.org/licenses/>.
  18
  19 #include "resampler.h"
  20
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <math.h>
  24 #include <zita-resampler/vresampler.h>
  25
  26 Resampler::Resampler(unsigned freq_in, unsigned freq_out, unsigned num_channels)
  27         : freq_in(freq_in), freq_out(freq_out), num_channels(num_channels),
  28           ratio(double(freq_out) / double(freq_in))
  29 {
  30         vresampler.setup(ratio, num_channels, /*hlen=*/32);
  31
  32         // Prime the resampler so there's no more delay.
  33         vresampler.inp_count = vresampler.inpsize() / 2 - 1;
  34         vresampler.out_count = 1048576;
  35         vresampler.process ();
  36 }
  37
  38 void Resampler::add_input_samples(double pts, const float *samples, ssize_t num_samples)
  39 {
  40         if (first_input) {
  41                 // Synthesize a fake length.
  42                 last_input_len = double(num_samples) / freq_in;
  43                 first_input = false;
  44         } else {
  45                 last_input_len = pts - last_input_pts;
  46         }
  47
  48         last_input_pts = pts;
  49
  50         k_a0 = k_a1;
  51         k_a1 += num_samples;
  52
  53         for (ssize_t i = 0; i < num_samples * num_channels; ++i) {
  54                 buffer.push_back(samples[i]);
  55         }
  56 }
  57
  58 void Resampler::get_output_samples(double pts, float *samples, ssize_t num_samples)
  59 {
  60         double last_output_len;
  61         if (first_output) {
  62                 // Synthesize a fake length.
  63                 last_output_len = double(num_samples) / freq_out;
  64         } else {
  65                 last_output_len = pts - last_output_pts;
  66         }
  67         last_output_pts = pts;
  68
  69         // Using the time point since just before the last call to add_input_samples() as a base,
  70         // estimate actual delay based on activity since then, measured in number of input samples:
  71         double actual_delay = 0.0;
  72         actual_delay += (k_a1 - k_a0) * last_output_len / last_input_len;    // Inserted samples since k_a0, rescaled for the different time periods.
  73         actual_delay += k_a0 - total_consumed_samples;                       // Samples inserted before k_a0 but not consumed yet.
  74         actual_delay += vresampler.inpdist();                                // Delay in the resampler itself.
  75         double err = actual_delay - expected_delay;
  76         if (first_output && err < 0.0) {
  77                 // Before the very first block, insert artificial delay based on our initial estimate,
  78                 // so that we don't need a long period to stabilize at the beginning.
  79                 int delay_samples_to_add = lrintf(-err);
  80                 for (ssize_t i = 0; i < delay_samples_to_add * num_channels; ++i) {
  81                         buffer.push_front(0.0f);
  82                 }
  83                 total_consumed_samples -= delay_samples_to_add;  // Equivalent to increasing k_a0 and k_a1.
  84                 err += delay_samples_to_add;
  85                 first_output = false;
  86         }
  87
  88         // Compute loop filter coefficients for the two filters. We need to compute them
  89         // every time, since they depend on the number of samples the user asked for.
  90         //
  91         // The loop bandwidth starts at 1.0 Hz, then goes down to 0.05 Hz after four seconds.
  92         double loop_bandwidth_hz = (k_a0 < 4 * freq_in) ? 1.0 : 0.05;
  93
  94         // Set filters. The first filter much wider than the first one (20x as wide).
  95         double w = (2.0 * M_PI) * loop_bandwidth_hz * num_samples / freq_out;
  96         double w0 = 1.0 - exp(-20.0 * w);
  97         double w1 = w * 1.5 / num_samples / ratio;
  98         double w2 = w / 1.5;
  99
 100         // Filter <err> through the loop filter to find the correction ratio.
 101         z1 += w0 * (w1 * err - z1);
 102         z2 += w0 * (z1 - z2);
 103         z3 += w2 * z2;
 104         double rcorr = 1.0 - z2 - z3;
 105         if (rcorr > 1.05) rcorr = 1.05;
 106         if (rcorr < 0.95) rcorr = 0.95;
 107         vresampler.set_rratio(rcorr);
 108
 109         // Finally actually resample, consuming exactly <num_samples> output samples.
 110         vresampler.out_data = samples;
 111         vresampler.out_count = num_samples;
 112         while (vresampler.out_count > 0) {
 113                 if (buffer.empty()) {
 114                         // This should never happen unless delay is set way too low,
 115                         // or we're dropping a lot of data.
 116                         fprintf(stderr, "PANIC: Out of input samples to resample, still need %d output samples!\n",
 117                                 int(vresampler.out_count));
 118                         memset(vresampler.out_data, 0, vresampler.out_count * sizeof(float));
 119                         break;
 120                 }
 121
 122                 float inbuf[1024];
 123                 size_t num_input_samples = sizeof(inbuf) / (sizeof(float) * num_channels);
 124                 if (num_input_samples * num_channels > buffer.size()) {
 125                         num_input_samples = buffer.size() / num_channels;
 126                 }
 127                 for (size_t i = 0; i < num_input_samples * num_channels; ++i) {
 128                         inbuf[i] = buffer[i];
 129                 }
 130
 131                 vresampler.inp_count = num_input_samples;
 132                 vresampler.inp_data = inbuf;
 133
 134                 vresampler.process();
 135
 136                 size_t consumed_samples = num_input_samples - vresampler.inp_count;
 137                 total_consumed_samples += consumed_samples;
 138                 buffer.erase(buffer.begin(), buffer.begin() + consumed_samples * num_channels);
 139         }
 140 }