code.delx.au - pulseaudio/blob - src/pulsecore/time-smoother.c

   1 /* $Id$ */
   2
   3 /***
   4   This file is part of PulseAudio.
   5
   6   Copyright 2007 Lennart Poettering
   7
   8   PulseAudio is free software; you can redistribute it and/or modify
   9   it under the terms of the GNU Lesser General Public License as
  10   published by the Free Software Foundation; either version 2.1 of the
  11   License, or (at your option) any later version.
  12
  13   PulseAudio is distributed in the hope that it will be useful, but
  14   WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  16   Lesser General Public License for more details.
  17
  18   You should have received a copy of the GNU Lesser General Public
  19   License along with PulseAudio; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
  21   USA.
  22 ***/
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29
  30 #include <pulse/sample.h>
  31 #include <pulse/xmalloc.h>
  32
  33 #include <pulsecore/macro.h>
  34
  35 #include "time-smoother.h"
  36
  37 #define HISTORY_MAX 100
  38
  39 /*
  40  * Implementation of a time smoothing algorithm to synchronize remote
  41  * clocks to a local one. Evens out noise, adjusts to clock skew and
  42  * allows cheap estimations of the remote time while clock updates may
  43  * be seldom and recieved in non-equidistant intervals.
  44  *
  45  * Basically, we estimate the gradient of received clock samples in a
  46  * certain history window (of size 'history_time') with linear
  47  * regression. With that info we estimate the remote time in
  48  * 'adjust_time' ahead and smoothen our current estimation function
  49  * towards that point with a 3rd order polynomial interpolation with
  50  * fitting derivatives. (more or less a b-spline)
  51  *
  52  * The larger 'history_time' is chosen the better we will surpress
  53  * noise -- but we'll adjust to clock skew slower..
  54  *
  55  * The larger 'adjust_time' is chosen the smoother our estimation
  56  * function will be -- but we'll adjust to clock skew slower, too.
  57  *
  58  * If 'monotonic' is TRUE the resulting estimation function is
  59  * guaranteed to be monotonic.
  60  */
  61
  62 struct pa_smoother {
  63     pa_usec_t adjust_time, history_time;
  64     pa_bool_t monotonic;
  65
  66     pa_usec_t px, py;     /* Point p, where we want to reach stability */
  67     double dp;            /* Gradient we want at point p */
  68
  69     pa_usec_t ex, ey;     /* Point e, which we estimated before and need to smooth to */
  70     double de;            /* Gradient we estimated for point e */
  71
  72                           /* History of last measurements */
  73     pa_usec_t history_x[HISTORY_MAX], history_y[HISTORY_MAX];
  74     unsigned history_idx, n_history;
  75
  76     /* To even out for monotonicity */
  77     pa_usec_t last_y;
  78
  79     /* Cached parameters for our interpolation polynomial y=ax^3+b^2+cx */
  80     double a, b, c;
  81     pa_bool_t abc_valid;
  82 };
  83
  84 pa_smoother* pa_smoother_new(pa_usec_t adjust_time, pa_usec_t history_time, pa_bool_t monotonic) {
  85     pa_smoother *s;
  86
  87     pa_assert(adjust_time > 0);
  88     pa_assert(history_time > 0);
  89
  90     s = pa_xnew(pa_smoother, 1);
  91     s->adjust_time = adjust_time;
  92     s->history_time = history_time;
  93     s->monotonic = monotonic;
  94
  95     s->px = s->py = 0;
  96     s->dp = 1;
  97
  98     s->ex = s->ey = 0;
  99     s->de = 1;
 100
 101     s->history_idx = 0;
 102     s->n_history = 0;
 103
 104     s->last_y = 0;
 105
 106     s->abc_valid = FALSE;
 107
 108     return s;
 109 }
 110
 111 void pa_smoother_free(pa_smoother* s) {
 112     pa_assert(s);
 113
 114     pa_xfree(s);
 115 }
 116
 117 static void drop_old(pa_smoother *s, pa_usec_t x) {
 118     unsigned j;
 119
 120     /* First drop items from history which are too old, but make sure
 121      * to always keep two entries in the history */
 122
 123     for (j = s->n_history; j > 2; j--) {
 124
 125         if (s->history_x[s->history_idx] + s->history_time >= x) {
 126             /* This item is still valid, and thus all following ones
 127              * are too, so let's quit this loop */
 128             break;
 129         }
 130
 131         /* Item is too old, let's drop it */
 132         s->history_idx ++;
 133         while (s->history_idx >= HISTORY_MAX)
 134             s->history_idx -= HISTORY_MAX;
 135
 136         s->n_history --;
 137     }
 138 }
 139
 140 static void add_to_history(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
 141     unsigned j;
 142     pa_assert(s);
 143
 144     drop_old(s, x);
 145
 146     /* Calculate position for new entry */
 147     j = s->history_idx + s->n_history;
 148     while (j >= HISTORY_MAX)
 149         j -= HISTORY_MAX;
 150
 151     /* Fill in entry */
 152     s->history_x[j] = x;
 153     s->history_y[j] = y;
 154
 155     /* Adjust counter */
 156     s->n_history ++;
 157
 158     /* And make sure we don't store more entries than fit in */
 159     if (s->n_history >= HISTORY_MAX) {
 160         s->history_idx += s->n_history - HISTORY_MAX;
 161         s->n_history = HISTORY_MAX;
 162     }
 163 }
 164
 165 static double avg_gradient(pa_smoother *s, pa_usec_t x) {
 166     unsigned i, j, c = 0;
 167     int64_t ax = 0, ay = 0, k, t;
 168     double r;
 169
 170     drop_old(s, x);
 171
 172     /* First, calculate average of all measurements */
 173     i = s->history_idx;
 174     for (j = s->n_history; j > 0; j--) {
 175
 176         ax += s->history_x[i];
 177         ay += s->history_y[i];
 178         c++;
 179
 180         i++;
 181         while (i >= HISTORY_MAX)
 182             i -= HISTORY_MAX;
 183     }
 184
 185     /* Too few measurements, assume gradient of 1 */
 186     if (c < 2)
 187         return 1;
 188
 189     ax /= c;
 190     ay /= c;
 191
 192     /* Now, do linear regression */
 193     k = t = 0;
 194
 195     i = s->history_idx;
 196     for (j = s->n_history; j > 0; j--) {
 197         int64_t dx, dy;
 198
 199         dx = (int64_t) s->history_x[i] - ax;
 200         dy = (int64_t) s->history_y[i] - ay;
 201
 202         k += dx*dy;
 203         t += dx*dx;
 204
 205         i++;
 206         while (i >= HISTORY_MAX)
 207             i -= HISTORY_MAX;
 208     }
 209
 210     r = (double) k / t;
 211
 212     return s->monotonic && r < 0 ? 0 : r;
 213 }
 214
 215 static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
 216     pa_assert(s);
 217     pa_assert(y);
 218
 219     if (x >= s->px) {
 220         int64_t t;
 221
 222         /* The requested point is right of the point where we wanted
 223          * to be on track again, thus just linearly estimate */
 224
 225         t = (int64_t) s->py + (int64_t) (s->dp * (x - s->px));
 226
 227         if (t < 0)
 228             t = 0;
 229
 230         *y = (pa_usec_t) t;
 231
 232         if (deriv)
 233             *deriv = s->dp;
 234
 235     } else {
 236
 237         if (!s->abc_valid) {
 238             pa_usec_t ex, ey, px, py;
 239             int64_t kx, ky;
 240             double de, dp;
 241
 242             /* Ok, we're not yet on track, thus let's interpolate, and
 243              * make sure that the first derivative is smooth */
 244
 245             /* We have two points: (ex|ey) and (px|py) with two gradients
 246              * at these points de and dp. We do a polynomial interpolation
 247              * of degree 3 with these 6 values */
 248
 249             ex = s->ex; ey = s->ey;
 250             px = s->px; py = s->py;
 251             de = s->de; dp = s->dp;
 252
 253             pa_assert(ex < px);
 254
 255             /* To increase the dynamic range and symplify calculation, we
 256              * move these values to the origin */
 257             kx = (int64_t) px - (int64_t) ex;
 258             ky = (int64_t) py - (int64_t) ey;
 259
 260             /* Calculate a, b, c for y=ax^3+b^2+cx */
 261             s->c = de;
 262             s->b = (((double) (3*ky)/kx - dp - 2*de)) / kx;
 263             s->a = (dp/kx - 2*s->b - de/kx) / (3*kx);
 264
 265             s->abc_valid = TRUE;
 266         }
 267
 268         /* Move to origin */
 269         x -= s->ex;
 270
 271         /* Horner scheme */
 272         *y = (pa_usec_t) ((double) x * (s->c + (double) x * (s->b + (double) x * s->a)));
 273
 274         /* Move back from origin */
 275         *y += s->ey;
 276
 277         /* Horner scheme */
 278         if (deriv)
 279             *deriv = s->c + ((double) x * (s->b*2 + (double) x * s->a*3));
 280     }
 281
 282     /* Guarantee monotonicity */
 283     if (s->monotonic) {
 284
 285         if (*y < s->last_y)
 286             *y = s->last_y;
 287         else
 288             s->last_y = *y;
 289
 290         if (deriv && *deriv < 0)
 291             *deriv = 0;
 292     }
 293 }
 294
 295 void pa_smoother_put(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
 296     pa_assert(s);
 297
 298     /* First, we calculate the position we'd estimate for x, so that
 299      * we can adjust our position smoothly from this one */
 300     estimate(s, x, &s->ey, &s->de);
 301     s->ex = x;
 302
 303     /* Then, we add the new measurement to our history */
 304     add_to_history(s, x, y);
 305
 306     /* And determine the average gradient of the history */
 307     s->dp = avg_gradient(s, x);
 308
 309     /* And calculate when we want to be on track again */
 310     s->px = x + s->adjust_time;
 311     s->py = y + s->dp *s->adjust_time;
 312
 313     s->abc_valid = FALSE;
 314 }
 315
 316 pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
 317     pa_usec_t y;
 318
 319     pa_assert(s);
 320
 321     estimate(s, x, &y, NULL);
 322     return y;
 323 }