onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(pps): preserve loop dynamics across nav periods

Browse Source
This commit is contained in:
onur
2026-07-29 00:45:16 +03:00
parent a6bb080270
commit 347e1c671d
19 changed files with 889 additions and 111 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/pps_python/adaptive_pps.py
+129 -16
View File
@@ -8,7 +8,7 @@ rounding, lock acquisition/release and holdover/re-acquisition.
from __future__ import annotations
from dataclasses import dataclass
from dataclasses import dataclass, replace
from enum import IntEnum
import math
from typing import Iterable, Mapping
@@ -16,7 +16,9 @@ from typing import Iterable, Mapping
TRACKING_HZ = 125_000_000
SPEED_OF_LIGHT_MPS = 299_792_458.0
REFERENCE_WATCHDOG_SEC = 2.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
@@ -24,6 +26,27 @@ 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
@@ -58,14 +81,74 @@ class LoopProfile:
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
kp_q46 = round((2.0 * self.damping * omega / TRACKING_HZ) * Q46)
ki_q46 = round(
(omega * omega * update_period_s / TRACKING_HZ) * Q46
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
)
max_slew_q32 = round(self.max_slew_ppm * 1.0e-6 * Q32)
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(
@@ -149,10 +232,12 @@ def classify_quality(metrics: QualityMetrics) -> Profile:
class QualitySelector:
"""Three-profile selector with asymmetric transition hysteresis."""
def __init__(self) -> None:
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)
@@ -165,7 +250,11 @@ class QualitySelector:
self.candidate_updates = 1
else:
self.candidate_updates += 1
required = 8 if target > self.profile else 3
required = (
self.upgrade_updates
if target > self.profile
else self.degrade_updates
)
if self.candidate_updates >= required:
self.profile = target
self.candidate_updates = 0
@@ -339,19 +428,28 @@ class FixedPointAdaptivePps:
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
self.profiles = dict(PROFILES if profiles is None else profiles)
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)}")
self.acquire_profile = (
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:
@@ -360,7 +458,11 @@ class FixedPointAdaptivePps:
)
self.software_drift_gain = software_drift_gain
self.software_fallback_drift_gain = software_fallback_drift_gain
self.selector = QualitySelector()
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 = (
@@ -652,12 +754,21 @@ class FixedPointAdaptivePps:
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
+ self.parameters.software_bias_gain * innovation_m
+ effective_bias_gain * innovation_m
)
if observation.clock_drift_valid:
self.software_clock_drift_mps += self.software_drift_gain * (
self.software_clock_drift_mps += effective_drift_gain * (
measured_drift_mps - self.software_clock_drift_mps
)
else:
@@ -665,7 +776,7 @@ class FixedPointAdaptivePps:
self.software_clock_drift_mps = fallback_drift_mps
else:
self.software_clock_drift_mps += (
self.software_fallback_drift_gain *
effective_fallback_drift_gain *
(fallback_drift_mps - self.software_clock_drift_mps)
)
self.software_previous_measurement_m = measured_bias_m
@@ -731,7 +842,7 @@ class FixedPointAdaptivePps:
if (
self.last_observation_time_s is None
or observation.time_s - self.last_observation_time_s
> REFERENCE_WATCHDOG_SEC
> self.reference_watchdog_sec
):
self.reference_watchdog_holdover = True
self._record(observation, 0, 0)
@@ -747,7 +858,7 @@ class FixedPointAdaptivePps:
if (
self.last_observation_time_s is None
or observation.time_s - self.last_observation_time_s
> REFERENCE_WATCHDOG_SEC
> self.reference_watchdog_sec
):
self.reference_watchdog_holdover = True
self._record(observation, 0, 0)
@@ -817,6 +928,7 @@ 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,
@@ -828,6 +940,7 @@ def run_observations(
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,