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