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>master
parent
68aea3058c
commit
56a5adc1cc
24
clock.c
24
clock.c
|
@ -25,6 +25,7 @@
|
||||||
#include "bmc.h"
|
#include "bmc.h"
|
||||||
#include "clock.h"
|
#include "clock.h"
|
||||||
#include "foreign.h"
|
#include "foreign.h"
|
||||||
|
#include "mave.h"
|
||||||
#include "missing.h"
|
#include "missing.h"
|
||||||
#include "msg.h"
|
#include "msg.h"
|
||||||
#include "phc.h"
|
#include "phc.h"
|
||||||
|
@ -34,6 +35,8 @@
|
||||||
#include "tmv.h"
|
#include "tmv.h"
|
||||||
#include "util.h"
|
#include "util.h"
|
||||||
|
|
||||||
|
#define MAVE_LENGTH 10
|
||||||
|
|
||||||
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
|
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
|
||||||
|
|
||||||
struct clock {
|
struct clock {
|
||||||
|
@ -50,6 +53,7 @@ struct clock {
|
||||||
int nports;
|
int nports;
|
||||||
tmv_t master_offset;
|
tmv_t master_offset;
|
||||||
tmv_t path_delay;
|
tmv_t path_delay;
|
||||||
|
struct mave *avg_delay;
|
||||||
tmv_t c1;
|
tmv_t c1;
|
||||||
tmv_t c2;
|
tmv_t c2;
|
||||||
tmv_t t1;
|
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");
|
pr_err("Failed to create clock servo");
|
||||||
return NULL;
|
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;
|
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,
|
void clock_path_delay(struct clock *c, struct timespec req, struct timestamp rx,
|
||||||
Integer64 correction)
|
Integer64 correction)
|
||||||
{
|
{
|
||||||
tmv_t c1, c2, c3, t1, t2, t3, t4;
|
tmv_t c1, c2, c3, pd, t1, t2, t3, t4;
|
||||||
|
|
||||||
c1 = c->c1;
|
c1 = c->c1;
|
||||||
c2 = c->c2;
|
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 -= c_sync + c_fup + c_delay_resp;
|
||||||
* c->path_delay /= 2.0;
|
* c->path_delay /= 2.0;
|
||||||
*/
|
*/
|
||||||
c->path_delay = tmv_add(tmv_sub(t2, t3), tmv_sub(t4, t1));
|
pd = 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)));
|
pd = tmv_sub(pd, tmv_add(c1, tmv_add(c2, c3)));
|
||||||
c->path_delay = tmv_div(c->path_delay, 2);
|
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)
|
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",
|
pr_info("selected best master clock %s",
|
||||||
cid2str(&best->dataset.identity));
|
cid2str(&best->dataset.identity));
|
||||||
|
|
||||||
|
if (c->best != best)
|
||||||
|
mave_reset(c->avg_delay);
|
||||||
|
|
||||||
c->best = best;
|
c->best = best;
|
||||||
|
|
||||||
for (i = 0; i < c->nports; i++) {
|
for (i = 0; i < c->nports; i++) {
|
||||||
|
|
4
makefile
4
makefile
|
@ -24,8 +24,8 @@ CFLAGS = -Wall $(INC) $(DEBUG)
|
||||||
LDFLAGS =
|
LDFLAGS =
|
||||||
LDLIBS = -lm -lrt
|
LDLIBS = -lm -lrt
|
||||||
PRG = linuxptp
|
PRG = linuxptp
|
||||||
OBJ = bmc.o clock.o fsm.o linuxptp.o msg.o phc.o pi.o port.o print.o \
|
OBJ = bmc.o clock.o fsm.o linuxptp.o mave.o msg.o phc.o pi.o port.o \
|
||||||
servo.o tmtab.o transport.o udp.o util.o
|
print.o servo.o tmtab.o transport.o udp.o util.o
|
||||||
|
|
||||||
SRC = $(OBJ:.o=.c)
|
SRC = $(OBJ:.o=.c)
|
||||||
DEPEND = $(OBJ:.o=.d)
|
DEPEND = $(OBJ:.o=.d)
|
||||||
|
|
|
@ -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));
|
||||||
|
}
|
|
@ -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
|
Loading…
Reference in New Issue