230 lines
8.8 KiB
Python
230 lines
8.8 KiB
Python
"""Discrete event model of a 125 MHz GPS-disciplined PPS clock."""
|
|
|
|
from __future__ import annotations
|
|
|
|
from dataclasses import dataclass
|
|
|
|
import numpy as np
|
|
|
|
try:
|
|
from .pps_model import EpochResult, TRACKING_HZ, ClockTrajectory
|
|
except ImportError: # direct execution from the pps_python directory
|
|
from pps_model import EpochResult, TRACKING_HZ, ClockTrajectory
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class PIConfig:
|
|
natural_frequency_hz: float = 0.035
|
|
damping: float = 0.9
|
|
max_slew_ppm: float = 20.0
|
|
|
|
@property
|
|
def kp_per_s(self) -> float:
|
|
omega = 2.0 * np.pi * self.natural_frequency_hz
|
|
return 2.0 * self.damping * omega
|
|
|
|
@property
|
|
def ki_per_s2(self) -> float:
|
|
omega = 2.0 * np.pi * self.natural_frequency_hz
|
|
return omega * omega
|
|
|
|
|
|
@dataclass
|
|
class ControllerTrace:
|
|
name: str
|
|
time_s: np.ndarray
|
|
phase_error_s: np.ndarray
|
|
frequency_command: np.ndarray
|
|
initialized: np.ndarray
|
|
measurement_sequence: np.ndarray
|
|
measurement_error_s: np.ndarray
|
|
applied_error_s: np.ndarray
|
|
rejected_updates: int
|
|
|
|
def pps_samples(self, gps_tow_start_s: float) -> tuple[np.ndarray, np.ndarray]:
|
|
# For a near-unity clock rate, negative clock phase at a true integer
|
|
# second is the PPS edge displacement to first order. Quantize the
|
|
# physical edge to the 125 MHz output grid.
|
|
start = int(np.ceil(gps_tow_start_s + self.time_s[0]))
|
|
stop = int(np.floor(gps_tow_start_s + self.time_s[-1]))
|
|
gps_seconds = np.arange(start, stop + 1)
|
|
true_times = gps_seconds - gps_tow_start_s
|
|
phase = np.interp(true_times, self.time_s, self.phase_error_s)
|
|
valid = np.isfinite(phase)
|
|
edge_error = np.full_like(phase, np.nan, dtype=float)
|
|
edge_error[valid] = np.round(-phase[valid] * TRACKING_HZ) / TRACKING_HZ
|
|
return true_times, edge_error
|
|
|
|
|
|
class DisciplinedClock:
|
|
"""Logical GPS time accumulator driven by a free-running tracking clock."""
|
|
|
|
def __init__(
|
|
self,
|
|
tracking_clock: ClockTrajectory,
|
|
gps_tow_start_s: float,
|
|
config: PIConfig,
|
|
*,
|
|
mode: str,
|
|
) -> None:
|
|
if mode not in {"timestamped", "untagged"}:
|
|
raise ValueError(f"unknown controller mode: {mode}")
|
|
self.tracking_clock = tracking_clock
|
|
self.gps_tow_start_s = gps_tow_start_s
|
|
self.config = config
|
|
self.mode = mode
|
|
self.current_true_s = 0.0
|
|
self.local_gps_s = np.nan
|
|
self.frequency_command = 0.0
|
|
self.integrator = 0.0
|
|
self.initialized = False
|
|
self.last_applied_sequence = -1
|
|
self.last_measurement_true_s: float | None = None
|
|
self.capture_errors: dict[int, float] = {}
|
|
self.rejected_updates = 0
|
|
self.measurement_sequence: list[int] = []
|
|
self.measurement_error_s: list[float] = []
|
|
self.applied_error_s: list[float] = []
|
|
|
|
def advance(self, true_time_s: float) -> None:
|
|
if true_time_s < self.current_true_s - 1.0e-15:
|
|
raise ValueError("controller events are not time ordered")
|
|
if self.initialized and true_time_s > self.current_true_s:
|
|
raw_elapsed = float(
|
|
self.tracking_clock.seconds_at(true_time_s)
|
|
- self.tracking_clock.seconds_at(self.current_true_s)
|
|
)
|
|
self.local_gps_s += raw_elapsed * (1.0 + self.frequency_command)
|
|
self.current_true_s = true_time_s
|
|
|
|
def capture(self, epoch: EpochResult) -> None:
|
|
self.advance(epoch.reference_arrival_true_s)
|
|
if not self.initialized:
|
|
return
|
|
continuous_tick = float(
|
|
self.tracking_clock.ticks_at(epoch.reference_arrival_true_s)
|
|
)
|
|
tick_displacement_s = (
|
|
epoch.reference_tracking_tick - continuous_tick
|
|
) / TRACKING_HZ
|
|
local_at_timestamp = self.local_gps_s + tick_displacement_s * (
|
|
1.0 + self.frequency_command
|
|
)
|
|
error = epoch.gps_reference_estimate_s - local_at_timestamp
|
|
self.capture_errors[epoch.sequence] = error
|
|
self.measurement_sequence.append(epoch.sequence)
|
|
self.measurement_error_s.append(epoch.timing_measurement_error_s)
|
|
|
|
def _coarse_initialize(self, epoch: EpochResult) -> None:
|
|
if self.mode == "timestamped":
|
|
current_tick = float(self.tracking_clock.ticks_at(self.current_true_s))
|
|
elapsed_counter_s = (
|
|
current_tick - epoch.reference_tracking_tick
|
|
) / TRACKING_HZ
|
|
self.local_gps_s = epoch.gps_reference_estimate_s + elapsed_counter_s
|
|
else:
|
|
# This is the problematic interpretation: a reference belonging
|
|
# to the capture epoch is treated as valid at APB commit time.
|
|
self.local_gps_s = epoch.gps_reference_estimate_s
|
|
self.frequency_command = 0.0
|
|
self.integrator = 0.0
|
|
self.initialized = True
|
|
self.last_applied_sequence = epoch.sequence
|
|
self.last_measurement_true_s = epoch.reference_arrival_true_s
|
|
self.applied_error_s.append(0.0)
|
|
|
|
def apply(self, epoch: EpochResult) -> None:
|
|
self.advance(epoch.apply_true_s)
|
|
if epoch.sequence <= self.last_applied_sequence:
|
|
self.rejected_updates += 1
|
|
return
|
|
if not self.initialized:
|
|
self._coarse_initialize(epoch)
|
|
return
|
|
|
|
if self.mode == "timestamped":
|
|
error = self.capture_errors.pop(epoch.sequence, np.nan)
|
|
if not np.isfinite(error):
|
|
# The measurement may have been captured before coarse init.
|
|
self.last_applied_sequence = epoch.sequence
|
|
return
|
|
else:
|
|
# Old GPS reference incorrectly compared with local time at commit.
|
|
error = epoch.gps_reference_estimate_s - self.local_gps_s
|
|
|
|
if self.last_measurement_true_s is None:
|
|
update_interval = 0.2
|
|
else:
|
|
update_interval = max(
|
|
epoch.reference_arrival_true_s - self.last_measurement_true_s,
|
|
1.0e-6,
|
|
)
|
|
max_slew = self.config.max_slew_ppm * 1.0e-6
|
|
self.integrator += self.config.ki_per_s2 * error * update_interval
|
|
self.integrator = float(np.clip(self.integrator, -max_slew, max_slew))
|
|
command = self.integrator + self.config.kp_per_s * error
|
|
self.frequency_command = float(np.clip(command, -max_slew, max_slew))
|
|
self.last_measurement_true_s = epoch.reference_arrival_true_s
|
|
self.last_applied_sequence = epoch.sequence
|
|
self.applied_error_s.append(float(error))
|
|
|
|
|
|
def simulate_controller(
|
|
name: str,
|
|
mode: str,
|
|
epochs: list[EpochResult],
|
|
tracking_clock: ClockTrajectory,
|
|
gps_tow_start_s: float,
|
|
duration_s: float,
|
|
config: PIConfig,
|
|
*,
|
|
sample_period_s: float = 0.01,
|
|
) -> ControllerTrace:
|
|
controller = DisciplinedClock(
|
|
tracking_clock, gps_tow_start_s, config, mode=mode
|
|
)
|
|
sample_times = np.arange(0.0, duration_s + sample_period_s, sample_period_s)
|
|
# capture must precede apply; priorities also make coincident samples show
|
|
# the post-commit state deterministically.
|
|
events: list[tuple[float, int, str, EpochResult | None]] = []
|
|
for epoch in epochs:
|
|
events.append((epoch.reference_arrival_true_s, 0, "capture", epoch))
|
|
events.append((epoch.apply_true_s, 1, "apply", epoch))
|
|
for time_s in sample_times:
|
|
events.append((float(time_s), 2, "sample", None))
|
|
events.sort(key=lambda item: (item[0], item[1]))
|
|
|
|
phase_error = np.full(sample_times.shape, np.nan)
|
|
frequency_command = np.full(sample_times.shape, np.nan)
|
|
initialized = np.zeros(sample_times.shape, dtype=bool)
|
|
sample_index = 0
|
|
for event_time, _, event_type, epoch in events:
|
|
if event_time > duration_s + 1.0e-12:
|
|
continue
|
|
if event_type == "capture":
|
|
assert epoch is not None
|
|
controller.capture(epoch)
|
|
elif event_type == "apply":
|
|
assert epoch is not None
|
|
controller.apply(epoch)
|
|
else:
|
|
controller.advance(event_time)
|
|
if controller.initialized:
|
|
true_gps = gps_tow_start_s + event_time
|
|
phase_error[sample_index] = controller.local_gps_s - true_gps
|
|
frequency_command[sample_index] = controller.frequency_command
|
|
initialized[sample_index] = True
|
|
sample_index += 1
|
|
|
|
return ControllerTrace(
|
|
name=name,
|
|
time_s=sample_times,
|
|
phase_error_s=phase_error,
|
|
frequency_command=frequency_command,
|
|
initialized=initialized,
|
|
measurement_sequence=np.asarray(controller.measurement_sequence, dtype=int),
|
|
measurement_error_s=np.asarray(controller.measurement_error_s),
|
|
applied_error_s=np.asarray(controller.applied_error_s),
|
|
rejected_updates=controller.rejected_updates,
|
|
)
|