719 lines
25 KiB
Python
719 lines
25 KiB
Python
"""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
|
|
|
|
|
|
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 = {
|
|
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
|
|
and time_s >= after_s
|
|
and abs(time_s - round(time_s)) < 1.0e-9
|
|
}
|
|
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 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,
|
|
) -> 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.selector = QualitySelector()
|
|
self.profile = Profile.HOLDOVER if adaptive else Profile.PRECISION
|
|
self.parameters = 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 = (
|
|
ACQUIRE_PROFILE
|
|
if self.adaptive and
|
|
self.profile != Profile.HOLDOVER and not self.locked
|
|
else 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 += 0.25 * (
|
|
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 += (1.0 / 16.0) * (
|
|
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,
|
|
) -> 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,
|
|
)
|
|
for observation in observations:
|
|
model.update(observation)
|
|
return model.finish(finish_time_s)
|