957 lines
35 KiB
Python
957 lines
35 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, replace
|
|
from enum import IntEnum
|
|
import math
|
|
from typing import Iterable, Mapping
|
|
|
|
|
|
TRACKING_HZ = 125_000_000
|
|
SPEED_OF_LIGHT_MPS = 299_792_458.0
|
|
BASELINE_UPDATE_PERIOD_S = 0.2
|
|
MIN_REFERENCE_WATCHDOG_SEC = 2.0
|
|
REFERENCE_WATCHDOG_PERIODS = 3.0
|
|
MAX_PREDICTION_GAP_SEC = 30.0
|
|
Q32 = 1 << 32
|
|
Q46 = 1 << 46
|
|
MAX_SIGNED_64 = (1 << 63) - 1
|
|
MIN_SIGNED_64 = -(1 << 63)
|
|
|
|
|
|
def _period_updates(
|
|
baseline_updates: int,
|
|
update_period_s: float,
|
|
minimum_updates: int,
|
|
) -> int:
|
|
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
|
raise ValueError("update_period_s must be finite and positive")
|
|
duration_s = baseline_updates * BASELINE_UPDATE_PERIOD_S
|
|
return min(
|
|
255,
|
|
max(minimum_updates, math.ceil(duration_s / update_period_s - 1e-12)),
|
|
)
|
|
|
|
|
|
def _elapsed_gain(baseline_gain: float, elapsed_s: float) -> float:
|
|
periods = elapsed_s / BASELINE_UPDATE_PERIOD_S
|
|
if abs(periods - 1.0) <= 1e-12:
|
|
return baseline_gain
|
|
return -math.expm1(periods * math.log1p(-baseline_gain))
|
|
|
|
|
|
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]:
|
|
"""Map the proven 200 ms discrete poles to ``update_period_s``.
|
|
|
|
The 200 ms result is exactly the legacy production coefficient set.
|
|
At another NAV cadence each baseline pole is raised to T/0.2 before
|
|
recovering Kp/Ki for the RTL's integrate-then-proportional ordering.
|
|
"""
|
|
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
|
raise ValueError("update_period_s must be finite and positive")
|
|
omega = 2.0 * math.pi * self.natural_frequency_hz
|
|
baseline_kp = math.floor(
|
|
(2.0 * self.damping * omega / TRACKING_HZ) * Q46 + 0.5
|
|
) / Q46
|
|
baseline_ki = math.floor(
|
|
(
|
|
omega * omega * BASELINE_UPDATE_PERIOD_S / TRACKING_HZ
|
|
) * Q46 + 0.5
|
|
) / Q46
|
|
baseline_ticks = TRACKING_HZ * BASELINE_UPDATE_PERIOD_S
|
|
baseline_sum = 2.0 - baseline_ticks * (
|
|
baseline_kp + baseline_ki
|
|
)
|
|
baseline_product = 1.0 - baseline_ticks * baseline_kp
|
|
discriminant = baseline_sum * baseline_sum - 4.0 * baseline_product
|
|
period_ratio = update_period_s / BASELINE_UPDATE_PERIOD_S
|
|
if discriminant >= 0.0:
|
|
root = math.sqrt(discriminant)
|
|
pole_a = 0.5 * (baseline_sum + root)
|
|
pole_b = 0.5 * (baseline_sum - root)
|
|
if pole_a < 0.0 or pole_b < 0.0:
|
|
raise ValueError("baseline loop has an unmappable negative pole")
|
|
target_a = pole_a**period_ratio
|
|
target_b = pole_b**period_ratio
|
|
target_sum = target_a + target_b
|
|
target_product = target_a * target_b
|
|
else:
|
|
if baseline_product <= 0.0:
|
|
raise ValueError("baseline loop has invalid complex poles")
|
|
radius = math.sqrt(baseline_product)
|
|
cosine = max(-1.0, min(1.0, baseline_sum / (2.0 * radius)))
|
|
angle = math.acos(cosine)
|
|
target_radius = radius**period_ratio
|
|
target_sum = 2.0 * target_radius * math.cos(
|
|
angle * period_ratio
|
|
)
|
|
target_product = target_radius * target_radius
|
|
target_ticks = TRACKING_HZ * update_period_s
|
|
kp = (1.0 - target_product) / target_ticks
|
|
ki = (1.0 - target_sum + target_product) / target_ticks
|
|
if not math.isfinite(kp) or not math.isfinite(ki) or kp < 0 or ki < 0:
|
|
raise ValueError("mapped loop coefficients are invalid")
|
|
kp_q46 = math.floor(kp * Q46 + 0.5)
|
|
ki_q46 = math.floor(ki * Q46 + 0.5)
|
|
max_slew_q32 = math.floor(
|
|
self.max_slew_ppm * 1.0e-6 * Q32 + 0.5
|
|
)
|
|
return kp_q46, ki_q46, max_slew_q32
|
|
|
|
def for_update_period(self, update_period_s: float) -> LoopProfile:
|
|
return replace(
|
|
self,
|
|
lock_acquire_updates=_period_updates(
|
|
self.lock_acquire_updates, update_period_s, 2
|
|
),
|
|
software_settle_updates=_period_updates(
|
|
self.software_settle_updates, update_period_s, 3
|
|
),
|
|
)
|
|
|
|
|
|
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, update_period_s: float = BASELINE_UPDATE_PERIOD_S) -> None:
|
|
self.profile = Profile.HOLDOVER
|
|
self.candidate = Profile.HOLDOVER
|
|
self.candidate_updates = 0
|
|
self.upgrade_updates = _period_updates(8, update_period_s, 2)
|
|
self.degrade_updates = _period_updates(3, update_period_s, 2)
|
|
|
|
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 = (
|
|
self.upgrade_updates
|
|
if target > self.profile
|
|
else self.degrade_updates
|
|
)
|
|
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:
|
|
if not math.isfinite(update_period_s) or update_period_s <= 0.0:
|
|
raise ValueError("update_period_s must be finite and positive")
|
|
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
|
|
baseline_profiles = dict(PROFILES if profiles is None else profiles)
|
|
self.profiles = {
|
|
profile: parameters.for_update_period(update_period_s)
|
|
for profile, parameters in baseline_profiles.items()
|
|
}
|
|
missing_profiles = set(Profile) - self.profiles.keys()
|
|
if missing_profiles:
|
|
raise ValueError(f"missing loop profiles: {sorted(missing_profiles)}")
|
|
baseline_acquire_profile = (
|
|
ACQUIRE_PROFILE if acquire_profile is None else acquire_profile
|
|
)
|
|
self.acquire_profile = baseline_acquire_profile.for_update_period(
|
|
update_period_s
|
|
)
|
|
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(update_period_s)
|
|
self.reference_watchdog_sec = max(
|
|
MIN_REFERENCE_WATCHDOG_SEC,
|
|
REFERENCE_WATCHDOG_PERIODS * update_period_s,
|
|
)
|
|
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))
|
|
effective_bias_gain = _elapsed_gain(
|
|
self.parameters.software_bias_gain, interval_s
|
|
)
|
|
effective_drift_gain = _elapsed_gain(
|
|
self.software_drift_gain, interval_s
|
|
)
|
|
effective_fallback_drift_gain = _elapsed_gain(
|
|
self.software_fallback_drift_gain, interval_s
|
|
)
|
|
self.software_bias_m = (
|
|
predicted_bias_m
|
|
+ effective_bias_gain * innovation_m
|
|
)
|
|
if observation.clock_drift_valid:
|
|
self.software_clock_drift_mps += effective_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 += (
|
|
effective_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
|
|
> self.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
|
|
> self.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,
|
|
update_period_s: float = BASELINE_UPDATE_PERIOD_S,
|
|
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(
|
|
update_period_s=update_period_s,
|
|
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)
|