]>
code.delx.au - pulseaudio/blob - src/pulsecore/time-smoother.c
2 This file is part of PulseAudio.
4 Copyright 2007 Lennart Poettering
6 PulseAudio is free software; you can redistribute it and/or modify
7 it under the terms of the GNU Lesser General Public License as
8 published by the Free Software Foundation; either version 2.1 of the
9 License, or (at your option) any later version.
11 PulseAudio is distributed in the hope that it will be useful, but
12 WITHOUT ANY WARRANTY; without even the implied warranty of
13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14 Lesser General Public License for more details.
16 You should have received a copy of the GNU Lesser General Public
17 License along with PulseAudio; if not, write to the Free Software
18 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
28 #include <pulse/sample.h>
29 #include <pulse/xmalloc.h>
31 #include <pulsecore/macro.h>
33 #include "time-smoother.h"
35 #define HISTORY_MAX 64
38 * Implementation of a time smoothing algorithm to synchronize remote
39 * clocks to a local one. Evens out noise, adjusts to clock skew and
40 * allows cheap estimations of the remote time while clock updates may
41 * be seldom and recieved in non-equidistant intervals.
43 * Basically, we estimate the gradient of received clock samples in a
44 * certain history window (of size 'history_time') with linear
45 * regression. With that info we estimate the remote time in
46 * 'adjust_time' ahead and smoothen our current estimation function
47 * towards that point with a 3rd order polynomial interpolation with
48 * fitting derivatives. (more or less a b-spline)
50 * The larger 'history_time' is chosen the better we will surpress
51 * noise -- but we'll adjust to clock skew slower..
53 * The larger 'adjust_time' is chosen the smoother our estimation
54 * function will be -- but we'll adjust to clock skew slower, too.
56 * If 'monotonic' is TRUE the resulting estimation function is
57 * guaranteed to be monotonic.
61 pa_usec_t adjust_time
, history_time
;
63 pa_usec_t time_offset
;
65 pa_usec_t px
, py
; /* Point p, where we want to reach stability */
66 double dp
; /* Gradient we want at point p */
68 pa_usec_t ex
, ey
; /* Point e, which we estimated before and need to smooth to */
69 double de
; /* Gradient we estimated for point e */
70 pa_usec_t ry
; /* The original y value for ex */
72 /* History of last measurements */
73 pa_usec_t history_x
[HISTORY_MAX
], history_y
[HISTORY_MAX
];
74 unsigned history_idx
, n_history
;
76 /* To even out for monotonicity */
77 pa_usec_t last_y
, last_x
;
79 /* Cached parameters for our interpolation polynomial y=ax^3+b^2+cx */
83 pa_bool_t monotonic
:1;
91 pa_smoother
* pa_smoother_new(pa_usec_t adjust_time
, pa_usec_t history_time
, pa_bool_t monotonic
, unsigned min_history
) {
94 pa_assert(adjust_time
> 0);
95 pa_assert(history_time
> 0);
96 pa_assert(min_history
>= 2);
97 pa_assert(min_history
<= HISTORY_MAX
);
99 s
= pa_xnew(pa_smoother
, 1);
100 s
->adjust_time
= adjust_time
;
101 s
->history_time
= history_time
;
103 s
->monotonic
= monotonic
;
108 s
->ex
= s
->ey
= s
->ry
= 0;
114 s
->last_y
= s
->last_x
= 0;
116 s
->abc_valid
= FALSE
;
120 s
->min_history
= min_history
;
125 void pa_smoother_free(pa_smoother
* s
) {
133 x = (x) % HISTORY_MAX; \
136 #define REDUCE_INC(x) \
138 x = ((x)+1) % HISTORY_MAX; \
142 static void drop_old(pa_smoother
*s
, pa_usec_t x
) {
144 /* Drop items from history which are too old, but make sure to
145 * always keep min_history in the history */
147 while (s
->n_history
> s
->min_history
) {
149 if (s
->history_x
[s
->history_idx
] + s
->history_time
>= x
)
150 /* This item is still valid, and thus all following ones
151 * are too, so let's quit this loop */
154 /* Item is too old, let's drop it */
155 REDUCE_INC(s
->history_idx
);
161 static void add_to_history(pa_smoother
*s
, pa_usec_t x
, pa_usec_t y
) {
165 /* First try to update an existing history entry */
167 for (j
= s
->n_history
; j
> 0; j
--) {
169 if (s
->history_x
[i
] == x
) {
177 /* Drop old entries */
180 /* Calculate position for new entry */
181 j
= s
->history_idx
+ s
->n_history
;
191 /* And make sure we don't store more entries than fit in */
192 if (s
->n_history
> HISTORY_MAX
) {
193 s
->history_idx
+= s
->n_history
- HISTORY_MAX
;
194 REDUCE(s
->history_idx
);
195 s
->n_history
= HISTORY_MAX
;
199 static double avg_gradient(pa_smoother
*s
, pa_usec_t x
) {
200 unsigned i
, j
, c
= 0;
201 int64_t ax
= 0, ay
= 0, k
, t
;
204 /* Too few measurements, assume gradient of 1 */
205 if (s
->n_history
< s
->min_history
)
208 /* First, calculate average of all measurements */
210 for (j
= s
->n_history
; j
> 0; j
--) {
212 ax
+= (int64_t) s
->history_x
[i
];
213 ay
+= (int64_t) s
->history_y
[i
];
219 pa_assert(c
>= s
->min_history
);
223 /* Now, do linear regression */
227 for (j
= s
->n_history
; j
> 0; j
--) {
230 dx
= (int64_t) s
->history_x
[i
] - ax
;
231 dy
= (int64_t) s
->history_y
[i
] - ay
;
239 r
= (double) k
/ (double) t
;
241 return (s
->monotonic
&& r
< 0) ? 0 : r
;
244 static void calc_abc(pa_smoother
*s
) {
245 pa_usec_t ex
, ey
, px
, py
;
254 /* We have two points: (ex|ey) and (px|py) with two gradients at
255 * these points de and dp. We do a polynomial
256 * interpolation of degree 3 with these 6 values */
258 ex
= s
->ex
; ey
= s
->ey
;
259 px
= s
->px
; py
= s
->py
;
260 de
= s
->de
; dp
= s
->dp
;
264 /* To increase the dynamic range and symplify calculation, we
265 * move these values to the origin */
266 kx
= (int64_t) px
- (int64_t) ex
;
267 ky
= (int64_t) py
- (int64_t) ey
;
269 /* Calculate a, b, c for y=ax^3+bx^2+cx */
271 s
->b
= (((double) (3*ky
)/ (double) kx
- dp
- (double) (2*de
))) / (double) kx
;
272 s
->a
= (dp
/(double) kx
- 2*s
->b
- de
/(double) kx
) / (double) (3*kx
);
277 static void estimate(pa_smoother
*s
, pa_usec_t x
, pa_usec_t
*y
, double *deriv
) {
284 /* The requested point is right of the point where we wanted
285 * to be on track again, thus just linearly estimate */
287 t
= (int64_t) s
->py
+ (int64_t) (s
->dp
* (double) (x
- s
->px
));
300 /* Ok, we're not yet on track, thus let's interpolate, and
301 * make sure that the first derivative is smooth */
308 tx
-= (double) s
->ex
;
311 ty
= (tx
* (s
->c
+ tx
* (s
->b
+ tx
* s
->a
)));
313 /* Move back from origin */
314 ty
+= (double) s
->ey
;
316 *y
= ty
>= 0 ? (pa_usec_t
) ty
: 0;
320 *deriv
= s
->c
+ (tx
* (s
->b
*2 + tx
* s
->a
*3));
323 /* Guarantee monotonicity */
326 if (deriv
&& *deriv
< 0)
331 void pa_smoother_put(pa_smoother
*s
, pa_usec_t x
, pa_usec_t y
) {
342 x
= PA_LIKELY(x
>= s
->time_offset
) ? x
- s
->time_offset
: 0;
347 /* First, we calculate the position we'd estimate for x, so that
348 * we can adjust our position smoothly from this one */
349 estimate(s
, x
, &ney
, &nde
);
350 s
->ex
= x
; s
->ey
= ney
; s
->de
= nde
;
355 /* Then, we add the new measurement to our history */
356 add_to_history(s
, x
, y
);
358 /* And determine the average gradient of the history */
359 s
->dp
= avg_gradient(s
, x
);
361 /* And calculate when we want to be on track again */
362 s
->px
= s
->ex
+ s
->adjust_time
;
363 s
->py
= s
->ry
+ (pa_usec_t
) (s
->dp
* (double) s
->adjust_time
);
365 s
->abc_valid
= FALSE
;
367 /* pa_log_debug("put(%llu | %llu) = %llu", (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
370 pa_usec_t
pa_smoother_get(pa_smoother
*s
, pa_usec_t x
) {
379 x
= PA_LIKELY(x
>= s
->time_offset
) ? x
- s
->time_offset
: 0;
385 estimate(s
, x
, &y
, NULL
);
389 /* Make sure the querier doesn't jump forth and back. */
398 /* pa_log_debug("get(%llu | %llu) = %llu", (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
403 void pa_smoother_set_time_offset(pa_smoother
*s
, pa_usec_t offset
) {
406 s
->time_offset
= offset
;
408 /* pa_log_debug("offset(%llu)", (unsigned long long) offset); */
411 void pa_smoother_pause(pa_smoother
*s
, pa_usec_t x
) {
417 /* pa_log_debug("pause(%llu)", (unsigned long long) x); */
423 void pa_smoother_resume(pa_smoother
*s
, pa_usec_t x
) {
429 if (x
< s
->pause_time
)
432 /* pa_log_debug("resume(%llu)", (unsigned long long) x); */
435 s
->time_offset
+= x
- s
->pause_time
;
438 pa_usec_t
pa_smoother_translate(pa_smoother
*s
, pa_usec_t x
, pa_usec_t y_delay
) {
448 x
= PA_LIKELY(x
>= s
->time_offset
) ? x
- s
->time_offset
: 0;
450 estimate(s
, x
, &ney
, &nde
);
452 /* Play safe and take the larger gradient, so that we wakeup
453 * earlier when this is used for sleeping */
457 /* pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde); */
459 return (pa_usec_t
) ((double) y_delay
/ nde
);
462 void pa_smoother_reset(pa_smoother
*s
) {
466 s
->abc_valid
= FALSE
;