feat(pps): add adaptive disciplined timing loop
This commit is contained in:
@@ -0,0 +1,718 @@
|
||||
"""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)
|
||||
Reference in New Issue
Block a user