Skip to content

P
PPSi
Commit d0fa0908 authored by Alessandro Rubini's avatar Alessandro Rubini
Browse files
Options

servo: calculate mean path delay before using it 

The code was using "mean path delay" before calculating it. Which means
that is used wrong values (likely 0) the first time and the previous
value at each successive iteration.

This commit moves calculation of "mpd" before code that uses it.  The
calculation itself is not changed.
Signed-off-by: Alessandro Rubini's avatarAlessandro Rubini <rubini@gnudd.com>
parent b5df3c7a
Hide whitespace changes
Inline Side-by-side
Showing with 38 additions and 39 deletions
proto-standard/servo.c
View file @ d0fa0908
......@@ -97,6 +97,44 @@ void pp_servo_got_resp(struct pp_instance *ppi)
return;
}
/* Calc mean path delay, used later to calc "offset from master" */
add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
div2_TimeInternal(mpd);
if (mpd->seconds) {
/* cannot filter with secs, clear filter */
owd_fltr->s_exp = 0;
owd_fltr->nsec_prev = 0;
goto done_mpd;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(owd_fltr->y) >> (31 - s))
--s;
/* crank down filter cutoff by increasing 's_exp' */
if (owd_fltr->s_exp < 1)
owd_fltr->s_exp = 1;
else if (owd_fltr->s_exp < 1 << s)
++owd_fltr->s_exp;
else if (owd_fltr->s_exp > 1 << s)
owd_fltr->s_exp = 1 << s;
/* Use the average between current value and previous one */
mpd->nanoseconds = (mpd->nanoseconds + owd_fltr->nsec_prev) / 2;
owd_fltr->nsec_prev = mpd->nanoseconds;
/* filter 'meanPathDelay' (running average) */
owd_fltr->y = (owd_fltr->y * (owd_fltr->s_exp - 1) + mpd->nanoseconds)
/ owd_fltr->s_exp;
mpd->nanoseconds = owd_fltr->y;
pp_diag(ppi, servo, 1, "delay filter %d, %d\n",
(int)owd_fltr->y, (int)owd_fltr->s_exp);
done_mpd:
/* update 'offsetFromMaster', (End to End mode) */
sub_TimeInternal(&DSCUR(ppi)->offsetFromMaster,
m_to_s_dly,
......@@ -180,45 +218,6 @@ adjust:
ppi->t_ops->adjust_offset(ppi, -adj);
}
/* update 'one_way_delay' */
add_TimeInternal(mpd, &SRV(ppi)->m_to_s_dly, &SRV(ppi)->s_to_m_dly);
/* Compute one-way delay */
div2_TimeInternal(mpd);
if (mpd->seconds) {
/* cannot filter with secs, clear filter */
owd_fltr->s_exp = 0;
owd_fltr->nsec_prev = 0;
return;
}
/* avoid overflowing filter */
s = OPTS(ppi)->s;
while (abs(owd_fltr->y) >> (31 - s))
--s;
/* crank down filter cutoff by increasing 's_exp' */
if (owd_fltr->s_exp < 1)
owd_fltr->s_exp = 1;
else if (owd_fltr->s_exp < 1 << s)
++owd_fltr->s_exp;
else if (owd_fltr->s_exp > 1 << s)
owd_fltr->s_exp = 1 << s;
/* Use the average between current value and previous one */
mpd->nanoseconds = (mpd->nanoseconds + owd_fltr->nsec_prev) / 2;
owd_fltr->nsec_prev = mpd->nanoseconds;
/* filter 'meanPathDelay' (running average) */
owd_fltr->y = (owd_fltr->y * (owd_fltr->s_exp - 1) + mpd->nanoseconds)
/ owd_fltr->s_exp;
mpd->nanoseconds = owd_fltr->y;
pp_diag(ppi, servo, 1, "delay filter %d, %d\n",
(int)owd_fltr->y, (int)owd_fltr->s_exp);
/* Ok: print data if asked to, or don't even format_TimeInternal */
if (pp_diag_allow(ppi, servo, 2)) {
char s[24];
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment