From 735811b2b97d4a4bee850465b1bff37c62d9923e Mon Sep 17 00:00:00 2001 From: Miroslav Lichvar Date: Wed, 1 Jul 2009 15:40:50 +0200 Subject: [PATCH] Add median filter for refclocks --- conf.c | 14 +++- refclock.c | 241 ++++++++++++++++++++++++++++++++++++++++++++++++----- refclock.h | 2 + 3 files changed, 235 insertions(+), 22 deletions(-) diff --git a/conf.c b/conf.c index 9914413..b64f390 100644 --- a/conf.c +++ b/conf.c @@ -428,7 +428,7 @@ parse_peer(const char *line) static void parse_refclock(const char *line) { - int i, n, param, poll; + int i, n, param, poll, dpoll, filter_length; unsigned long ref_id; double offset; char name[5], cmd[10 + 1]; @@ -439,6 +439,8 @@ parse_refclock(const char *line) return; poll = 4; + dpoll = 0; + filter_length = 15; offset = 0.0; ref_id = 0; @@ -459,6 +461,14 @@ parse_refclock(const char *line) if (sscanf(line, "%d%n", &poll, &n) != 1) { break; } + } else if (!strncasecmp(cmd, "dpoll", 5)) { + if (sscanf(line, "%d%n", &dpoll, &n) != 1) { + break; + } + } else if (!strncasecmp(cmd, "filter", 6)) { + if (sscanf(line, "%d%n", &filter_length, &n) != 1) { + break; + } } else if (!strncasecmp(cmd, "offset", 6)) { if (sscanf(line, "%lf%n", &offset, &n) != 1) break; @@ -471,7 +481,9 @@ parse_refclock(const char *line) strncpy(refclock_sources[i].driver_name, name, 4); refclock_sources[i].driver_parameter = param; + refclock_sources[i].driver_poll = dpoll; refclock_sources[i].poll = poll; + refclock_sources[i].filter_length = filter_length; refclock_sources[i].offset = offset; refclock_sources[i].ref_id = ref_id; diff --git a/refclock.c b/refclock.c index 3b1f951..1d8a5b3 100644 --- a/refclock.c +++ b/refclock.c @@ -28,6 +28,7 @@ #include "refclock.h" #include "conf.h" #include "local.h" +#include "memory.h" #include "util.h" #include "sources.h" #include "logging.h" @@ -36,12 +37,28 @@ /* list of refclock drivers */ extern RefclockDriver RCL_SHM_driver; +struct FilterSample { + double offset; + struct timeval sample_time; +}; + +struct MedianFilter { + int length; + int index; + int used; + struct FilterSample *samples; +}; + struct RCL_Instance_Record { RefclockDriver *driver; void *data; int driver_parameter; + int driver_poll; + int driver_polled; int poll; int missed_samples; + int leap_status; + struct MedianFilter filter; unsigned long ref_id; double offset; SCH_TimeoutID timeout_id; @@ -54,6 +71,15 @@ static struct RCL_Instance_Record refclocks[MAX_RCL_SOURCES]; static int n_sources = 0; static void poll_timeout(void *arg); +static void slew_samples(struct timeval *raw, struct timeval *cooked, double dfreq, double afreq, + double doffset, int is_step_change, void *anything); + +static void filter_init(struct MedianFilter *filter, int length); +static void filter_fini(struct MedianFilter *filter); +static void filter_reset(struct MedianFilter *filter); +static void filter_add_sample(struct MedianFilter *filter, struct timeval *sample_time, double offset); +static int filter_get_sample(struct MedianFilter *filter, struct timeval *sample_time, double *offset, double *dispersion); +static void filter_slew_samples(struct MedianFilter *filter, struct timeval *when, double dfreq, double doffset); void RCL_Initialise(void) @@ -71,7 +97,12 @@ RCL_Finalise(void) if (inst->driver->fini) inst->driver->fini(inst); + + filter_fini(&inst->filter); } + + if (n_sources > 0) + LCL_RemoveParameterChangeHandler(slew_samples, NULL); } int @@ -91,12 +122,18 @@ RCL_AddRefclock(RefclockParameters *params) inst->data = NULL; inst->driver_parameter = params->driver_parameter; + inst->driver_poll = params->driver_poll; inst->poll = params->poll; inst->missed_samples = 0; + inst->driver_polled = 0; + inst->leap_status = 0; inst->offset = params->offset; inst->timeout_id = -1; inst->source = NULL; + if (inst->driver_poll > inst->poll) + inst->driver_poll = inst->poll; + if (params->ref_id) inst->ref_id = params->ref_id; else { @@ -112,8 +149,11 @@ RCL_AddRefclock(RefclockParameters *params) return 0; } + filter_init(&inst->filter, params->filter_length); + #if 0 - LOG(LOGS_INFO, LOGF_Refclock, "refclock added"); + LOG(LOGS_INFO, LOGF_Refclock, "refclock added poll=%d dpoll=%d filter=%d", + inst->poll, inst->driver_poll, params->filter_length); #endif n_sources++; @@ -129,9 +169,11 @@ RCL_StartRefclocks(void) RCL_Instance inst = &refclocks[i]; inst->source = SRC_CreateNewInstance(inst->ref_id, SRC_REFCLOCK); - if (inst->driver->poll) - inst->timeout_id = SCH_AddTimeoutByDelay(0.0, poll_timeout, (void *)inst); + inst->timeout_id = SCH_AddTimeoutByDelay(0.0, poll_timeout, (void *)inst); } + + if (n_sources > 0) + LCL_AddParameterChangeHandler(slew_samples, NULL); } void @@ -175,21 +217,17 @@ RCL_AddSample(RCL_Instance instance, struct timeval *sample_time, double offset, { double correction; struct timeval cooked_time; - SRC_Instance inst = instance->source; - -#if 0 - LOG(LOGS_INFO, LOGF_Refclock, "refclock offset: %f", offset); -#endif - - SRC_SetReachable(inst); correction = LCL_GetOffsetCorrection(sample_time); UTI_AddDoubleToTimeval(sample_time, correction, &cooked_time); - SRC_AccumulateSample(inst, &cooked_time, offset - correction + instance->offset, - 1e-6, 0.0, 0.0, 0.0, 0, leap_status); +#if 0 + LOG(LOGS_INFO, LOGF_Refclock, "refclock sample offset=%.9f cooked=%.9f", + offset, offset - correction + instance->offset); +#endif - instance->missed_samples = 0; + filter_add_sample(&instance->filter, &cooked_time, offset - correction + instance->offset); + instance->leap_status = leap_status; return 1; } @@ -198,20 +236,181 @@ static void poll_timeout(void *arg) { double next; + int poll; RCL_Instance inst = (RCL_Instance)arg; - inst->missed_samples++; - inst->driver->poll(inst); - - if (inst->missed_samples > 9) - SRC_UnsetReachable(inst->source); + poll = inst->poll; - if (inst->poll >= 0) - next = 1 << inst->poll; + if (inst->driver->poll) { + poll = inst->driver_poll; + inst->driver->poll(inst); + inst->driver_polled++; + } + + if (!(inst->driver->poll && inst->driver_polled < (1 << (inst->poll - inst->driver_poll)))) { + double offset, dispersion; + struct timeval sample_time; + int sample_ok; + + sample_ok = filter_get_sample(&inst->filter, &sample_time, &offset, &dispersion); + filter_reset(&inst->filter); + inst->driver_polled = 0; + + if (sample_ok) { +#if 0 + LOG(LOGS_INFO, LOGF_Refclock, "refclock filtered sample: offset=%.9f dispersion=%.9f [%s]", + offset, dispersion, UTI_TimevalToString(&sample_time)); +#endif + SRC_SetReachable(inst->source); + SRC_AccumulateSample(inst->source, &sample_time, offset, + 1e-9, dispersion, 1e-9, dispersion, 0, inst->leap_status); + inst->missed_samples = 0; + } else { + inst->missed_samples++; + if (inst->missed_samples > 9) + SRC_UnsetReachable(inst->source); + } + } + + if (poll >= 0) + next = 1 << poll; else - next = 1.0 / (1 << -inst->poll); + next = 1.0 / (1 << -poll); inst->timeout_id = SCH_AddTimeoutByDelay(next, poll_timeout, arg); } +static void +slew_samples(struct timeval *raw, struct timeval *cooked, double dfreq, double afreq, + double doffset, int is_step_change, void *anything) +{ + int i; + + for (i = 0; i < n_sources; i++) + filter_slew_samples(&refclocks[i].filter, cooked, dfreq, doffset); +} + +static void +filter_init(struct MedianFilter *filter, int length) +{ + if (length < 1) + length = 1; + + filter->length = length; + filter->index = -1; + filter->used = 0; + filter->samples = MallocArray(struct FilterSample, filter->length); +} + +static void +filter_fini(struct MedianFilter *filter) +{ + Free(filter->samples); +} + +static void +filter_reset(struct MedianFilter *filter) +{ + filter->index = -1; + filter->used = 0; +} + +static void +filter_add_sample(struct MedianFilter *filter, struct timeval *sample_time, double offset) +{ + filter->index++; + filter->index %= filter->length; + if (filter->used < filter->length) + filter->used++; + + filter->samples[filter->index].sample_time = *sample_time; + filter->samples[filter->index].offset = offset; +} + +static int +sample_compare(const void *a, const void *b) +{ + const struct FilterSample *s1 = a, *s2 = b; + + if (s1->offset < s2->offset) + return -1; + else if (s1->offset > s2->offset) + return 1; + return 0; +} + +static int +filter_get_sample(struct MedianFilter *filter, struct timeval *sample_time, double *offset, double *dispersion) +{ + if (filter->used == 0) + return 0; + + if (filter->used == 1) { + *sample_time = filter->samples[filter->index].sample_time; + *offset = filter->samples[filter->index].offset; + *dispersion = 0.0; + } else { + int i, from, to; + double x, x1, y, d; + + /* sort samples by offset */ + qsort(filter->samples, filter->used, sizeof (struct FilterSample), sample_compare); + + /* average the half of the samples closest to the median */ + if (filter->used > 2) { + from = (filter->used + 2) / 4; + to = filter->used - from; + } else { + from = 0; + to = filter->used; + } + + for (i = from, x = y = 0.0; i < to; i++) { +#if 0 + LOG(LOGS_INFO, LOGF_Refclock, "refclock averaging offset %.9f [%s]", + filter->samples[i].offset, UTI_TimevalToString(&filter->samples[i].sample_time)); +#endif + UTI_DiffTimevalsToDouble(&x1, &filter->samples[i].sample_time, &filter->samples[0].sample_time); + x += x1; + y += filter->samples[i].offset; + } + + x /= to - from; + y /= to - from; + + for (i = from, d = 0.0; i < to; i++) + d += (filter->samples[i].offset - y) * (filter->samples[i].offset - y); + + d = sqrt(d / (to - from)); + + UTI_AddDoubleToTimeval(&filter->samples[0].sample_time, x, sample_time); + *offset = y; + *dispersion = d; + } + + return 1; +} + +static void +filter_slew_samples(struct MedianFilter *filter, struct timeval *when, double dfreq, double doffset) +{ + int i; + double elapsed, delta_time, prev_offset; + struct timeval *sample; + + for (i = 0; i < filter->used; i++) { + sample = &filter->samples[i].sample_time; + + UTI_DiffTimevalsToDouble(&elapsed, when, sample); + delta_time = elapsed * dfreq - doffset; + UTI_AddDoubleToTimeval(sample, delta_time, sample); + + prev_offset = filter->samples[i].offset; + filter->samples[i].offset -= delta_time; +#if 0 + LOG(LOGS_INFO, LOGF_Refclock, "i=%d old_off=%.9f new_off=%.9f", + i, prev_offset, filter->samples[i].offset); +#endif + } +} diff --git a/refclock.h b/refclock.h index 4260616..9e451b6 100644 --- a/refclock.h +++ b/refclock.h @@ -34,7 +34,9 @@ typedef struct { char driver_name[4]; int driver_parameter; + int driver_poll; int poll; + int filter_length; unsigned long ref_id; double offset; } RefclockParameters;