diff --git a/samplefilt.c b/samplefilt.c index fe32b5c..9cb892d 100644 --- a/samplefilt.c +++ b/samplefilt.c @@ -410,6 +410,27 @@ SPF_GetFilteredSample(SPF_Instance filter, NTP_Sample *sample) return 1; } +/* ================================================== */ + +static int +get_first_last(SPF_Instance filter, int *first, int *last) +{ + if (filter->last < 0) + return 0; + + /* Always slew the last sample as it may be returned even if no new + samples were accumulated */ + if (filter->used > 0) { + *first = 0; + *last = filter->used - 1; + } else { + *first = *last = filter->last; + } + + return 1; +} + + /* ================================================== */ void @@ -418,18 +439,9 @@ SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double int i, first, last; double delta_time; - if (filter->last < 0) + if (!get_first_last(filter, &first, &last)) return; - /* Always slew the last sample as it may be returned even if no new - samples were accumulated */ - if (filter->used > 0) { - first = 0; - last = filter->used - 1; - } else { - first = last = filter->last; - } - for (i = first; i <= last; i++) { UTI_AdjustTimespec(&filter->samples[i].time, when, &filter->samples[i].time, &delta_time, dfreq, doffset); @@ -439,6 +451,20 @@ SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double /* ================================================== */ +void +SPF_CorrectOffset(SPF_Instance filter, double doffset) +{ + int i, first, last; + + if (!get_first_last(filter, &first, &last)) + return; + + for (i = first; i <= last; i++) + filter->samples[i].offset -= doffset; +} + +/* ================================================== */ + void SPF_AddDispersion(SPF_Instance filter, double dispersion) { diff --git a/samplefilt.h b/samplefilt.h index 3b4350a..e55a776 100644 --- a/samplefilt.h +++ b/samplefilt.h @@ -44,6 +44,7 @@ extern void SPF_DropSamples(SPF_Instance filter); extern int SPF_GetFilteredSample(SPF_Instance filter, NTP_Sample *sample); extern void SPF_SlewSamples(SPF_Instance filter, struct timespec *when, double dfreq, double doffset); +extern void SPF_CorrectOffset(SPF_Instance filter, double doffset); extern void SPF_AddDispersion(SPF_Instance filter, double dispersion); #endif diff --git a/test/unit/samplefilt.c b/test/unit/samplefilt.c index 513ff2c..20e6feb 100644 --- a/test/unit/samplefilt.c +++ b/test/unit/samplefilt.c @@ -69,6 +69,7 @@ test_unit(void) TEST_CHECK(!memcmp(&sample_in, &sample_out, sizeof (sample_in))); SPF_SlewSamples(filter, &sample_in.time, 0.0, 0.0); + SPF_CorrectOffset(filter, 0.0); SPF_AddDispersion(filter, 0.0); if (k + 1 < min_samples)