"""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 seed_tracking_state( self, *, time_s: float, profile: Profile, phase_error_s: float, rate_command_ppm: float, integrator_ppm: float, software_error_s: float, software_bias_m: float, software_clock_drift_mps: float, software_previous_measurement_m: float, reference_id: int, ) -> None: """Start a replay from a captured, already-locked board snapshot. Normal simulations deliberately start unaligned and exercise coarse acquisition. Hardware logs, however, are commonly captured after the board has been tracking for minutes. Replaying such a log from reset introduces an artificial acquisition transient and clears the PI integrator that was present on the board. This method imports only state that the production diagnostic registers and firmware status expose; subsequent updates still use the normal bit-accurate path. The method must be called before any observation has been recorded. It is a model facility and has no firmware/RTL side effect. """ finite_values = ( time_s, phase_error_s, rate_command_ppm, integrator_ppm, software_error_s, software_bias_m, software_clock_drift_mps, software_previous_measurement_m, ) if self.trace.time_s: raise ValueError("tracking state must be seeded before replay") if not all(math.isfinite(value) for value in finite_values): raise ValueError("tracking state values must be finite") if time_s < 0.0: raise ValueError("tracking state time must be non-negative") if profile == Profile.HOLDOVER: raise ValueError("a locked snapshot cannot use holdover profile") self.current_time_s = time_s self.raw_tick_remainder = 0.0 self.phase_q32 = self._seconds_to_phase_q32(phase_error_s) self.profile = profile self.selector.profile = profile self.selector.candidate = profile self.selector.candidate_updates = 0 self.aligned = True self.locked = True self.lock_good_updates = 0 self.reference_watchdog_holdover = False self._select_loop_parameters() self.integrator_q32 = self._clamp( round(integrator_ppm * 1.0e-6 * Q32), self.max_slew_q32 ) self.rate_word_q32 = self._clamp( round(rate_command_ppm * 1.0e-6 * Q32), self.max_slew_q32 ) self.software_error_s = software_error_s self.software_bias_m = software_bias_m self.software_clock_drift_mps = software_clock_drift_mps self.software_previous_measurement_m = ( software_previous_measurement_m ) self.software_drift_anchor_time_s = time_s self.software_drift_anchor_measurement_m = ( software_previous_measurement_m ) self.software_drift_initialized = True self.software_updates = max( 1, self.parameters.software_settle_updates ) self.software_reference_id = reference_id self.last_observation_time_s = time_s 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)