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 = 20000;
  46         catch_factor2 = 4000;
  47         pclamp = 150.0;
  48         controlquant = 10000.0;
  49         fir_empty = false;
  50 }
  51
  52 PIController::~PIController ()
  53 {
  54         delete [] offset_array;
  55         delete [] window_array;
  56 }
  57
  58 double
  59 PIController::get_ratio (int fill_level)
  60 {
  61         double offset = fill_level;
  62         double this_catch_factor = catch_factor;
  63
  64
  65         // Save offset.
  66         if( fir_empty ) {
  67             for (int i = 0; i < smooth_size; i++) {
  68                     offset_array[i] = offset;
  69             }
  70             fir_empty = false;
  71         } else {
  72             offset_array[(offset_differential_index++) % smooth_size] = offset;
  73         }
  74
  75         // Build the mean of the windowed offset array basically fir lowpassing.
  76         smooth_offset = 0.0;
  77         for (int i = 0; i < smooth_size; i++) {
  78                 smooth_offset += offset_array[(i + offset_differential_index - 1) % smooth_size] * window_array[i];
  79         }
  80         smooth_offset /= double(smooth_size);
  81
  82         // This is the integral of the smoothed_offset
  83         offset_integral += smooth_offset;
  84
  85         std::cerr << smooth_offset << " ";
  86
  87         // Clamp offset : the smooth offset still contains unwanted noise which would go straigth onto the resample coeff.
  88         // It only used in the P component and the I component is used for the fine tuning anyways.
  89
  90         if (fabs(smooth_offset) < pclamp)
  91                 smooth_offset = 0.0;
  92
  93         smooth_offset += (static_resample_factor - resample_mean) * this_catch_factor;
  94
  95         // Ok, now this is the PI controller.
  96         // u(t) = K * (e(t) + 1/T \int e(t') dt')
  97         // Kp = 1/catch_factor and T = catch_factor2  Ki = Kp/T
  98         current_resample_factor
  99                 = static_resample_factor - smooth_offset / this_catch_factor - offset_integral / this_catch_factor / catch_factor2;
 100
 101         // Now quantize this value around resample_mean, so that the noise which is in the integral component doesnt hurt.
 102         current_resample_factor = floor((current_resample_factor - resample_mean) * controlquant + 0.5) / controlquant + resample_mean;
 103
 104         // Calculate resample_mean so we can init ourselves to saner values.
 105         // resample_mean = 0.9999 * resample_mean + 0.0001 * current_resample_factor;
 106         resample_mean = (1.0-0.01) * resample_mean + 0.01 * current_resample_factor;
 107         std::cerr << fill_level << " " << smooth_offset << " " << offset_integral << " " << current_resample_factor << " " << resample_mean << "\n";
 108         return current_resample_factor;
 109 }
 110
 111 void
 112 PIController::out_of_bounds()
 113 {
 114         int i;
 115         // Set the resample_rate... we need to adjust the offset integral, to do this.
 116         // first look at the PI controller, this code is just a special case, which should never execute once
 117         // everything is swung in.
 118         offset_integral = - (resample_mean - static_resample_factor) * catch_factor * catch_factor2;
 119         // Also clear the array. we are beginning a new control cycle.
 120         for (i = 0; i < smooth_size; i++) {
 121                 offset_array[i] = 0.0;
 122         }
 123         fir_empty = false;
 124 }
 125
 126
 127 PIChaser::PIChaser() {
 128         pic = new PIController( 1.0, 16 );
 129         array_index = 0;
 130         for( int i=0; i<ESTIMATOR_SIZE; i++ ) {
 131             realtime_stamps[i] = 0;
 132             chasetime_stamps[i] = 0;
 133         }
 134
 135         speed_threshold = 0.2;
 136         pos_threshold = 4000;
 137         want_locate_val = 0;
 138 }
 139
 140 void
 141 PIChaser::reset() {
 142         array_index = 0;
 143         for( int i=0; i<ESTIMATOR_SIZE; i++ ) {
 144             realtime_stamps[i] = 0;
 145             chasetime_stamps[i] = 0;
 146         }
 147         pic->reset(1.0);
 148 }
 149 PIChaser::~PIChaser() {
 150         delete pic;
 151 }
 152
 153 double
 154 PIChaser::get_ratio(nframes64_t realtime, nframes64_t chasetime, nframes64_t slavetime, bool in_control ) {
 155
 156         feed_estimator( realtime, chasetime );
 157         std::cerr << (double)realtime/48000.0 << " " << chasetime << " " << slavetime << " ";
 158         double crude = get_estimate();
 159         double fine;
 160
 161             fine = pic->get_ratio( slavetime - chasetime );
 162         if (in_control) {
 163             if (fabs(fine-crude) > crude*speed_threshold) {
 164                 std::cout << "reset to " << crude << " fine = " << fine << "\n";
 165                 pic->reset( crude );
 166                 speed = crude;
 167             } else {
 168                 speed = fine;
 169             }
 170
 171             if (abs(chasetime-slavetime) > pos_threshold) {
 172                 pic->reset( crude );
 173                 speed = crude;
 174                 want_locate_val = chasetime;
 175                 std::cout << "we are off by " << chasetime-slavetime << " want_locate:" << chasetime << "\n";
 176             } else {
 177                 want_locate_val = 0;
 178             }
 179         } else {
 180             std::cout << "not in control..." << crude << "\n";
 181             speed = crude;
 182             pic->reset( crude );
 183         }
 184
 185         return speed;
 186 }
 187
 188 void
 189 PIChaser::feed_estimator( nframes64_t realtime, nframes64_t chasetime ) {
 190         array_index += 1;
 191         realtime_stamps [ array_index%ESTIMATOR_SIZE ] = realtime;
 192         chasetime_stamps[ array_index%ESTIMATOR_SIZE ] = chasetime;
 193 }
 194
 195 double
 196 PIChaser::get_estimate() {
 197         double est = 0;
 198         int num=0;
 199         int i;
 200         nframes64_t n1_realtime;
 201         nframes64_t n1_chasetime;
 202         for( i=(array_index + 1); i<=(array_index + ESTIMATOR_SIZE); i++ ) {
 203             if( realtime_stamps[(i)%ESTIMATOR_SIZE] ) {
 204                 n1_realtime = realtime_stamps[(i)%ESTIMATOR_SIZE];
 205                 n1_chasetime = chasetime_stamps[(i)%ESTIMATOR_SIZE];
 206                 i+=1;
 207                 break;
 208             }
 209         }
 210
 211         for( ; i<=(array_index + ESTIMATOR_SIZE); i++ ) {
 212             if( realtime_stamps[(i)%ESTIMATOR_SIZE] ) {
 213                 if( (realtime_stamps[(i)%ESTIMATOR_SIZE] - n1_realtime) > 200 ) {
 214                     nframes64_t n_realtime = realtime_stamps[(i)%ESTIMATOR_SIZE];
 215                     nframes64_t n_chasetime = chasetime_stamps[(i)%ESTIMATOR_SIZE];
 216                     est += ((double)( n_chasetime - n1_chasetime ))
 217                           / ((double)( n_realtime - n1_realtime ));
 218                     n1_realtime = n_realtime;
 219                     n1_chasetime = n_chasetime;
 220                     num += 1;
 221                 }
 222             }
 223         }
 224
 225         if(num)
 226             return est/(double)num;
 227         else
 228             return 0.0;
 229 }