common/JackFilters.h

   1 /*
   2 Copyright (C) 2008 Grame
   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
  20 #ifndef __JackFilters__
  21 #define __JackFilters__
  22
  23 #include "jack.h"
  24 #include "JackAtomicState.h"
  25 #include <math.h>
  26 #include <stdlib.h>
  27
  28 namespace Jack
  29 {
  30
  31     #define MAX_SIZE 64
  32
  33         struct JackFilter
  34     {
  35
  36         jack_time_t fTable[MAX_SIZE];
  37
  38         JackFilter()
  39         {
  40             for (int i = 0; i < MAX_SIZE; i++)
  41                 fTable[i] = 0;
  42         }
  43
  44         void AddValue(jack_time_t val)
  45         {
  46             memcpy(&fTable[1], &fTable[0], sizeof(jack_time_t) * (MAX_SIZE - 1));
  47             fTable[0] = val;
  48         }
  49
  50         jack_time_t GetVal()
  51         {
  52             jack_time_t mean = 0;
  53             for (int i = 0; i < MAX_SIZE; i++)
  54                 mean += fTable[i];
  55             return mean / MAX_SIZE;
  56         }
  57
  58     } POST_PACKED_STRUCTURE;
  59
  60     class JackDelayLockedLoop
  61     {
  62
  63         private:
  64
  65             jack_nframes_t fFrames;
  66             jack_time_t fCurrentWakeup;
  67             jack_time_t fCurrentCallback;
  68             jack_time_t fNextWakeUp;
  69             float fSecondOrderIntegrator;
  70             jack_nframes_t fBufferSize;
  71             jack_nframes_t fSampleRate;
  72             jack_time_t fPeriodUsecs;
  73             float fFilterCoefficient;   /* set once, never altered */
  74             bool fUpdating;
  75
  76         public:
  77
  78             JackDelayLockedLoop()
  79             {}
  80
  81             JackDelayLockedLoop(jack_nframes_t buffer_size, jack_nframes_t sample_rate)
  82             {
  83                 Init(buffer_size, sample_rate);
  84             }
  85
  86             void Init(jack_nframes_t buffer_size, jack_nframes_t sample_rate)
  87             {
  88                 fFrames = 0;
  89                 fCurrentWakeup = 0;
  90                 fCurrentCallback = 0;
  91                 fNextWakeUp = 0;
  92                 fFilterCoefficient = 0.01f;
  93                 fSecondOrderIntegrator = 0.0f;
  94                 fBufferSize = buffer_size;
  95                 fSampleRate = sample_rate;
  96                 fPeriodUsecs = jack_time_t(1000000.f / fSampleRate * fBufferSize);      // in microsec
  97             }
  98
  99             void Init(jack_time_t callback_usecs)
 100             {
 101                 fFrames = 0;
 102                 fCurrentWakeup = 0;
 103                 fSecondOrderIntegrator = 0.0f;
 104                 fCurrentCallback = callback_usecs;
 105                 fNextWakeUp = callback_usecs + fPeriodUsecs;
 106             }
 107
 108             void IncFrame(jack_time_t callback_usecs)
 109             {
 110                 float delta = (int64_t)callback_usecs - (int64_t)fNextWakeUp;
 111                 fCurrentWakeup = fNextWakeUp;
 112                 fCurrentCallback = callback_usecs;
 113                 fFrames += fBufferSize;
 114                 fSecondOrderIntegrator += 0.5f * fFilterCoefficient * delta;
 115                 fNextWakeUp = fCurrentWakeup + fPeriodUsecs + (int64_t) floorf((fFilterCoefficient * (delta + fSecondOrderIntegrator)));
 116             }
 117
 118             jack_nframes_t Time2Frames(jack_time_t time)
 119             {
 120                 long delta = (long) rint(((double) ((long long)(time - fCurrentWakeup)) / ((long long)(fNextWakeUp - fCurrentWakeup))) * fBufferSize);
 121                 return (delta < 0) ? ((fFrames > 0) ? fFrames : 1) : (fFrames + delta);
 122             }
 123
 124             jack_time_t Frames2Time(jack_nframes_t frames)
 125             {
 126                 long delta = (long) rint(((double) ((long long)(frames - fFrames)) * ((long long)(fNextWakeUp - fCurrentWakeup))) / fBufferSize);
 127                 return (delta < 0) ? ((fCurrentWakeup > 0) ? fCurrentWakeup : 1) : (fCurrentWakeup + delta);
 128             }
 129
 130             jack_nframes_t CurFrame()
 131             {
 132                 return fFrames;
 133             }
 134
 135             jack_time_t CurTime()
 136             {
 137                 return fCurrentWakeup;
 138             }
 139
 140     } POST_PACKED_STRUCTURE;
 141
 142     class JackAtomicDelayLockedLoop : public JackAtomicState<JackDelayLockedLoop>
 143     {
 144          public:
 145
 146             JackAtomicDelayLockedLoop(jack_nframes_t buffer_size, jack_nframes_t sample_rate)
 147             {
 148                 fState[0].Init(buffer_size, sample_rate);
 149                 fState[1].Init(buffer_size, sample_rate);
 150             }
 151
 152             void Init(jack_time_t callback_usecs)
 153             {
 154                 JackDelayLockedLoop* dll = WriteNextStateStart();
 155                 dll->Init(callback_usecs);
 156                 WriteNextStateStop();
 157                 TrySwitchState(); // always succeed since there is only one writer
 158             }
 159
 160             void Init(jack_nframes_t buffer_size, jack_nframes_t sample_rate)
 161             {
 162                 JackDelayLockedLoop* dll = WriteNextStateStart();
 163                 dll->Init(buffer_size, sample_rate);
 164                 WriteNextStateStop();
 165                 TrySwitchState(); // always succeed since there is only one writer
 166             }
 167
 168             void IncFrame(jack_time_t callback_usecs)
 169             {
 170                 JackDelayLockedLoop* dll = WriteNextStateStart();
 171                 dll->IncFrame(callback_usecs);
 172                 WriteNextStateStop();
 173                 TrySwitchState(); // always succeed since there is only one writer
 174             }
 175
 176             jack_nframes_t Time2Frames(jack_time_t time)
 177             {
 178                 UInt16 next_index = GetCurrentIndex();
 179                 UInt16 cur_index;
 180                 jack_nframes_t res;
 181
 182                 do {
 183                     cur_index = next_index;
 184                     res = ReadCurrentState()->Time2Frames(time);
 185                     next_index = GetCurrentIndex();
 186                 } while (cur_index != next_index); // Until a coherent state has been read
 187
 188                 return res;
 189             }
 190
 191             jack_time_t Frames2Time(jack_nframes_t frames)
 192             {
 193                 UInt16 next_index = GetCurrentIndex();
 194                 UInt16 cur_index;
 195                 jack_time_t res;
 196
 197                 do {
 198                     cur_index = next_index;
 199                     res = ReadCurrentState()->Frames2Time(frames);
 200                     next_index = GetCurrentIndex();
 201                 } while (cur_index != next_index); // Until a coherent state has been read
 202
 203                 return res;
 204             }
 205     } POST_PACKED_STRUCTURE;
 206
 207     /*
 208     Torben Hohn PI controler from JACK1
 209     */
 210
 211     struct JackPIControler {
 212
 213         double resample_mean;
 214         double static_resample_factor;
 215
 216         double* offset_array;
 217         double* window_array;
 218         int offset_differential_index;
 219
 220         double offset_integral;
 221
 222         double catch_factor;
 223         double catch_factor2;
 224         double pclamp;
 225         double controlquant;
 226         int smooth_size;
 227
 228         double hann(double x)
 229         {
 230             return 0.5 * (1.0 - cos(2 * M_PI * x));
 231         }
 232
 233         JackPIControler(double resample_factor, int fir_size)
 234         {
 235             resample_mean = resample_factor;
 236             static_resample_factor = resample_factor;
 237             offset_array = new double[fir_size];
 238             window_array = new double[fir_size];
 239             offset_differential_index = 0;
 240             offset_integral = 0.0;
 241             smooth_size = fir_size;
 242
 243             for (int i = 0; i < fir_size; i++) {
 244                 offset_array[i] = 0.0;
 245                 window_array[i] = hann(double(i) / (double(fir_size) - 1.0));
 246             }
 247
 248             // These values could be configurable
 249             catch_factor = 100000;
 250             catch_factor2 = 10000;
 251             pclamp = 15.0;
 252             controlquant = 10000.0;
 253         }
 254
 255         ~JackPIControler()
 256         {
 257             delete[] offset_array;
 258             delete[] window_array;
 259         }
 260
 261         void Init(double resample_factor)
 262         {
 263             resample_mean = resample_factor;
 264             static_resample_factor = resample_factor;
 265         }
 266
 267         /*
 268         double GetRatio(int fill_level)
 269         {
 270             double offset = fill_level;
 271
 272             // Save offset.
 273             offset_array[(offset_differential_index++) % smooth_size] = offset;
 274
 275             // Build the mean of the windowed offset array basically fir lowpassing.
 276             double smooth_offset = 0.0;
 277             for (int i = 0; i < smooth_size; i++) {
 278                 smooth_offset += offset_array[(i + offset_differential_index - 1) % smooth_size] * window_array[i];
 279             }
 280             smooth_offset /= double(smooth_size);
 281
 282             // This is the integral of the smoothed_offset
 283             offset_integral += smooth_offset;
 284
 285             // Clamp offset : the smooth offset still contains unwanted noise which would go straigth onto the resample coeff.
 286             // It only used in the P component and the I component is used for the fine tuning anyways.
 287             if (fabs(smooth_offset) < pclamp)
 288                 smooth_offset = 0.0;
 289
 290             // Ok, now this is the PI controller.
 291             // u(t) = K * (e(t) + 1/T \int e(t') dt')
 292             // Kp = 1/catch_factor and T = catch_factor2  Ki = Kp/T
 293             double current_resample_factor
 294                 = static_resample_factor - smooth_offset / catch_factor - offset_integral / catch_factor / catch_factor2;
 295
 296             // Now quantize this value around resample_mean, so that the noise which is in the integral component doesnt hurt.
 297             current_resample_factor = floor((current_resample_factor - resample_mean) * controlquant + 0.5) / controlquant + resample_mean;
 298
 299             // Calculate resample_mean so we can init ourselves to saner values.
 300             resample_mean = 0.9999 * resample_mean + 0.0001 * current_resample_factor;
 301             return current_resample_factor;
 302         }
 303         */
 304
 305         double GetRatio(int error)
 306         {
 307             double smooth_offset = error;
 308
 309             // This is the integral of the smoothed_offset
 310             offset_integral += smooth_offset;
 311
 312             // Ok, now this is the PI controller.
 313             // u(t) = K * (e(t) + 1/T \int e(t') dt')
 314             // Kp = 1/catch_factor and T = catch_factor2 Ki = Kp/T
 315             return static_resample_factor - smooth_offset/catch_factor - offset_integral/catch_factor/catch_factor2;
 316         }
 317
 318         void OurOfBounds()
 319         {
 320             int i;
 321             // Set the resample_rate... we need to adjust the offset integral, to do this.
 322             // first look at the PI controller, this code is just a special case, which should never execute once
 323             // everything is swung in.
 324             offset_integral = - (resample_mean - static_resample_factor) * catch_factor * catch_factor2;
 325             // Also clear the array. we are beginning a new control cycle.
 326             for (i = 0; i < smooth_size; i++) {
 327                 offset_array[i] = 0.0;
 328             }
 329         }
 330
 331     };
 332
 333 }
 334
 335 #endif