Add median filter for refclocks

diff --git a/conf.c b/conf.c
index 99144131489688da312bf60827fa510230c7a3f7..b64f3909ad211ae5e87a72ff991620be2c2d8efc 100644 (file)
--- 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 3b1f9519a8d3c573199a19ba68f755f2d22bbca0..1d8a5b314f72f28dbc1b1b29664d8f73f0870b2d 100644 (file)
--- 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);
+  poll = inst->poll;
+
+  if (inst->driver->poll) {
+    poll = inst->driver_poll;
+    inst->driver->poll(inst);
+    inst->driver_polled++;
+  }
   
-  if (inst->missed_samples > 9)
-    SRC_UnsetReachable(inst->source);
+  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 (inst->poll >= 0)
-    next = 1 << inst->poll;
+    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 42606161174c91db7079341a1c8157b215acda8b..9e451b653260462fdcfac12cafedb0d42ff3849a 100644 (file)
--- 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;