libs/ardour/pi_controller.cc

   1 /*
   2   Copyright (C) 2008 Torben Hohn
   3
   4   This program is free software; you can redistribute it and/or modify
   5   it under the terms of the GNU General Public License as published by
   6   the Free Software Foundation; either version 2 of the License, or
   7   (at your option) any later version.
   8
   9   This program is distributed in the hope that it will be useful,
  10   but WITHOUT ANY WARRANTY; without even the implied warranty of
  11   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12   GNU General Public License for more details.
  13
  14   You should have received a copy of the GNU General Public License
  15   along with this program; if not, write to the Free Software
  16   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  17 */
  18
  19 #include <iostream>
  20 #include <cmath>
  21 #include <cstdlib>
  22
  23 #include "ardour/pi_controller.h"
  24
  25 static inline double hann(double x) {
  26         return 0.5 * (1.0 - cos(2 * M_PI * x));
  27 }
  28
  29 PIController::PIController (double resample_factor, int fir_size)
  30 {
  31         resample_mean = resample_factor;
  32         static_resample_factor = resample_factor;
  33         offset_array = new double[fir_size];
  34         window_array = new double[fir_size];
  35         offset_differential_index = 0;
  36         offset_integral = 0.0;
  37         smooth_size = fir_size;
  38
  39         for (int i = 0; i < fir_size; i++) {
  40                 offset_array[i] = 0.0;
  41                 window_array[i] = hann(double(i) / (double(fir_size) - 1.0));
  42         }
  43
  44         // These values could be configurable
  45         catch_factor = 100000;
  46         catch_factor2 = 10000;
  47         pclamp = 15.0;
  48         controlquant = 10000.0;
  49 }
  50
  51 PIController::~PIController ()
  52 {
  53         delete [] offset_array;
  54         delete [] window_array;
  55 }
  56
  57 double
  58 PIController::get_ratio (int fill_level)
  59 {
  60         double offset = fill_level;
  61
  62         // Save offset.
  63         offset_array[(offset_differential_index++) % smooth_size] = offset;
  64
  65         // Build the mean of the windowed offset array basically fir lowpassing.
  66         smooth_offset = 0.0;
  67         for (int i = 0; i < smooth_size; i++) {
  68                 smooth_offset += offset_array[(i + offset_differential_index - 1) % smooth_size] * window_array[i];
  69         }
  70         smooth_offset /= double(smooth_size);
  71
  72         // This is the integral of the smoothed_offset
  73         offset_integral += smooth_offset;
  74
  75
  76         // Clamp offset : the smooth offset still contains unwanted noise which would go straigth onto the resample coeff.
  77         // It only used in the P component and the I component is used for the fine tuning anyways.
  78         if (fabs(smooth_offset) < pclamp)
  79                 smooth_offset = 0.0;
  80
  81         // Ok, now this is the PI controller.
  82         // u(t) = K * (e(t) + 1/T \int e(t') dt')
  83         // Kp = 1/catch_factor and T = catch_factor2  Ki = Kp/T
  84         current_resample_factor
  85                 = static_resample_factor - smooth_offset / catch_factor - offset_integral / catch_factor / catch_factor2;
  86
  87         // Now quantize this value around resample_mean, so that the noise which is in the integral component doesnt hurt.
  88         current_resample_factor = floor((current_resample_factor - resample_mean) * controlquant + 0.5) / controlquant + resample_mean;
  89
  90         // Calculate resample_mean so we can init ourselves to saner values.
  91         // resample_mean = 0.9999 * resample_mean + 0.0001 * current_resample_factor;
  92         resample_mean = 0.9 * resample_mean + 0.1 * current_resample_factor;
  93         return current_resample_factor;
  94 }
  95
  96 void
  97 PIController::out_of_bounds()
  98 {
  99         int i;
 100         // Set the resample_rate... we need to adjust the offset integral, to do this.
 101         // first look at the PI controller, this code is just a special case, which should never execute once
 102         // everything is swung in.
 103         offset_integral = - (resample_mean - static_resample_factor) * catch_factor * catch_factor2;
 104         // Also clear the array. we are beginning a new control cycle.
 105         for (i = 0; i < smooth_size; i++) {
 106                 offset_array[i] = 0.0;
 107         }
 108 }