Commit 7f163d5b authored by Aurelio Colosimo's avatar Aurelio Colosimo

servo partially implemented

All structs defined; pp_update_delay and pp_init_clock are the
first implemented servo functions
parent fa0377e2
......@@ -4,5 +4,6 @@
/* Architecture-specific defines, included by top-level stuff */
#include <arpa/inet.h> /* ntohs etc */
#include <stdlib.h> /* abs */
#endif /* __ARCH_H__ */
......@@ -39,6 +39,7 @@ int main(int argc, char **argv)
ppi->portDS = calloc(1, sizeof(*ppi->portDS));
ppi->timePropertiesDS = calloc(1, sizeof(*ppi->timePropertiesDS));
ppi->net_path = calloc(1, sizeof(*ppi->net_path));
ppi->servo = calloc(1, sizeof(*ppi->servo));
ppi->buf_out = calloc(1, PP_PACKET_SIZE);
ppi->buf_in = calloc(1, PP_PACKET_SIZE);
ppi->frgn_master = calloc(1, sizeof(*ppi->frgn_master));
......
......@@ -89,6 +89,47 @@ struct pp_timer {
uint32_t interval;
};
/*
* Servo. Structs which contain filters for delay average computation. They
* are used in servo.c src, where specific function for time setting of the
* machine are implemented.
*
* pp_ofm_fltr: The FIR filtering of the offset from master input is a simple,
* two-sample average
*
* pp_owd_fltr: It is a variable cutoff/delay low-pass, infinite impulse
* response (IIR) filter. The one-way delay filter has the difference equation:
* s*y[n] - (s-1)*y[n-1] = x[n]/2 + x[n-1]/2,
* where increasing the stiffness (s) lowers the cutoff and increases the delay.
*/
struct pp_ofm_fltr {
Integer32 nsec_prev;
Integer32 y;
};
struct pp_owd_fltr {
Integer32 nsec_prev;
Integer32 y;
Integer32 s_exp;
};
struct pp_servo {
/* TODO check. Which is the difference between m_to_s_dly (which
* comes from ptpd's master_to_slave_delay) and delay_ms (which comes
* from ptpd's delay_MS? Seems like ptpd actually uses only delay_MS.
* The same of course must be checked for their equivalents,
* s_to_m_dly and delay_sm
*/
TimeInternal m_to_s_dly;
TimeInternal s_to_m_dly;
TimeInternal delay_ms;
TimeInternal delay_sm;
Integer32 obs_drift;
struct pp_owd_fltr owd_fltr;
struct pp_ofm_fltr ofm_fltr;
};
/*
* Net Path. Struct which contains the network configuration parameters and
* the event/general channels (sockets on most platforms, see above)
......@@ -114,6 +155,7 @@ struct pp_instance {
void *ext_data; /* if protocol ext needs it */
struct pp_runtime_opts *rt_opts;
struct pp_net_path *net_path;
struct pp_servo *servo;
/* Data sets */
DSDefault *defaultDS;
......@@ -182,6 +224,8 @@ struct pp_instance {
#define NP(x) ((x)->net_path)
#define SRV(x) ((x)->servo)
/* The channel for an instance must be created and possibly destroyed. */
extern int pp_open_instance(struct pp_instance *ppi,
struct pp_runtime_opts *rt_opts);
......@@ -207,6 +251,8 @@ extern int pp_timer_expired(struct pp_timer *tm); /* returns 1 when expired */
/* Servo */
extern void pp_init_clock(struct pp_instance *ppi);
extern void pp_update_delay(struct pp_instance *ppi,
TimeInternal *correction_field);
extern void pp_update_offset(struct pp_instance *ppi,
TimeInternal *send_time,
TimeInternal *recv_time,
......
......@@ -5,10 +5,107 @@
#include <pptp/pptp.h>
void pp_init_clock(struct pp_instance *ppi)
{
/* TODO servo implement function*/
/* FIXME diag DBG("initClock\n"); */
/* clear vars */
set_TimeInternal(&SRV(ppi)->m_to_s_dly, 0, 0);
set_TimeInternal(&SRV(ppi)->s_to_m_dly, 0, 0);
SRV(ppi)->obs_drift = 0; /* clears clock servo accumulator (the
* I term) */
SRV(ppi)->owd_fltr.s_exp = 0; /* clears one-way delay filter */
/* level clock */
if (!OPTS(ppi)->no_adjust)
pp_adj_freq(0);
}
void pp_update_delay(struct pp_instance *ppi, TimeInternal *correction_field)
{
TimeInternal s_to_m_dly;
struct pp_owd_fltr *owd_fltr = &SRV(ppi)->owd_fltr;
/* calc 'slave to master' delay */
sub_TimeInternal(&s_to_m_dly, &ppi->delay_req_receive_time,
&ppi->delay_req_send_time);
if (OPTS(ppi)->max_dly) { /* If max_delay is 0 then it's OFF */
if (s_to_m_dly.seconds) {
/*FIXME diag
* INFO("updateDelay aborted, delay greater than 1"
" second.");
*/
return;
}
if (s_to_m_dly.nanoseconds > OPTS(ppi)->max_dly) {
/*FIXME diag
INFO("updateDelay aborted, delay %d greater than "
"administratively set maximum %d\n",
slave_to_master_delay.nanoseconds,
rtOpts->maxDelay);
*/
return;
}
}
if (OPTS(ppi)->ofst_first_updated) {
Integer16 s;
/* calc 'slave to_master' delay (master to slave delay is
* already computed in pp_update_offset)
*/
sub_TimeInternal(&SRV(ppi)->delay_sm,
&ppi->delay_req_receive_time,
&ppi->delay_req_send_time);
/* update 'one_way_delay' */
add_TimeInternal(&DSCUR(ppi)->meanPathDelay,
&SRV(ppi)->delay_sm, &SRV(ppi)->delay_ms);
/* Substract correction_field */
sub_TimeInternal(&DSCUR(ppi)->meanPathDelay,
&DSCUR(ppi)->meanPathDelay, correction_field);
/* Compute one-way delay */
DSCUR(ppi)->meanPathDelay.seconds /= 2;
DSCUR(ppi)->meanPathDelay.nanoseconds /= 2;
if (DSCUR(ppi)->meanPathDelay.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;
/* filter 'meanPathDelay' */
owd_fltr->y = (owd_fltr->s_exp - 1) *
owd_fltr->y / owd_fltr->s_exp +
(DSCUR(ppi)->meanPathDelay.nanoseconds / 2 +
owd_fltr->nsec_prev / 2) / owd_fltr->s_exp;
owd_fltr->nsec_prev = DSCUR(ppi)->meanPathDelay.nanoseconds;
DSCUR(ppi)->meanPathDelay.nanoseconds = owd_fltr->y;
/*FIXME diag
* DBGV("delay filter %d, %d\n", owd_filt->y, owd_fltr->s_exp);
*/
}
}
void pp_update_offset(struct pp_instance *ppi, TimeInternal *send_time,
......
......@@ -111,10 +111,7 @@ int pp_slave(struct pp_instance *ppi, unsigned char *pkt, int plen)
hdr->correctionfield,
&correction_field);
/* TODO servo
updateDelay(&ptpClock->owd_filt,
rtOpts,ptpClock, &correctionField);
*/
pp_update_delay(ppi, &correction_field);
ppi->log_min_delay_req_interval =
hdr->logMessageInterval;
......
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