Add median filter for refclocks
This commit is contained in:
Miroslav Lichvar
parent
923c58485f
commit
735811b2b9
3 changed files with 235 additions and 22 deletions
14
conf.c
14
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;
|
||||
|
||||
|
|
|
|||
241
refclock.c
241
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
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
Loading…
Reference in a new issue