use std::cmp::Ordering;
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct DisciplineConfig {
pub makestep_threshold: Option<f64>,
pub makestep_limit: u32,
pub max_slew_ppm: f64,
pub max_freq_ppm: f64,
pub min_poll: i8,
pub max_poll: i8,
pub iburst: bool,
pub freq_integral_gain: f64,
pub poll_down_noise_ratio: f64,
pub poll_up_streak: u32,
pub weight_floor_ratio: f64,
pub offset_weight_floor_ratio: f64,
pub offset_age_halflife_s: f64,
pub offset_weight_dispersion_k: f64,
pub slope_density_weighting: bool,
pub corr_time_s: f64,
pub adaptive_window: bool,
pub corr_time_max_s: f64,
pub leap_mode: LeapMode,
pub max_change_s: Option<f64>,
pub max_change_start: u32,
pub max_change_ignore: i32,
pub corr_time_ratio: f64,
}
impl Default for DisciplineConfig {
fn default() -> Self {
DisciplineConfig {
makestep_threshold: Some(1.0),
makestep_limit: 3,
max_slew_ppm: 83_333.0,
max_freq_ppm: 500.0,
min_poll: 6,
max_poll: 10,
iburst: true,
freq_integral_gain: FREQ_INTEGRAL_GAIN,
poll_down_noise_ratio: POLL_DOWN_NOISE_RATIO,
poll_up_streak: POLL_UP_STREAK,
weight_floor_ratio: crate::filter::WEIGHT_FLOOR_RATIO,
offset_weight_floor_ratio: crate::filter::OFFSET_WEIGHT_FLOOR_RATIO,
offset_age_halflife_s: f64::INFINITY,
offset_weight_dispersion_k: 0.0,
slope_density_weighting: false,
corr_time_s: 0.0,
corr_time_ratio: 0.0,
adaptive_window: true,
corr_time_max_s: CORR_TIME_MAX_S,
leap_mode: LeapMode::Slew,
max_change_s: None,
max_change_start: 1,
max_change_ignore: 2,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum ClockCommand {
Step { add_seconds: f64 },
Slew {
freq_ppm: f64,
drain_offset: f64,
drain_rate_ppm: f64,
},
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub struct Plan {
pub command: ClockCommand,
pub next_poll_s: f64,
pub reset_register: bool,
pub verdict: ChangeVerdict,
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub enum LeapMode {
Slew,
Step,
Ignore,
}
#[derive(Clone, Copy, Debug, PartialEq)]
pub enum ChangeVerdict {
Accepted,
Refused { offset_s: f64, seen: u32 },
GiveUp { offset_s: f64 },
}
const IBURST_COUNT: u32 = 4;
const IBURST_SPACING_S: f64 = 2.0;
const CORR_TIME_RATIO: f64 = 1.0;
const ACQUIRE_CORR_RATIO: f64 = 1.0;
const ACQUIRE_NOISE_MULTIPLE: f64 = 10.0;
const ACQUIRE_UPDATES: u32 = 8;
const ACQUIRE_SLEW_SHARE: f64 = 0.25;
const ACQUIRE_FULL_SPEED_CONFIDENCE: f64 = 10_000.0;
const FREQ_TRUST_SLEW_SHARE: f64 = 0.25;
const MAX_ACQUIRE_BURST: u32 = 16;
const FREQ_INTEGRAL_GAIN: f64 = 0.0;
const POLL_DOWN_NOISE_RATIO: f64 = 10.0;
const CORR_TIME_MAX_S: f64 = 128.0;
const LEAP_EXEMPTION_S: f64 = 2.0;
const POLL_UP_STREAK: u32 = 3;
const OFFSET_EWMA_ALPHA: f64 = 0.25;
const ACQUIRE_DONE_S: f64 = 1e-3;
#[derive(Clone, Debug)]
pub struct Discipline {
cfg: DisciplineConfig,
freq_ppm: f64,
updates: u32,
poll: i8,
stable_streak: u32,
iburst_left: u32,
last_drain_share: f64,
burst_used: u32,
offset_ewma: f64,
ewma_seeded: bool,
change_refusals: u32,
}
impl Discipline {
pub fn new(cfg: DisciplineConfig) -> Self {
let iburst_left = if cfg.iburst { IBURST_COUNT } else { 0 };
Discipline {
cfg,
freq_ppm: 0.0,
updates: 0,
poll: cfg.min_poll,
stable_streak: 0,
iburst_left,
last_drain_share: 0.0,
burst_used: 0,
offset_ewma: 0.0,
ewma_seeded: false,
change_refusals: 0,
}
}
fn acquire_share(&self, offset: f64, noise: f64) -> f64 {
let confidence = offset.abs() / noise.max(1e-9);
if confidence >= ACQUIRE_FULL_SPEED_CONFIDENCE {
1.0
} else {
ACQUIRE_SLEW_SHARE
}
}
pub fn freq_ppm(&self) -> f64 {
self.freq_ppm
}
pub fn poll_log2(&self) -> i8 {
self.poll
}
pub fn on_estimate(&mut self, offset: f64, freq_ppm_meas: Option<f64>, offset_sd: f64) -> Plan {
self.on_estimate_with_leap(offset, freq_ppm_meas, offset_sd, false)
}
pub fn on_estimate_with_leap(
&mut self,
offset: f64,
freq_ppm_meas: Option<f64>,
offset_sd: f64,
leap_pending: bool,
) -> Plan {
self.updates += 1;
let leap_exempt = leap_pending
&& self.cfg.leap_mode != LeapMode::Ignore
&& offset.abs() <= LEAP_EXEMPTION_S;
if leap_exempt && self.cfg.leap_mode == LeapMode::Step {
self.stable_streak = 0;
self.change_refusals = 0;
return Plan {
command: ClockCommand::Step {
add_seconds: offset,
},
next_poll_s: self.take_poll_interval(),
reset_register: true,
verdict: ChangeVerdict::Accepted,
};
}
if let Some(limit) = self.cfg.max_change_s
&& !leap_exempt
&& self.updates > self.cfg.max_change_start
&& !matches!(
offset.abs().partial_cmp(&limit),
Some(Ordering::Less | Ordering::Equal)
)
{
self.change_refusals = self.change_refusals.saturating_add(1);
let spent = self.cfg.max_change_ignore >= 0
&& self.change_refusals as i64 > i64::from(self.cfg.max_change_ignore);
self.stable_streak = 0;
return Plan {
command: ClockCommand::Slew {
freq_ppm: self.freq_ppm,
drain_offset: 0.0,
drain_rate_ppm: 0.0,
},
next_poll_s: self.take_poll_interval(),
reset_register: false,
verdict: if spent {
ChangeVerdict::GiveUp { offset_s: offset }
} else {
ChangeVerdict::Refused {
offset_s: offset,
seen: self.change_refusals,
}
},
};
}
self.change_refusals = 0;
if let Some(threshold) = self.cfg.makestep_threshold
&& offset.abs() > threshold
&& self.updates <= self.cfg.makestep_limit
{
self.stable_streak = 0;
return Plan {
command: ClockCommand::Step {
add_seconds: offset,
},
next_poll_s: self.take_poll_interval(),
reset_register: true,
verdict: ChangeVerdict::Accepted,
};
}
let hauling = self.last_drain_share > FREQ_TRUST_SLEW_SHARE;
if let Some(fm) = freq_ppm_meas
&& !hauling
{
self.freq_ppm =
(self.freq_ppm + fm).clamp(-self.cfg.max_freq_ppm, self.cfg.max_freq_ppm);
}
let noise = offset_sd.max(1e-7);
if self.ewma_seeded {
self.offset_ewma =
(1.0 - OFFSET_EWMA_ALPHA) * self.offset_ewma + OFFSET_EWMA_ALPHA * offset;
} else {
self.offset_ewma = offset;
self.ewma_seeded = true;
}
if self.cfg.freq_integral_gain != 0.0
&& self.updates > ACQUIRE_UPDATES
&& self.offset_ewma.abs() > noise
{
let poll_now = self.peek_poll_interval();
let implied_freq_ppm = (self.offset_ewma / (CORR_TIME_RATIO * poll_now)) * 1e6;
self.freq_ppm = (self.freq_ppm + self.cfg.freq_integral_gain * implied_freq_ppm)
.clamp(-self.cfg.max_freq_ppm, self.cfg.max_freq_ppm);
}
if offset.abs() < 2.0 * noise {
self.stable_streak += 1;
if self.stable_streak >= self.cfg.poll_up_streak && self.poll < self.cfg.max_poll {
self.poll += 1;
self.stable_streak = 0;
}
} else {
self.stable_streak = 0;
if offset.abs() > self.cfg.poll_down_noise_ratio * noise
&& self.poll > self.cfg.min_poll
{
self.poll -= 1;
}
}
if self.iburst_left == 0
&& self.cfg.iburst
&& self.burst_used < MAX_ACQUIRE_BURST
&& offset.abs() > ACQUIRE_DONE_S.max(2.0 * noise)
{
self.iburst_left = 1;
}
let poll_s = self.peek_poll_interval();
let acquiring = self.updates <= ACQUIRE_UPDATES;
let wanted_rate_ppm = if acquiring && offset.abs() > ACQUIRE_NOISE_MULTIPLE * noise {
((offset.abs() / (ACQUIRE_CORR_RATIO * poll_s)) * 1e6)
.min(self.cfg.max_slew_ppm * self.acquire_share(offset, noise))
} else {
let ratio = if self.cfg.corr_time_ratio > 0.0 {
self.cfg.corr_time_ratio
} else {
CORR_TIME_RATIO
};
let corr_time = if self.cfg.corr_time_s > 0.0 {
self.cfg.corr_time_s
} else {
(ratio * poll_s).min(self.cfg.corr_time_max_s)
};
(offset.abs() / corr_time) * 1e6
};
let drain_rate_ppm = wanted_rate_ppm.min(self.cfg.max_slew_ppm);
self.last_drain_share = if self.cfg.max_slew_ppm > 0.0 {
drain_rate_ppm / self.cfg.max_slew_ppm
} else {
0.0
};
Plan {
command: ClockCommand::Slew {
freq_ppm: self.freq_ppm,
drain_offset: offset,
drain_rate_ppm,
},
next_poll_s: self.take_poll_interval(),
reset_register: false,
verdict: ChangeVerdict::Accepted,
}
}
pub fn retry_interval_s(&self) -> f64 {
self.peek_poll_interval()
}
fn peek_poll_interval(&self) -> f64 {
if self.iburst_left > 0 {
IBURST_SPACING_S
} else {
2f64.powi(self.poll as i32)
}
}
fn take_poll_interval(&mut self) -> f64 {
if self.iburst_left > 0 {
self.iburst_left -= 1;
self.burst_used += 1;
IBURST_SPACING_S
} else {
2f64.powi(self.poll as i32)
}
}
}
#[cfg(test)]
mod acquisition_tests {
use super::*;
fn acquiring() -> Discipline {
Discipline::new(DisciplineConfig {
makestep_threshold: None,
min_poll: 4, iburst: true,
..DisciplineConfig::default()
})
}
#[test]
fn the_burst_continues_while_a_correction_is_outstanding() {
let mut d = acquiring();
let mut plan = None;
for _ in 0..IBURST_COUNT + 3 {
plan = Some(d.on_estimate(0.010, None, 1e-6));
}
let next = plan.expect("a plan").next_poll_s;
assert!(
next <= IBURST_SPACING_S,
"burst ended with 10 ms still outstanding: next poll {next} s"
);
}
#[test]
fn the_burst_ends_once_the_offset_is_small() {
let mut d = acquiring();
let mut plan = None;
for _ in 0..IBURST_COUNT + 3 {
plan = Some(d.on_estimate(1e-6, None, 1e-6));
}
let next = plan.expect("a plan").next_poll_s;
assert!(
next > IBURST_SPACING_S,
"burst kept running on a converged clock: next poll {next} s"
);
}
#[test]
fn the_extended_burst_is_bounded() {
let mut d = acquiring();
let mut plan = None;
for _ in 0..MAX_ACQUIRE_BURST * 3 {
plan = Some(d.on_estimate(0.010, None, 1e-6));
}
let next = plan.expect("a plan").next_poll_s;
assert!(
next > IBURST_SPACING_S,
"burst never backed off despite never converging: next poll {next} s"
);
}
}
#[cfg(test)]
mod leap_tests {
use super::*;
fn cfg(mode: LeapMode, max_change: Option<f64>) -> DisciplineConfig {
DisciplineConfig {
leap_mode: mode,
max_change_s: max_change,
max_change_start: 1,
max_change_ignore: 2,
makestep_threshold: Some(1.0),
makestep_limit: 3,
..DisciplineConfig::default()
}
}
#[test]
fn an_announced_leap_does_not_trip_the_change_guard() {
let mut d = Discipline::new(cfg(LeapMode::Slew, Some(0.1)));
d.on_estimate_with_leap(0.0001, None, 1e-6, false); for _ in 0..5 {
let plan = d.on_estimate_with_leap(1.0, None, 1e-6, true);
assert_eq!(
plan.verdict,
ChangeVerdict::Accepted,
"an announced leap second was refused by the change guard"
);
}
}
#[test]
fn the_same_offset_unannounced_is_still_refused() {
let mut d = Discipline::new(cfg(LeapMode::Slew, Some(0.1)));
d.on_estimate_with_leap(0.0001, None, 1e-6, false);
assert!(matches!(
d.on_estimate_with_leap(1.0, None, 1e-6, false).verdict,
ChangeVerdict::Refused { .. }
));
}
#[test]
fn an_announcement_does_not_excuse_an_arbitrary_correction() {
let mut d = Discipline::new(cfg(LeapMode::Slew, Some(0.1)));
d.on_estimate_with_leap(0.0001, None, 1e-6, false);
assert!(
matches!(
d.on_estimate_with_leap(3600.0, None, 1e-6, true).verdict,
ChangeVerdict::Refused { .. }
),
"the leap bit was used to smuggle a correction past the guard"
);
}
#[test]
fn step_mode_steps_the_second() {
let mut d = Discipline::new(cfg(LeapMode::Step, None));
for _ in 0..6 {
d.on_estimate_with_leap(0.0001, None, 1e-6, false);
}
let plan = d.on_estimate_with_leap(1.0, None, 1e-6, true);
match plan.command {
ClockCommand::Step { add_seconds } => {
assert!((add_seconds - 1.0).abs() < 1e-9);
assert!(plan.reset_register, "a step invalidates stored samples");
}
other => panic!("expected a step, got {other:?}"),
}
}
#[test]
fn ignore_mode_treats_a_leap_as_an_ordinary_offset() {
let mut d = Discipline::new(cfg(LeapMode::Ignore, Some(0.1)));
d.on_estimate_with_leap(0.0001, None, 1e-6, false);
assert!(matches!(
d.on_estimate_with_leap(1.0, None, 1e-6, true).verdict,
ChangeVerdict::Refused { .. }
));
}
#[test]
fn slew_is_the_default_and_does_not_step() {
assert_eq!(DisciplineConfig::default().leap_mode, LeapMode::Slew);
let mut d = Discipline::new(cfg(LeapMode::Slew, None));
for _ in 0..6 {
d.on_estimate_with_leap(0.0001, None, 1e-6, false);
}
let plan = d.on_estimate_with_leap(1.0, None, 1e-6, true);
assert!(
matches!(plan.command, ClockCommand::Slew { .. }),
"slew mode stepped the clock"
);
}
#[test]
fn no_announcement_is_the_old_behaviour() {
let mut a = Discipline::new(cfg(LeapMode::Slew, None));
let mut b = Discipline::new(cfg(LeapMode::Slew, None));
for i in 0..8 {
let off = 0.001 * f64::from(i);
let p = a.on_estimate(off, None, 1e-6);
let q = b.on_estimate_with_leap(off, None, 1e-6, false);
assert_eq!(p.command, q.command);
assert_eq!(p.next_poll_s, q.next_poll_s);
}
}
}
#[cfg(test)]
mod max_change_tests {
use super::*;
fn guarded(limit: f64, start: u32, ignore: i32) -> Discipline {
Discipline::new(DisciplineConfig {
max_change_s: Some(limit),
max_change_start: start,
max_change_ignore: ignore,
makestep_threshold: Some(1.0),
makestep_limit: 3,
..DisciplineConfig::default()
})
}
#[test]
fn no_limit_by_default() {
let mut d = Discipline::new(DisciplineConfig::default());
let plan = d.on_estimate(86_400.0, None, 1e-6);
assert_eq!(plan.verdict, ChangeVerdict::Accepted);
assert!(
matches!(plan.command, ClockCommand::Step { .. }),
"with no limit configured a large offset must still be corrected"
);
}
#[test]
fn the_first_correction_is_still_allowed_through() {
let mut d = guarded(1000.0, 1, 2);
let plan = d.on_estimate(50_000.0, None, 1e-6);
assert_eq!(
plan.verdict,
ChangeVerdict::Accepted,
"a machine with a dead clock must still be able to set it once"
);
assert!(matches!(plan.command, ClockCommand::Step { .. }));
}
#[test]
fn a_large_correction_is_refused_and_changes_nothing() {
let mut d = guarded(1000.0, 1, 5);
d.on_estimate(0.0001, None, 1e-6); let plan = d.on_estimate(50_000.0, None, 1e-6); match plan.verdict {
ChangeVerdict::Refused { offset_s, seen } => {
assert_eq!(seen, 1);
assert!((offset_s - 50_000.0).abs() < 1e-9);
}
other => panic!("expected a refusal, got {other:?}"),
}
match plan.command {
ClockCommand::Slew {
drain_offset,
drain_rate_ppm,
..
} => {
assert_eq!(
drain_offset, 0.0,
"a refused correction still moved the clock"
);
assert_eq!(drain_rate_ppm, 0.0);
}
other => panic!("a refusal must not step or drain: {other:?}"),
}
}
#[test]
fn a_good_update_clears_the_run() {
let mut d = guarded(1000.0, 1, 2);
d.on_estimate(0.0001, None, 1e-6);
assert!(matches!(
d.on_estimate(50_000.0, None, 1e-6).verdict,
ChangeVerdict::Refused { seen: 1, .. }
));
assert_eq!(
d.on_estimate(0.0001, None, 1e-6).verdict,
ChangeVerdict::Accepted
);
assert!(
matches!(
d.on_estimate(50_000.0, None, 1e-6).verdict,
ChangeVerdict::Refused { seen: 1, .. }
),
"the refusal count did not reset after an accepted update"
);
}
#[test]
fn persistent_refusal_gives_up() {
let mut d = guarded(1000.0, 1, 2);
d.on_estimate(0.0001, None, 1e-6);
for expected in 1..=2 {
assert!(matches!(
d.on_estimate(50_000.0, None, 1e-6).verdict,
ChangeVerdict::Refused { seen, .. } if seen == expected
));
}
assert!(
matches!(
d.on_estimate(50_000.0, None, 1e-6).verdict,
ChangeVerdict::GiveUp { .. }
),
"the allowance was spent and the daemon did not give up"
);
}
#[test]
fn a_negative_allowance_never_gives_up() {
let mut d = guarded(1000.0, 1, -1);
d.on_estimate(0.0001, None, 1e-6);
for _ in 0..50 {
assert!(matches!(
d.on_estimate(50_000.0, None, 1e-6).verdict,
ChangeVerdict::Refused { .. }
));
}
}
#[test]
fn a_correction_exactly_at_the_limit_is_allowed() {
let mut d = guarded(1000.0, 1, 2);
d.on_estimate(0.0001, None, 1e-6);
assert_eq!(
d.on_estimate(1000.0, None, 1e-6).verdict,
ChangeVerdict::Accepted
);
assert_eq!(
d.on_estimate(-1000.0, None, 1e-6).verdict,
ChangeVerdict::Accepted,
"the limit is on the magnitude, so it must be symmetric"
);
}
#[test]
fn a_nonsense_estimate_is_refused() {
for bad in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
let mut d = guarded(1000.0, 1, 5);
d.on_estimate(0.0001, None, 1e-6);
let plan = d.on_estimate(bad, None, 1e-6);
assert!(
matches!(plan.verdict, ChangeVerdict::Refused { .. }),
"an estimate of {bad} was not refused"
);
match plan.command {
ClockCommand::Slew {
freq_ppm,
drain_offset,
drain_rate_ppm,
} => {
assert!(freq_ppm.is_finite(), "a refusal emitted {freq_ppm} ppm");
assert_eq!(drain_offset, 0.0);
assert_eq!(drain_rate_ppm, 0.0);
}
other => panic!("expected a hold, got {other:?}"),
}
}
}
#[test]
fn a_zero_start_guards_immediately() {
let mut d = guarded(1.0, 0, 5);
assert!(
matches!(
d.on_estimate(500.0, None, 1e-6).verdict,
ChangeVerdict::Refused { seen: 1, .. }
),
"with start = 0 even the first correction must be checked"
);
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn big_initial_offset_is_stepped() {
let mut d = Discipline::new(DisciplineConfig::default());
let plan = d.on_estimate(120.0, None, 1e-4);
assert!(matches!(
plan.command,
ClockCommand::Step { add_seconds } if (add_seconds - 120.0).abs() < 1e-9
));
assert!(plan.reset_register);
}
#[test]
fn step_window_closes() {
let mut d = Discipline::new(DisciplineConfig::default());
for _ in 0..3 {
let _ = d.on_estimate(0.0001, None, 1e-4);
}
let plan = d.on_estimate(5.0, None, 1e-4);
assert!(matches!(plan.command, ClockCommand::Slew { .. }));
}
#[test]
fn freq_accumulates_and_clamps() {
let mut d = Discipline::new(DisciplineConfig::default());
let _ = d.on_estimate(1e-4, Some(100.0), 1e-4);
assert!((d.freq_ppm() - 100.0).abs() < 1e-9);
let _ = d.on_estimate(1e-4, Some(1000.0), 1e-4);
assert!((d.freq_ppm() - 500.0).abs() < 1e-9, "clamped at max_freq");
}
#[test]
fn iburst_then_normal_cadence() {
let mut d = Discipline::new(DisciplineConfig::default());
let mut intervals = Vec::new();
for _ in 0..6 {
let plan = d.on_estimate(1e-5, None, 1e-4);
intervals.push(plan.next_poll_s);
}
assert!(intervals[..4].iter().all(|&i| i == 2.0), "{intervals:?}");
assert!(intervals[4] >= 64.0, "{intervals:?}");
}
#[test]
fn closed_loop_converges() {
let mut d = Discipline::new(DisciplineConfig {
iburst: false,
makestep_threshold: None,
..DisciplineConfig::default()
});
let mut clock_err_s = 0.030_f64; let base_freq_ppm = 40.0;
let mut t = 0.0;
for _ in 0..60 {
let offset = -clock_err_s;
let slope_ppm = -(base_freq_ppm + d.freq_ppm());
let plan = d.on_estimate(offset, Some(slope_ppm), 1e-5);
let dt = plan.next_poll_s;
if let ClockCommand::Slew {
freq_ppm,
drain_offset,
drain_rate_ppm,
} = plan.command
{
let drift = (base_freq_ppm + freq_ppm) * 1e-6 * dt;
let max_drain = drain_rate_ppm * 1e-6 * dt;
let drain = drain_offset.abs().min(max_drain) * drain_offset.signum();
clock_err_s += drift + drain;
}
t += dt;
}
assert!(
clock_err_s.abs() < 1e-4,
"did not converge: err {clock_err_s} at t {t}"
);
assert!(
(d.freq_ppm() + 40.0).abs() < 2.0,
"freq not learned (want ~-40): {}",
d.freq_ppm()
);
}
}