statime/filters/
basic.rs

1//! Implementation of [BasicFilter]
2
3use fixed::traits::LossyInto;
4
5use super::{Filter, FilterUpdate};
6#[allow(unused_imports)]
7use crate::float_polyfill::FloatPolyfill;
8use crate::{
9    port::Measurement,
10    time::{Duration, Time},
11    Clock,
12};
13
14#[derive(Debug)]
15struct PrevStepData {
16    event_time: Time,
17    offset: Duration,
18    correction: Duration,
19}
20
21/// A simple averaging filter.
22///
23/// This filter uses simple averaging to determine what the clock control
24/// outputs should be.
25#[derive(Debug)]
26pub struct BasicFilter {
27    last_step: Option<PrevStepData>,
28
29    offset_confidence: Duration,
30    freq_confidence: f64,
31
32    gain: f64,
33
34    cur_freq: f64,
35
36    last_offset: Duration,
37    last_delay: Duration,
38}
39
40impl Filter for BasicFilter {
41    type Config = f64;
42
43    fn new(gain: f64) -> Self {
44        Self {
45            last_step: None,
46            offset_confidence: Duration::from_nanos(1_000_000_000),
47            freq_confidence: 1e-4,
48            gain,
49            cur_freq: 0.0,
50            last_offset: Duration::ZERO,
51            last_delay: Duration::ZERO,
52        }
53    }
54
55    fn measurement<C: Clock>(&mut self, measurement: Measurement, clock: &mut C) -> FilterUpdate {
56        let mut update = FilterUpdate::default();
57
58        if let Some(delay) = measurement.delay {
59            self.last_delay = delay;
60            update.mean_delay = Some(delay);
61        }
62
63        if let Some(peer_delay) = measurement.peer_delay {
64            self.last_delay = peer_delay;
65            update.mean_delay = Some(peer_delay);
66        }
67
68        let Some(offset) = measurement.offset else {
69            // No measurement, so no further actions
70            return update;
71        };
72
73        self.last_offset = offset;
74
75        // Reset on too-large difference
76        if offset.abs() > Duration::from_nanos(1_000_000_000) {
77            log::debug!("Offset too large, stepping {}", offset);
78            self.offset_confidence = Duration::from_nanos(1_000_000_000);
79            self.freq_confidence = 1e-4;
80
81            if let Err(error) = clock.step_clock(-offset) {
82                log::error!("Could not step clock: {:?}", error);
83            }
84            return update;
85        }
86
87        // Determine offset
88        let mut clamped_offset = offset;
89        if offset.abs() > self.offset_confidence {
90            clamped_offset = offset.clamp(-self.offset_confidence, self.offset_confidence);
91            self.offset_confidence *= 2i32;
92        } else {
93            self.offset_confidence -= (self.offset_confidence - offset.abs()) * self.gain;
94        }
95
96        // And decide it's correction
97        let correction = -clamped_offset * self.gain;
98
99        let freq_corr = if let Some(last_step) = &self.last_step {
100            // Calculate interval for us
101            let interval_local: f64 =
102                (measurement.event_time - last_step.event_time - last_step.correction)
103                    .nanos()
104                    .lossy_into();
105            // and for the master
106            let interval_master: f64 = ((measurement.event_time - offset)
107                - (last_step.event_time - last_step.offset))
108                .nanos()
109                .lossy_into();
110
111            // get relative frequency difference
112            let mut freq_diff = interval_local / interval_master;
113            if (freq_diff - 1.0).abs() > self.freq_confidence {
114                freq_diff = freq_diff.clamp(1.0 - self.freq_confidence, 1.0 + self.freq_confidence);
115                self.freq_confidence *= 2.0;
116            } else {
117                self.freq_confidence -=
118                    (self.freq_confidence - (freq_diff - 1.0).abs()) * self.gain;
119            }
120
121            // and decide the correction (and convert to ppm)
122            -(freq_diff - 1.0) * self.gain * 0.1 * 1e6
123        } else {
124            // No data, so first run, so initialize
125            if let Err(error) = clock.set_frequency(0.0) {
126                log::error!("Could not initialize clock frequency: {:?}", error);
127            }
128            self.cur_freq = 0.0;
129            0.0
130        };
131
132        // unwrap is ok here since we always have an offset
133        log::info!(
134            "Offset to master: {:e}ns, corrected with phase change {:e}ns and freq change {:e}ppm",
135            offset.nanos(),
136            correction.nanos(),
137            freq_corr
138        );
139
140        // Store data for next time
141        self.last_step = Some(PrevStepData {
142            event_time: measurement.event_time,
143            offset,
144            correction,
145        });
146
147        if let Err(error) = clock.step_clock(correction) {
148            log::error!("Could not step clock: {:?}", error);
149        }
150        if let Err(error) = clock.set_frequency(self.cur_freq + freq_corr) {
151            log::error!("Could not adjust clock frequency: {:?}", error);
152        } else {
153            self.cur_freq += freq_corr;
154        }
155        update
156    }
157
158    fn demobilize<C: Clock>(self, _clock: &mut C) {
159        // ignore
160    }
161
162    fn update<C: Clock>(&mut self, _clock: &mut C) -> FilterUpdate {
163        // ignore
164        Default::default()
165    }
166
167    fn current_estimates(&self) -> super::FilterEstimate {
168        super::FilterEstimate {
169            offset_from_master: self.last_offset,
170            mean_delay: self.last_delay,
171        }
172    }
173}
statime/filters/basic.rs

statime/filters/
basic.rs
