diff --git a/refclock.c b/refclock.c index f0e2f10..cfa958c 100644 --- a/refclock.c +++ b/refclock.c @@ -448,20 +448,24 @@ accumulate_sample(RCL_Instance instance, struct timespec *sample_time, double of } int -RCL_AddSample(RCL_Instance instance, struct timespec *sample_time, double offset, int leap) +RCL_AddSample(RCL_Instance instance, struct timespec *sample_time, + struct timespec *ref_time, int leap) { - double correction, dispersion; + double correction, dispersion, raw_offset, offset; struct timespec cooked_time; if (instance->pps_forced) - return RCL_AddPulse(instance, sample_time, -offset); + return RCL_AddPulse(instance, sample_time, + 1.0e-9 * (sample_time->tv_nsec - ref_time->tv_nsec)); + + raw_offset = UTI_DiffTimespecsToDouble(ref_time, sample_time); LCL_GetOffsetCorrection(sample_time, &correction, &dispersion); UTI_AddDoubleToTimespec(sample_time, correction, &cooked_time); dispersion += instance->precision; /* Make sure the timestamp and offset provided by the driver are sane */ - if (!UTI_IsTimeOffsetSane(sample_time, offset) || + if (!UTI_IsTimeOffsetSane(sample_time, raw_offset) || !valid_sample_time(instance, &cooked_time)) return 0; @@ -476,18 +480,24 @@ RCL_AddSample(RCL_Instance instance, struct timespec *sample_time, double offset return 0; } + /* Calculate offset = raw_offset - correction + instance->offset + in parts to avoid loss of precision if there are large differences */ + offset = ref_time->tv_sec - sample_time->tv_sec - + (time_t)correction + (time_t)instance->offset; + offset += 1.0e-9 * (ref_time->tv_nsec - sample_time->tv_nsec) - + (correction - (time_t)correction) + (instance->offset - (time_t)instance->offset); + if (instance->tai && !convert_tai_offset(sample_time, &offset)) { DEBUG_LOG("refclock sample ignored unknown TAI offset"); return 0; } - if (!accumulate_sample(instance, &cooked_time, - offset - correction + instance->offset, dispersion)) + if (!accumulate_sample(instance, &cooked_time, offset, dispersion)) return 0; instance->pps_active = 0; - log_sample(instance, &cooked_time, 0, 0, offset, offset - correction + instance->offset, dispersion); + log_sample(instance, &cooked_time, 0, 0, raw_offset, offset, dispersion); /* for logging purposes */ if (!instance->driver->poll) diff --git a/refclock.h b/refclock.h index 985f0a8..7947b62 100644 --- a/refclock.h +++ b/refclock.h @@ -75,7 +75,8 @@ extern void *RCL_GetDriverData(RCL_Instance instance); extern char *RCL_GetDriverParameter(RCL_Instance instance); extern void RCL_CheckDriverOptions(RCL_Instance instance, const char **options); extern char *RCL_GetDriverOption(RCL_Instance instance, char *name); -extern int RCL_AddSample(RCL_Instance instance, struct timespec *sample_time, double offset, int leap); +extern int RCL_AddSample(RCL_Instance instance, struct timespec *sample_time, + struct timespec *ref_time, int leap); extern int RCL_AddPulse(RCL_Instance instance, struct timespec *pulse_time, double second); extern int RCL_AddCookedPulse(RCL_Instance instance, struct timespec *cooked_time, double second, double dispersion, double raw_correction); diff --git a/refclock_phc.c b/refclock_phc.c index e516b86..3efc667 100644 --- a/refclock_phc.c +++ b/refclock_phc.c @@ -143,7 +143,7 @@ static int phc_poll(RCL_Instance instance) { struct phc_instance *phc; struct timespec phc_ts, sys_ts, local_ts; - double offset, phc_err, local_err; + double phc_err, local_err; phc = (struct phc_instance *)RCL_GetDriverData(instance); @@ -157,11 +157,10 @@ static int phc_poll(RCL_Instance instance) return 0; } - offset = UTI_DiffTimespecsToDouble(&phc_ts, &sys_ts); + DEBUG_LOG("PHC offset: %+.9f err: %.9f", + UTI_DiffTimespecsToDouble(&phc_ts, &sys_ts), phc_err); - DEBUG_LOG("PHC offset: %+.9f err: %.9f", offset, phc_err); - - return RCL_AddSample(instance, &sys_ts, offset, LEAP_Normal); + return RCL_AddSample(instance, &sys_ts, &phc_ts, LEAP_Normal); } RefclockDriver RCL_PHC_driver = { diff --git a/refclock_shm.c b/refclock_shm.c index ed68095..ee13e87 100644 --- a/refclock_shm.c +++ b/refclock_shm.c @@ -95,7 +95,6 @@ static int shm_poll(RCL_Instance instance) { struct timespec receive_ts, clock_ts; struct shmTime t, *shm; - double offset; shm = (struct shmTime *)RCL_GetDriverData(instance); @@ -124,9 +123,8 @@ static int shm_poll(RCL_Instance instance) UTI_NormaliseTimespec(&clock_ts); UTI_NormaliseTimespec(&receive_ts); - offset = UTI_DiffTimespecsToDouble(&clock_ts, &receive_ts); - return RCL_AddSample(instance, &receive_ts, offset, t.leap); + return RCL_AddSample(instance, &receive_ts, &clock_ts, t.leap); } RefclockDriver RCL_SHM_driver = { diff --git a/refclock_sock.c b/refclock_sock.c index 86143cf..f0d91c5 100644 --- a/refclock_sock.c +++ b/refclock_sock.c @@ -60,8 +60,8 @@ struct sock_sample { static void read_sample(int sockfd, int event, void *anything) { + struct timespec sys_ts, ref_ts; struct sock_sample sample; - struct timespec ts; RCL_Instance instance; int s; @@ -86,13 +86,18 @@ static void read_sample(int sockfd, int event, void *anything) return; } - UTI_TimevalToTimespec(&sample.tv, &ts); - UTI_NormaliseTimespec(&ts); + UTI_TimevalToTimespec(&sample.tv, &sys_ts); + UTI_NormaliseTimespec(&sys_ts); + + if (!UTI_IsTimeOffsetSane(&sys_ts, sample.offset)) + return; + + UTI_AddDoubleToTimespec(&sys_ts, sample.offset, &ref_ts); if (sample.pulse) { - RCL_AddPulse(instance, &ts, sample.offset); + RCL_AddPulse(instance, &sys_ts, sample.offset); } else { - RCL_AddSample(instance, &ts, sample.offset, sample.leap); + RCL_AddSample(instance, &sys_ts, &ref_ts, sample.leap); } }