"""Bit-accurate event model for the adaptive PPS control path. The WLS/navigation model in :mod:`pps_model` validates timestamp ownership. This module focuses on the control corner cases seen on the board: noisy or missing WLS time observations, quality-driven coefficient changes, fixed-point rounding, lock acquisition/release and holdover/re-acquisition. """ from __future__ import annotations from dataclasses import dataclass from enum import IntEnum import math from typing import Iterable, Mapping TRACKING_HZ = 125_000_000 SPEED_OF_LIGHT_MPS = 299_792_458.0 REFERENCE_WATCHDOG_SEC = 2.0 MAX_PREDICTION_GAP_SEC = 30.0 Q32 = 1 << 32 Q46 = 1 << 46 MAX_SIGNED_64 = (1 << 63) - 1 MIN_SIGNED_64 = -(1 << 63) class Profile(IntEnum): HOLDOVER = 0 ROBUST = 1 PRECISION = 2 @dataclass(frozen=True) class QualityMetrics: tdop: float gdop: float clock_bias_sigma_m: float mean_cn0_dbhz: float minimum_cn0_dbhz: float chi_square_ratio: float velocity_residual_rms_mps: float used_satellites: int solution_valid: bool = True residual_test_valid: bool = True velocity_valid: bool = True fde_applied: bool = False @dataclass(frozen=True) class LoopProfile: natural_frequency_hz: float damping: float max_slew_ppm: float lock_acquire_ticks: int lock_release_ticks: int lock_acquire_updates: int software_bias_gain: float software_settle_updates: int def fixed_coefficients(self, update_period_s: float) -> tuple[int, int, int]: omega = 2.0 * math.pi * self.natural_frequency_hz kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46) ki_q46 = round( (omega * omega * update_period_s / TRACKING_HZ) * Q46 ) max_slew_q32 = round(self.max_slew_ppm * 1.0e-6 * Q32) return kp_q46, ki_q46, max_slew_q32 PROFILES = { Profile.HOLDOVER: LoopProfile( 0.0, 1.0, 20.0, 8, 128, 8, 1.0 / 32.0, 16 ), Profile.ROBUST: LoopProfile( 0.012, 1.0, 20.0, 8, 128, 8, 1.0 / 32.0, 16 ), Profile.PRECISION: LoopProfile( 0.035, 0.9, 20.0, 8, 128, 5, 1.0 / 32.0, 12 ), } ACQUIRE_PROFILE = LoopProfile( 0.12, 0.9, 20.0, 8, 128, 5, 1.0 / 16.0, 12 ) def _finite_metrics(metrics: QualityMetrics) -> bool: return all( math.isfinite(value) for value in ( metrics.tdop, metrics.gdop, metrics.clock_bias_sigma_m, metrics.mean_cn0_dbhz, metrics.minimum_cn0_dbhz, metrics.velocity_residual_rms_mps, ) ) def classify_quality(metrics: QualityMetrics) -> Profile: invalid = ( not _finite_metrics(metrics) or not metrics.solution_valid or metrics.used_satellites < 4 or metrics.tdop < 0.0 or metrics.gdop < 0.0 or metrics.clock_bias_sigma_m < 0.0 ) if invalid: return Profile.HOLDOVER residual_ok = ( metrics.residual_test_valid and math.isfinite(metrics.chi_square_ratio) and metrics.chi_square_ratio <= 1.0 ) or (not metrics.residual_test_valid and metrics.used_satellites == 4) robust = ( residual_ok and metrics.tdop <= 8.0 and metrics.gdop <= 12.0 and metrics.clock_bias_sigma_m <= 100.0 and metrics.mean_cn0_dbhz >= 22.0 and metrics.minimum_cn0_dbhz >= 14.0 and ( not metrics.velocity_valid or metrics.velocity_residual_rms_mps <= 3.0 ) ) if not robust: return Profile.HOLDOVER precision = ( metrics.used_satellites >= 6 and metrics.residual_test_valid and not metrics.fde_applied and metrics.tdop <= 3.0 and metrics.gdop <= 6.0 and metrics.clock_bias_sigma_m <= 35.0 and metrics.mean_cn0_dbhz >= 30.0 and metrics.minimum_cn0_dbhz >= 20.0 and metrics.chi_square_ratio <= 0.75 and metrics.velocity_valid and metrics.velocity_residual_rms_mps <= 1.0 ) return Profile.PRECISION if precision else Profile.ROBUST class QualitySelector: """Three-profile selector with asymmetric transition hysteresis.""" def __init__(self) -> None: self.profile = Profile.HOLDOVER self.candidate = Profile.HOLDOVER self.candidate_updates = 0 def update(self, metrics: QualityMetrics) -> tuple[Profile, bool]: target = classify_quality(metrics) if target == self.profile: self.candidate = target self.candidate_updates = 0 return self.profile, False if target != self.candidate: self.candidate = target self.candidate_updates = 1 else: self.candidate_updates += 1 required = 8 if target > self.profile else 3 if self.candidate_updates >= required: self.profile = target self.candidate_updates = 0 return self.profile, True return self.profile, False @dataclass(frozen=True) class Observation: time_s: float timing_error_s: float clock_drift_error_mps: float quality: QualityMetrics clock_drift_valid: bool = True reference_range_rate_mps: float = 0.0 reference_id: int = 1 reference_datum_offset_m: float = 0.0 @dataclass class AdaptiveTrace: time_s: list[float] phase_error_s: list[float] raw_measurement_error_s: list[float] software_filtered_error_s: list[float] raw_loop_error_s: list[float] control_loop_error_s: list[float] frequency_command_ppm: list[float] profile: list[Profile] locked: list[bool] coarse_commits: int normal_commits: int rejected_observations: int lock_events: int unlock_events: int def locked_fraction(self) -> float: return sum(self.locked) / len(self.locked) if self.locked else 0.0 def first_lock_time(self) -> float | None: for time_s, locked in zip(self.time_s, self.locked): if locked: return time_s return None def one_second_jitter_rms(self, *, after_s: float = 0.0) -> float: selected = [ (time_s, phase_s) for time_s, phase_s, locked in zip( self.time_s, self.phase_error_s, self.locked ) if locked and time_s >= after_s ] if len(selected) < 2: return math.nan differences = [] previous_index = 0 for index, (time_s, phase_s) in enumerate(selected): while ( previous_index < index and selected[previous_index][0] < time_s - 1.0 - 1.0e-9 ): previous_index += 1 if ( previous_index < index and abs(selected[previous_index][0] - (time_s - 1.0)) < 1.0e-9 ): differences.append(phase_s - selected[previous_index][1]) if not differences: return math.nan return math.sqrt( sum(value * value for value in differences) / len(differences) ) def one_second_edge_jitter_rms(self, *, after_s: float = 0.0) -> float: """Return 1 s PPS-edge jitter including the 125 MHz output grid.""" selected = dict(self.integer_second_edge_errors(after_s=after_s)) differences = [ edge_error - selected[round(time_s - 1.0, 9)] for time_s, edge_error in selected.items() if round(time_s - 1.0, 9) in selected ] if not differences: return math.nan return math.sqrt( sum(value * value for value in differences) / len(differences) ) def integer_second_edge_errors( self, *, after_s: float = 0.0, locked_only: bool = True, ) -> list[tuple[float, float]]: """Return physical PPS edge error samples on the 125 MHz grid. Period jitter alone hides low-frequency motion. Exposing the signed edge error lets stability analyses report absolute RMS, peak-to-peak wander and moving-average wander alongside cycle-to-cycle jitter. """ return [ ( round(time_s, 9), round(-phase_s * TRACKING_HZ) / TRACKING_HZ, ) for time_s, phase_s, locked in zip( self.time_s, self.phase_error_s, self.locked ) if (locked or not locked_only) and time_s >= after_s and abs(time_s - round(time_s)) < 1.0e-9 ] def phase_rms(self, *, after_s: float = 0.0) -> float: values = [ phase_s for time_s, phase_s, locked in zip( self.time_s, self.phase_error_s, self.locked ) if locked and time_s >= after_s ] if not values: return math.nan return math.sqrt(sum(value * value for value in values) / len(values)) def settling_time( self, *, rms_limit_s: float, window_s: float = 10.0, ) -> float | None: """First all-locked window whose phase RMS meets ``rms_limit_s``.""" for end_index, end_time_s in enumerate(self.time_s): start_time_s = end_time_s - window_s if start_time_s < 0.0: continue window = [ (phase_s, locked) for time_s, phase_s, locked in zip( self.time_s[: end_index + 1], self.phase_error_s[: end_index + 1], self.locked[: end_index + 1], ) if time_s >= start_time_s - 1.0e-9 ] if not window or not all(locked for _, locked in window): continue rms = math.sqrt( sum(phase_s * phase_s for phase_s, _ in window) / len(window) ) if rms <= rms_limit_s: return end_time_s return None class FixedPointAdaptivePps: """Event-driven model of the Q32/Q46 RTL plus the small clock-bias filter.""" def __init__( self, *, update_period_s: float = 0.2, oscillator_ppm: float = 1.65, adaptive: bool = True, software_filter: bool = True, receiver_clock_bias_m: float = 1200.0, receiver_clock_drift_mps: float = 475.0, profiles: Mapping[Profile, LoopProfile] | None = None, acquire_profile: LoopProfile | None = None, software_drift_gain: float = 1.0 / 4.0, software_fallback_drift_gain: float = 1.0 / 16.0, ) -> None: self.update_period_s = update_period_s self.oscillator_fraction = oscillator_ppm * 1.0e-6 self.adaptive = adaptive self.software_filter = software_filter self.receiver_clock_bias_m = receiver_clock_bias_m self.receiver_clock_drift_mps = receiver_clock_drift_mps self.profiles = dict(PROFILES if profiles is None else profiles) missing_profiles = set(Profile) - self.profiles.keys() if missing_profiles: raise ValueError(f"missing loop profiles: {sorted(missing_profiles)}") self.acquire_profile = ( ACQUIRE_PROFILE if acquire_profile is None else acquire_profile ) if not 0.0 < software_drift_gain <= 1.0: raise ValueError("software_drift_gain must be in (0, 1]") if not 0.0 < software_fallback_drift_gain <= 1.0: raise ValueError( "software_fallback_drift_gain must be in (0, 1]" ) self.software_drift_gain = software_drift_gain self.software_fallback_drift_gain = software_fallback_drift_gain self.selector = QualitySelector() self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION self.parameters = self.profiles[self.profile] self.kp_q46, self.ki_q46, self.max_slew_q32 = ( self.parameters.fixed_coefficients(update_period_s) ) self.current_time_s = 0.0 self.raw_tick_remainder = 0.0 self.phase_q32 = 0 self.rate_word_q32 = 0 self.integrator_q32 = 0 self.aligned = False self.locked = False self.lock_good_updates = 0 self.last_observation_time_s: float | None = None self.software_error_s = 0.0 self.software_bias_m = 0.0 self.software_clock_drift_mps = 0.0 self.software_previous_measurement_m = 0.0 self.software_drift_anchor_time_s = 0.0 self.software_drift_anchor_measurement_m = 0.0 self.software_drift_initialized = False self.software_updates = 0 self.software_reference_id: int | None = None self.reference_watchdog_holdover = True self.trace = AdaptiveTrace( [], [], [], [], [], [], [], [], [], 0, 0, 0, 0, 0 ) @staticmethod def _clamp(value: int, limit: int) -> int: return max(-limit, min(limit, value)) @staticmethod def _seconds_to_phase_q32(seconds: float) -> int: value = round(seconds * TRACKING_HZ * Q32) return max(MIN_SIGNED_64, min(MAX_SIGNED_64, value)) @staticmethod def _phase_q32_to_seconds(value: int) -> float: return value / (TRACKING_HZ * Q32) def _apply_profile(self, profile: Profile) -> None: self.profile = profile self._select_loop_parameters() def _select_loop_parameters(self) -> None: parameters = ( self.acquire_profile if self.adaptive and self.profile != Profile.HOLDOVER and not self.locked else self.profiles[self.profile] ) if parameters == self.parameters: return self.parameters = parameters self.kp_q46, self.ki_q46, self.max_slew_q32 = ( self.parameters.fixed_coefficients(self.update_period_s) ) def _advance(self, time_s: float) -> None: elapsed_s = time_s - self.current_time_s if elapsed_s < -1.0e-15: raise ValueError("observations are not time ordered") if elapsed_s <= 0.0: return true_ticks = elapsed_s * TRACKING_HZ raw_ticks_float = ( true_ticks * (1.0 + self.oscillator_fraction) + self.raw_tick_remainder ) raw_ticks = int(math.floor(raw_ticks_float)) self.raw_tick_remainder = raw_ticks_float - raw_ticks self.phase_q32 += ( raw_ticks * (Q32 + self.rate_word_q32) - round(true_ticks * Q32) ) self.phase_q32 = max( MIN_SIGNED_64, min(MAX_SIGNED_64, self.phase_q32) ) self.current_time_s = time_s def _software_filter_update(self, observation: Observation) -> tuple[bool, bool]: if ( not math.isfinite(observation.timing_error_s) or ( observation.clock_drift_valid and not math.isfinite(observation.clock_drift_error_mps) ) ): self.trace.rejected_observations += 1 return False, False if self.software_reference_id != observation.reference_id: # WLS bias has a reference-dependent datum. A satellite change is # a deliberate coarse re-anchor, never an outlier to interpolate. self.last_observation_time_s = None self.software_updates = 0 self.software_drift_initialized = False self.software_reference_id = observation.reference_id true_bias_m = ( self.receiver_clock_bias_m + observation.reference_datum_offset_m - observation.reference_range_rate_mps * observation.time_s ) measured_bias_m = ( true_bias_m - observation.timing_error_s * SPEED_OF_LIGHT_MPS ) measured_drift_mps = ( -observation.reference_range_rate_mps + observation.clock_drift_error_mps ) if self.last_observation_time_s is None: self.software_bias_m = measured_bias_m self.software_clock_drift_mps = ( measured_drift_mps if observation.clock_drift_valid else 0.0 ) self.software_previous_measurement_m = measured_bias_m self.software_drift_anchor_time_s = observation.time_s self.software_drift_anchor_measurement_m = measured_bias_m self.software_drift_initialized = observation.clock_drift_valid self.software_error_s = observation.timing_error_s self.software_updates = 1 self.last_observation_time_s = observation.time_s return True, True interval_s = observation.time_s - self.last_observation_time_s if interval_s < 0.04: self.trace.rejected_observations += 1 return False, False # The two-second reference watchdog controls PPS validity, not filter # state lifetime. Keeping the Doppler-predicted bias and the hardware # PI state across a bounded WLS scheduling/signal gap avoids a coarse # phase step when the same datum satellite returns. A reference # change is handled above and a genuinely long gap still re-anchors. if interval_s > MAX_PREDICTION_GAP_SEC: self.software_bias_m = measured_bias_m self.software_clock_drift_mps = ( measured_drift_mps if observation.clock_drift_valid else 0.0 ) self.software_previous_measurement_m = measured_bias_m self.software_drift_anchor_time_s = observation.time_s self.software_drift_anchor_measurement_m = measured_bias_m self.software_drift_initialized = observation.clock_drift_valid self.software_error_s = observation.timing_error_s self.software_updates = 1 self.last_observation_time_s = observation.time_s return True, True if not self.software_filter: self.software_bias_m = measured_bias_m self.software_clock_drift_mps = ( measured_drift_mps if observation.clock_drift_valid else self.software_clock_drift_mps ) self.software_previous_measurement_m = measured_bias_m self.software_drift_initialized |= observation.clock_drift_valid self.software_error_s = observation.timing_error_s self.software_updates += 1 self.last_observation_time_s = observation.time_s return True, False measurement_delta_m = ( measured_bias_m - self.software_previous_measurement_m ) if ( not observation.clock_drift_valid and abs(measurement_delta_m) > 10_000.0 * interval_s + 1500.0 ): self.trace.rejected_observations += 1 return False, False anchor_interval_s = ( observation.time_s - self.software_drift_anchor_time_s ) fallback_drift_mps = max( -10_000.0, min( 10_000.0, ( measured_bias_m - self.software_drift_anchor_measurement_m ) / anchor_interval_s if anchor_interval_s > 0.0 else measurement_delta_m / interval_s, ), ) if not self.software_drift_initialized: self.software_clock_drift_mps = fallback_drift_mps self.software_drift_initialized = True predicted_bias_m = ( self.software_bias_m + self.software_clock_drift_mps * interval_s ) innovation_m = measured_bias_m - predicted_bias_m if abs(innovation_m) > 1500.0: self.trace.rejected_observations += 1 return False, False innovation_m = max(-300.0, min(300.0, innovation_m)) self.software_bias_m = ( predicted_bias_m + self.parameters.software_bias_gain * innovation_m ) if observation.clock_drift_valid: self.software_clock_drift_mps += self.software_drift_gain * ( measured_drift_mps - self.software_clock_drift_mps ) else: if self.software_updates < self.parameters.software_settle_updates: self.software_clock_drift_mps = fallback_drift_mps else: self.software_clock_drift_mps += ( self.software_fallback_drift_gain * (fallback_drift_mps - self.software_clock_drift_mps) ) self.software_previous_measurement_m = measured_bias_m self.software_error_s = -( self.software_bias_m - true_bias_m ) / SPEED_OF_LIGHT_MPS self.software_updates += 1 self.last_observation_time_s = observation.time_s coarse = ( self.software_filter and self.software_updates < self.parameters.software_settle_updates ) return True, coarse def _normal_commit(self, measurement_error_q32: int) -> tuple[int, int]: raw_error_q32 = measurement_error_q32 - self.phase_q32 raw_error_q32 = max( MIN_SIGNED_64, min(MAX_SIGNED_64, raw_error_q32) ) proportional = (raw_error_q32 * self.kp_q46) >> 46 integrator_candidate = ( self.integrator_q32 + ((raw_error_q32 * self.ki_q46) >> 46) ) self.integrator_q32 = self._clamp( integrator_candidate, self.max_slew_q32 ) self.rate_word_q32 = self._clamp( self.integrator_q32 + proportional, self.max_slew_q32 ) acquire = self.parameters.lock_acquire_ticks * Q32 release = self.parameters.lock_release_ticks * Q32 was_locked = self.locked if abs(raw_error_q32) > release: self.locked = False self.lock_good_updates = 0 elif not self.locked: if abs(raw_error_q32) <= acquire: self.lock_good_updates += 1 if ( self.lock_good_updates >= self.parameters.lock_acquire_updates ): self.locked = True self.lock_good_updates = 0 else: self.lock_good_updates = 0 if self.locked and not was_locked: self.trace.lock_events += 1 elif was_locked and not self.locked: self.trace.unlock_events += 1 return raw_error_q32, raw_error_q32 def update(self, observation: Observation) -> None: self._advance(observation.time_s) if self.adaptive and observation.quality.solution_valid: selected, changed = self.selector.update(observation.quality) if changed or selected != self.profile: self._apply_profile(selected) if not observation.quality.solution_valid: if ( self.last_observation_time_s is None or observation.time_s - self.last_observation_time_s > REFERENCE_WATCHDOG_SEC ): self.reference_watchdog_holdover = True self._record(observation, 0, 0) return if self.profile == Profile.HOLDOVER: self.reference_watchdog_holdover = True self._record(observation, 0, 0) return self._select_loop_parameters() accepted, coarse = self._software_filter_update(observation) if not accepted: if ( self.last_observation_time_s is None or observation.time_s - self.last_observation_time_s > REFERENCE_WATCHDOG_SEC ): self.reference_watchdog_holdover = True self._record(observation, 0, 0) return measurement_error_q32 = self._seconds_to_phase_q32( self.software_error_s ) if coarse or not self.aligned: was_locked = self.locked self.phase_q32 = measurement_error_q32 self.rate_word_q32 = 0 self.integrator_q32 = 0 self.aligned = True self.locked = False self.lock_good_updates = 0 self.trace.coarse_commits += 1 if was_locked: self.trace.unlock_events += 1 raw_error_q32 = 0 control_error_q32 = 0 self.reference_watchdog_holdover = True else: raw_error_q32, control_error_q32 = self._normal_commit( measurement_error_q32 ) self.trace.normal_commits += 1 self.reference_watchdog_holdover = False self._record(observation, raw_error_q32, control_error_q32) def _record( self, observation: Observation, raw_error_q32: int, control_error_q32: int, ) -> None: self.trace.time_s.append(observation.time_s) self.trace.phase_error_s.append( self._phase_q32_to_seconds(self.phase_q32) ) self.trace.raw_measurement_error_s.append(observation.timing_error_s) self.trace.software_filtered_error_s.append(self.software_error_s) self.trace.raw_loop_error_s.append( self._phase_q32_to_seconds(raw_error_q32) ) self.trace.control_loop_error_s.append( self._phase_q32_to_seconds(control_error_q32) ) self.trace.frequency_command_ppm.append( self.rate_word_q32 / Q32 * 1.0e6 ) self.trace.profile.append(self.profile) # STATUS.LOCKED is retained internally during holdover, but external # PPS validity is deliberately low. Scenario metrics use the valid # lock contract rather than counting a held stale state as usable. self.trace.locked.append( self.locked and self.profile != Profile.HOLDOVER and not self.reference_watchdog_holdover ) def finish(self, time_s: float) -> AdaptiveTrace: self._advance(time_s) return self.trace def run_observations( observations: Iterable[Observation], *, finish_time_s: float, oscillator_ppm: float = 1.65, adaptive: bool = True, software_filter: bool = True, receiver_clock_bias_m: float = 1200.0, receiver_clock_drift_mps: float = 475.0, profiles: Mapping[Profile, LoopProfile] | None = None, acquire_profile: LoopProfile | None = None, software_drift_gain: float = 1.0 / 4.0, software_fallback_drift_gain: float = 1.0 / 16.0, ) -> AdaptiveTrace: model = FixedPointAdaptivePps( oscillator_ppm=oscillator_ppm, adaptive=adaptive, software_filter=software_filter, receiver_clock_bias_m=receiver_clock_bias_m, receiver_clock_drift_mps=receiver_clock_drift_mps, profiles=profiles, acquire_profile=acquire_profile, software_drift_gain=software_drift_gain, software_fallback_drift_gain=software_fallback_drift_gain, ) for observation in observations: model.update(observation) return model.finish(finish_time_s)