#endif
#include <stdio.h>
+#include <math.h>
#include <pulse/sample.h>
#include <pulse/xmalloc.h>
* Implementation of a time smoothing algorithm to synchronize remote
* clocks to a local one. Evens out noise, adjusts to clock skew and
* allows cheap estimations of the remote time while clock updates may
- * be seldom and recieved in non-equidistant intervals.
+ * be seldom and received in non-equidistant intervals.
*
* Basically, we estimate the gradient of received clock samples in a
* certain history window (of size 'history_time') with linear
* towards that point with a 3rd order polynomial interpolation with
* fitting derivatives. (more or less a b-spline)
*
- * The larger 'history_time' is chosen the better we will surpress
+ * The larger 'history_time' is chosen the better we will suppress
* noise -- but we'll adjust to clock skew slower..
*
* The larger 'adjust_time' is chosen the smoother our estimation
* function will be -- but we'll adjust to clock skew slower, too.
*
- * If 'monotonic' is TRUE the resulting estimation function is
+ * If 'monotonic' is true the resulting estimation function is
* guaranteed to be monotonic.
*/
/* Cached parameters for our interpolation polynomial y=ax^3+b^2+cx */
double a, b, c;
- pa_bool_t abc_valid;
+ bool abc_valid:1;
- pa_bool_t monotonic:1;
- pa_bool_t paused:1;
+ bool monotonic:1;
+ bool paused:1;
+ bool smoothing:1; /* If false we skip the polynomial interpolation step */
pa_usec_t pause_time;
unsigned min_history;
};
-pa_smoother* pa_smoother_new(pa_usec_t adjust_time, pa_usec_t history_time, pa_bool_t monotonic, unsigned min_history) {
+pa_smoother* pa_smoother_new(
+ pa_usec_t adjust_time,
+ pa_usec_t history_time,
+ bool monotonic,
+ bool smoothing,
+ unsigned min_history,
+ pa_usec_t time_offset,
+ bool paused) {
+
pa_smoother *s;
pa_assert(adjust_time > 0);
s = pa_xnew(pa_smoother, 1);
s->adjust_time = adjust_time;
s->history_time = history_time;
- s->time_offset = 0;
+ s->min_history = min_history;
s->monotonic = monotonic;
+ s->smoothing = smoothing;
- s->px = s->py = 0;
- s->dp = 1;
-
- s->ex = s->ey = s->ry = 0;
- s->de = 1;
-
- s->history_idx = 0;
- s->n_history = 0;
-
- s->last_y = s->last_x = 0;
-
- s->abc_valid = FALSE;
-
- s->paused = FALSE;
-
- s->min_history = min_history;
+ pa_smoother_reset(s, time_offset, paused);
return s;
}
#define REDUCE(x) \
do { \
x = (x) % HISTORY_MAX; \
- } while(FALSE)
+ } while(false)
#define REDUCE_INC(x) \
do { \
x = ((x)+1) % HISTORY_MAX; \
- } while(FALSE)
-
+ } while(false)
static void drop_old(pa_smoother *s, pa_usec_t x) {
int64_t ax = 0, ay = 0, k, t;
double r;
+ /* FIXME: Optimization: Jason Newton suggested that instead of
+ * going through the history on each iteration we could calculated
+ * avg_gradient() as we go.
+ *
+ * Second idea: it might make sense to weight history entries:
+ * more recent entries should matter more than old ones. */
+
/* Too few measurements, assume gradient of 1 */
if (s->n_history < s->min_history)
return 1;
pa_assert(ex < px);
- /* To increase the dynamic range and symplify calculation, we
+ /* To increase the dynamic range and simplify calculation, we
* move these values to the origin */
kx = (int64_t) px - (int64_t) ex;
ky = (int64_t) py - (int64_t) ey;
s->b = (((double) (3*ky)/ (double) kx - dp - (double) (2*de))) / (double) kx;
s->a = (dp/(double) kx - 2*s->b - de/(double) kx) / (double) (3*kx);
- s->abc_valid = TRUE;
+ s->abc_valid = true;
}
static void estimate(pa_smoother *s, pa_usec_t x, pa_usec_t *y, double *deriv) {
pa_assert(y);
if (x >= s->px) {
+ /* Linear interpolation right from px */
int64_t t;
/* The requested point is right of the point where we wanted
* to be on track again, thus just linearly estimate */
- t = (int64_t) s->py + (int64_t) (s->dp * (double) (x - s->px));
+ t = (int64_t) s->py + (int64_t) llrint(s->dp * (double) (x - s->px));
if (t < 0)
t = 0;
if (deriv)
*deriv = s->dp;
+ } else if (x <= s->ex) {
+ /* Linear interpolation left from ex */
+ int64_t t;
+
+ t = (int64_t) s->ey - (int64_t) llrint(s->de * (double) (s->ex - x));
+
+ if (t < 0)
+ t = 0;
+
+ *y = (pa_usec_t) t;
+
+ if (deriv)
+ *deriv = s->de;
+
} else {
+ /* Spline interpolation between ex and px */
double tx, ty;
/* Ok, we're not yet on track, thus let's interpolate, and
calc_abc(s);
- tx = (double) x;
-
/* Move to origin */
- tx -= (double) s->ex;
+ tx = (double) (x - s->ex);
/* Horner scheme */
ty = (tx * (s->c + tx * (s->b + tx * s->a)));
/* Move back from origin */
ty += (double) s->ey;
- *y = ty >= 0 ? (pa_usec_t) ty : 0;
+ *y = ty >= 0 ? (pa_usec_t) llrint(ty) : 0;
/* Horner scheme */
if (deriv)
void pa_smoother_put(pa_smoother *s, pa_usec_t x, pa_usec_t y) {
pa_usec_t ney;
double nde;
- pa_bool_t is_new;
+ bool is_new;
pa_assert(s);
* we can adjust our position smoothly from this one */
estimate(s, x, &ney, &nde);
s->ex = x; s->ey = ney; s->de = nde;
-
s->ry = y;
}
s->dp = avg_gradient(s, x);
/* And calculate when we want to be on track again */
- s->px = s->ex + s->adjust_time;
- s->py = s->ry + (pa_usec_t) (s->dp * (double) s->adjust_time);
+ if (s->smoothing) {
+ s->px = s->ex + s->adjust_time;
+ s->py = s->ry + (pa_usec_t) llrint(s->dp * (double) s->adjust_time);
+ } else {
+ s->px = s->ex;
+ s->py = s->ry;
+ }
- s->abc_valid = FALSE;
+ s->abc_valid = false;
-/* pa_log_debug("put(%llu | %llu) = %llu", (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
+#ifdef DEBUG_DATA
+ pa_log_debug("%p, put(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
+#endif
}
pa_usec_t pa_smoother_get(pa_smoother *s, pa_usec_t x) {
s->last_y = y;
}
-/* pa_log_debug("get(%llu | %llu) = %llu", (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y); */
+#ifdef DEBUG_DATA
+ pa_log_debug("%p, get(%llu | %llu) = %llu", s, (unsigned long long) (x + s->time_offset), (unsigned long long) x, (unsigned long long) y);
+#endif
return y;
}
s->time_offset = offset;
-/* pa_log_debug("offset(%llu)", (unsigned long long) offset); */
+#ifdef DEBUG_DATA
+ pa_log_debug("offset(%llu)", (unsigned long long) offset);
+#endif
}
void pa_smoother_pause(pa_smoother *s, pa_usec_t x) {
if (s->paused)
return;
-/* pa_log_debug("pause(%llu)", (unsigned long long) x); */
+#ifdef DEBUG_DATA
+ pa_log_debug("pause(%llu)", (unsigned long long) x);
+#endif
- s->paused = TRUE;
+ s->paused = true;
s->pause_time = x;
}
-void pa_smoother_resume(pa_smoother *s, pa_usec_t x) {
+void pa_smoother_resume(pa_smoother *s, pa_usec_t x, bool fix_now) {
pa_assert(s);
if (!s->paused)
if (x < s->pause_time)
x = s->pause_time;
-/* pa_log_debug("resume(%llu)", (unsigned long long) x); */
+#ifdef DEBUG_DATA
+ pa_log_debug("resume(%llu)", (unsigned long long) x);
+#endif
- s->paused = FALSE;
+ s->paused = false;
s->time_offset += x - s->pause_time;
+
+ if (fix_now)
+ pa_smoother_fix_now(s);
+}
+
+void pa_smoother_fix_now(pa_smoother *s) {
+ pa_assert(s);
+
+ s->px = s->ex;
+ s->py = s->ry;
}
pa_usec_t pa_smoother_translate(pa_smoother *s, pa_usec_t x, pa_usec_t y_delay) {
if (s->dp > nde)
nde = s->dp;
-/* pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde); */
+#ifdef DEBUG_DATA
+ pa_log_debug("translate(%llu) = %llu (%0.2f)", (unsigned long long) y_delay, (unsigned long long) ((double) y_delay / nde), nde);
+#endif
- return (pa_usec_t) ((double) y_delay / nde);
+ return (pa_usec_t) llrint((double) y_delay / nde);
}
-void pa_smoother_reset(pa_smoother *s) {
+void pa_smoother_reset(pa_smoother *s, pa_usec_t time_offset, bool paused) {
pa_assert(s);
+ s->px = s->py = 0;
+ s->dp = 1;
+
+ s->ex = s->ey = s->ry = 0;
+ s->de = 1;
+
+ s->history_idx = 0;
s->n_history = 0;
- s->abc_valid = FALSE;
+
+ s->last_y = s->last_x = 0;
+
+ s->abc_valid = false;
+
+ s->paused = paused;
+ s->time_offset = s->pause_time = time_offset;
+
+#ifdef DEBUG_DATA
+ pa_log_debug("reset()");
+#endif
}