use crate::discipline::{ClockCommand, Discipline, DisciplineConfig, Plan};
use crate::filter::{Sample, SampleRegister};
pub const REGISTER_CAPACITY: usize = 64;
pub struct SyncController {
register: SampleRegister,
discipline: Discipline,
freq_cmd_ppm: f64,
drain_ppm: f64,
drain_remaining_s: f64,
last_plan_mono_s: Option<f64>,
}
pub struct ControllerStep {
pub plan: Plan,
pub applied_ppm: f64,
pub estimate_offset_s: f64,
pub estimate_freq_ppm: Option<f64>,
pub samples_used: usize,
}
impl SyncController {
pub fn new(config: DisciplineConfig) -> Self {
SyncController {
register: {
let mut r = SampleRegister::new(REGISTER_CAPACITY);
r.set_weight_floor_ratio(config.weight_floor_ratio);
r.set_offset_weight_floor_ratio(config.offset_weight_floor_ratio);
r
},
discipline: Discipline::new(config),
freq_cmd_ppm: 0.0,
drain_ppm: 0.0,
drain_remaining_s: 0.0,
last_plan_mono_s: None,
}
}
pub fn freq_ppm(&self) -> f64 {
self.freq_cmd_ppm
}
pub fn drain_ppm(&self) -> f64 {
self.drain_ppm
}
pub fn applied_ppm(&self) -> f64 {
self.freq_cmd_ppm + self.drain_ppm
}
pub fn poll_log2(&self) -> i8 {
self.discipline.poll_log2()
}
pub fn samples(&self) -> usize {
self.register.len()
}
pub fn preload_frequency(&mut self, freq_ppm: f64) {
self.freq_cmd_ppm = freq_ppm;
}
pub fn retry_interval_s(&self) -> f64 {
self.discipline.retry_interval_s()
}
pub fn drain_completes_at(&self) -> Option<f64> {
let last = self.last_plan_mono_s?;
if self.drain_ppm == 0.0 || self.drain_remaining_s <= 0.0 {
return None;
}
Some(last + self.drain_remaining_s / (self.drain_ppm.abs() * 1e-6))
}
pub fn poll_drain(&mut self, mono_now_s: f64) -> Option<ClockCommand> {
let completes_at = self.drain_completes_at()?;
if mono_now_s < completes_at {
return None;
}
let last = self.last_plan_mono_s?;
let consumed = self.drain_ppm * 1e-6 * (mono_now_s - last).max(0.0);
if consumed != 0.0 {
self.register.slew_samples(mono_now_s, 0.0, consumed);
}
self.drain_ppm = 0.0;
self.drain_remaining_s = 0.0;
self.last_plan_mono_s = Some(mono_now_s);
Some(ClockCommand::Slew {
freq_ppm: self.freq_cmd_ppm,
drain_offset: 0.0,
drain_rate_ppm: 0.0,
})
}
pub fn on_sample(&mut self, mono_now_s: f64, sample: Sample) -> ControllerStep {
let drained = match self.last_plan_mono_s {
Some(last) if self.drain_ppm != 0.0 => {
self.drain_ppm * 1e-6 * (mono_now_s - last).max(0.0)
}
_ => 0.0,
};
if drained != 0.0 {
self.register.slew_samples(mono_now_s, 0.0, drained);
self.drain_remaining_s = (self.drain_remaining_s - drained.abs()).max(0.0);
}
self.register.push(sample);
let (offset, freq, sd) = match self.register.regress(mono_now_s) {
Some(estimate) => (
estimate.offset,
estimate.freq_ppm,
estimate.offset_sd.max(1e-7),
),
None => match self.register.best() {
Some(best) => (best.offset, None, (best.delay / 2.0).max(1e-7)),
None => (0.0, None, 1e-3),
},
};
let plan = self.discipline.on_estimate(offset, freq, sd);
let freq_cmd_new = self.discipline.freq_ppm();
match plan.command {
ClockCommand::Step { add_seconds } => {
self.register.slew_samples(
mono_now_s,
freq_cmd_new - self.freq_cmd_ppm,
add_seconds,
);
self.drain_ppm = 0.0;
self.drain_remaining_s = 0.0;
}
ClockCommand::Slew {
drain_offset,
drain_rate_ppm,
..
} => {
self.register
.slew_samples(mono_now_s, freq_cmd_new - self.freq_cmd_ppm, 0.0);
self.drain_ppm = drain_rate_ppm.copysign(drain_offset);
self.drain_remaining_s = drain_offset.abs();
}
}
self.freq_cmd_ppm = freq_cmd_new;
self.last_plan_mono_s = Some(mono_now_s);
ControllerStep {
plan,
applied_ppm: self.freq_cmd_ppm + self.drain_ppm,
estimate_offset_s: offset,
estimate_freq_ppm: freq,
samples_used: self.register.len(),
}
}
}
#[cfg(test)]
mod tests {
use super::*;
fn closed_loop(base_freq_ppm: f64, initial_err_s: f64, polls: usize) -> f64 {
let mut controller = SyncController::new(DisciplineConfig {
iburst: false,
makestep_threshold: None,
..DisciplineConfig::default()
});
let mut err = initial_err_s;
let mut t = 0.0f64;
for _ in 0..polls {
let mono = t + err;
let step = controller.on_sample(
mono,
Sample {
t: mono,
offset: -err,
delay: 0.0002,
dispersion: 0.0,
},
);
let dt = step.plan.next_poll_s;
err += (base_freq_ppm + step.applied_ppm) * 1e-6 * dt;
t += dt;
}
err
}
#[test]
fn the_controller_converges_on_a_drifting_clock() {
let residual = closed_loop(40.0, 0.030, 80);
assert!(
residual.abs() < 1e-4,
"did not converge: {residual} s remaining"
);
}
#[test]
fn it_converges_from_either_direction() {
for (drift, start) in [
(40.0, 0.030),
(-40.0, -0.030),
(100.0, -0.050),
(-15.0, 0.010),
] {
let residual = closed_loop(drift, start, 120);
assert!(
residual.abs() < 1e-3,
"drift {drift} ppm from {start} s left {residual} s"
);
}
}
#[test]
fn frequency_and_drain_stay_separate() {
let mut controller = SyncController::new(DisciplineConfig {
iburst: false,
makestep_threshold: None,
..DisciplineConfig::default()
});
let step = controller.on_sample(
0.0,
Sample {
t: 0.0,
offset: 0.001,
delay: 0.0002,
dispersion: 0.0,
},
);
assert!(
controller.drain_ppm() != 0.0,
"a non-zero offset should start a drain"
);
assert_eq!(
step.applied_ppm,
controller.freq_ppm() + controller.drain_ppm(),
"the applied total must be exactly the two parts"
);
}
#[test]
fn a_failed_exchange_retries_at_the_burst_spacing_not_the_poll_interval() {
let controller = SyncController::new(DisciplineConfig {
min_poll: 4, iburst: true,
..DisciplineConfig::default()
});
let retry = controller.retry_interval_s();
assert!(
retry <= 4.0,
"a cold-start retry waited {retry} s; the burst spacing is the point"
);
}
#[test]
fn once_the_burst_is_spent_retries_use_the_poll_interval() {
let mut controller = SyncController::new(DisciplineConfig {
min_poll: 4,
iburst: true,
makestep_threshold: None,
..DisciplineConfig::default()
});
for i in 0..8 {
controller.on_sample(
i as f64 * 2.0,
Sample {
t: i as f64 * 2.0,
offset: 1e-6,
delay: 0.0002,
dispersion: 0.0,
},
);
}
assert!(
controller.retry_interval_s() >= 16.0,
"after the burst, retries must back off to the poll interval"
);
}
#[test]
fn a_drain_ends_when_its_budget_is_spent() {
let mut controller = SyncController::new(DisciplineConfig {
iburst: false,
makestep_threshold: None,
..DisciplineConfig::default()
});
controller.on_sample(
0.0,
Sample {
t: 0.0,
offset: 0.010,
delay: 0.0002,
dispersion: 0.0,
},
);
let ends = controller
.drain_completes_at()
.expect("a drain should be running");
assert!(ends > 0.0, "drain has no completion time");
assert!(controller.poll_drain(ends - 1e-6).is_none());
assert!(controller.applied_ppm() != 0.0);
assert!(controller.poll_drain(ends).is_some());
assert!(controller.poll_drain(ends + 1.0).is_none());
assert_eq!(
controller.drain_ppm(),
0.0,
"a spent drain must stop slewing the clock"
);
}
#[test]
fn a_late_retirement_books_what_the_clock_actually_received() {
let mut controller = SyncController::new(DisciplineConfig {
iburst: false,
makestep_threshold: None,
..DisciplineConfig::default()
});
controller.on_sample(
0.0,
Sample {
t: 0.0,
offset: 0.010,
delay: 0.0002,
dispersion: 0.0,
},
);
let rate = controller.drain_ppm();
let ends = controller.drain_completes_at().expect("a drain");
let late = 0.5;
controller.poll_drain(ends + late);
let step = controller.on_sample(
ends + late,
Sample {
t: ends + late,
offset: 0.0,
delay: 0.0002,
dispersion: 0.0,
},
);
let overrun = rate.abs() * 1e-6 * late;
assert!(
overrun > 1e-6,
"test is vacuous unless the overrun is meaningful"
);
assert!(
step.estimate_offset_s.abs() < 0.010,
"late retirement lost correction the clock had already received: estimate {} s",
step.estimate_offset_s
);
}
#[test]
fn a_preloaded_frequency_is_the_starting_point() {
let mut controller = SyncController::new(DisciplineConfig::default());
controller.preload_frequency(-12.5);
assert!((controller.freq_ppm() + 12.5).abs() < 1e-12);
}
}