Smooth the path delay estimate with a moving average.

This is really just a first attempt using a hard coded length. Probably
it will be necessary to let the length be configurable and/or adaptable.

Signed-off-by: Richard Cochran <richardcochran@gmail.com>

clock.c

@@ -25,6 +25,7 @@
 #include "bmc.h"
 #include "clock.h"
 #include "foreign.h"
+#include "mave.h"
 #include "missing.h"
 #include "msg.h"
 #include "phc.h"
@@ -34,6 +35,8 @@
 #include "tmv.h"
 #include "util.h"
 
+#define MAVE_LENGTH 10
+
 #define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
 
 struct clock {
@@ -50,6 +53,7 @@ struct clock {
 	int nports;
 	tmv_t master_offset;
 	tmv_t path_delay;
+	struct mave *avg_delay;
 	tmv_t c1;
 	tmv_t c2;
 	tmv_t t1;
@@ -183,6 +187,11 @@ struct clock *clock_create(char *phc, struct interface *iface, int count,
 		pr_err("Failed to create clock servo");
 		return NULL;
 	}
+	c->avg_delay = mave_create(MAVE_LENGTH);
+	if (!c->avg_delay) {
+		pr_err("Failed to create moving average");
+		return NULL;
+	}
 
 	c->dds = *ds;
 
@@ -315,7 +324,7 @@ int clock_poll(struct clock *c)
 void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
 		      Integer64 correction)
 {
-	tmv_t c1, c2, c3, t1, t2, t3, t4;
+	tmv_t c1, c2, c3, pd, t1, t2, t3, t4;
 
 	c1 = c->c1;
 	c2 = c->c2;
@@ -330,11 +339,13 @@ void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
 	 * c->path_delay -= c_sync + c_fup + c_delay_resp;
 	 * c->path_delay /= 2.0;
 	 */
-	c->path_delay = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1));
-	c->path_delay = tmv_sub(c->path_delay, tmv_add(c1, tmv_add(c2, c3)));
-	c->path_delay = tmv_div(c->path_delay, 2);
+	pd = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1));
+	pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3)));
+	pd = tmv_div(pd, 2);
 
-	pr_debug("path delay    %10lld", c->path_delay);
+	c->path_delay = mave_accumulate(c->avg_delay, pd);
+
+	pr_debug("path delay    %10lld %10lld", c->path_delay, pd);
 }
 
 int clock_slave_only(struct clock *c)
@@ -404,6 +415,9 @@ static void handle_state_decision_event(struct clock *c)
 	pr_info("selected best master clock %s",
 		cid2str(&best->dataset.identity));
 
+	if (c->best != best)
+		mave_reset(c->avg_delay);
+
 	c->best = best;
 
 	for (i = 0; i < c->nports; i++) {

makefile

@@ -24,8 +24,8 @@ CFLAGS	= -Wall $(INC) $(DEBUG)
 LDFLAGS	=
 LDLIBS	= -lm -lrt
 PRG	= linuxptp
-OBJ	= bmc.o clock.o fsm.o linuxptp.o msg.o phc.o pi.o port.o print.o \
- servo.o tmtab.o transport.o udp.o util.o
+OBJ	= bmc.o clock.o fsm.o linuxptp.o mave.o msg.o phc.o pi.o port.o \
+ print.o servo.o tmtab.o transport.o udp.o util.o
 
 SRC	= $(OBJ:.o=.c)
 DEPEND	= $(OBJ:.o=.d)

mave.c

@@ -0,0 +1,72 @@
+/**
+ * @file mave.c
+ * @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#include <stdlib.h>
+#include <string.h>
+
+#include "mave.h"
+
+struct mave {
+	int cnt;
+	int len;
+	int index;
+	tmv_t sum;
+	tmv_t *val;
+};
+
+struct mave *mave_create(int length)
+{
+	struct mave *m;
+	m = calloc(1, sizeof(*m));
+	if (!m) {
+		return NULL;
+	}
+	m->val = calloc(1, length * sizeof(*m->val));
+	if (!m->val) {
+		free(m);
+		return NULL;
+	}
+	m->len = length;
+	return m;
+}
+
+void mave_destroy(struct mave *m)
+{
+	free(m->val);
+	free(m);
+}
+
+tmv_t mave_accumulate(struct mave *m, tmv_t val)
+{
+	m->sum = tmv_sub(m->sum, m->val[m->index]);
+	m->val[m->index] = val;
+	m->index = (1 + m->index) % m->len;
+	m->sum = tmv_add(m->sum, val);
+	if (m->cnt < m->len) {
+		m->cnt++;
+	}
+	return tmv_div(m->sum, m->cnt);
+}
+
+void mave_reset(struct mave *m)
+{
+	m->cnt = 0;
+	m->index = 0;
+	m->sum = 0;
+	memset(m->val, 0, m->len * sizeof(*m->val));
+}

mave.h

@@ -0,0 +1,35 @@
+/**
+ * @file mave.h
+ * @brief Implements a moving average.
+ * @note Copyright (C) 2011 Richard Cochran <richardcochran@gmail.com>
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
+ */
+#ifndef HAVE_MAVE_H
+#define HAVE_MAVE_H
+
+#include "tmv.h"
+
+struct mave;
+
+struct mave *mave_create(int length);
+
+void mave_destroy(struct mave *m);
+
+tmv_t mave_accumulate(struct mave *m, tmv_t val);
+
+void mave_reset(struct mave *m);
+
+#endif

tmv.h

@@ -20,6 +20,8 @@
 #ifndef HAVE_TMV_H
 #define HAVE_TMV_H
 
+#include <time.h>
+
 #include "ddt.h"
 #include "pdt.h"