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/README.md
+61 -2
View File
@@ -9,7 +9,8 @@ davranışlarını taklit eder:
- RF `ABS_SAMPLE` sayacı: `16.367981 MHz`, 32 bit ve wrap eden sayaç.
- Tracking/PPS sayacı: `125 MHz` free-running sayaç.
- Navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`.
- Varsayılan navigation solve periyodu: `NAV_SOLVE_TIMER=10`, yani `200 ms`;
period-aware loop doğrulaması ayrıca 500 ve 1000 ms'yi kapsar.
- Ortak navigation bit transmit epoch'u ve uyduların farklı propagation
delay'lerle art arda tracking olayı üretmesi.
- Firmware'deki corrected sample denklemi:
@@ -239,7 +240,7 @@ PI, raw 125 MHz sayacı değiştirmez. Ayrı GPS time accumulator'ın rate word'
ayarlar:
```text
integrator += Ki * phase_error * update_period
integrator += Ki_discrete * phase_error
rate_command = integrator + Kp * phase_error
```
@@ -267,6 +268,64 @@ olması gerekir. Üretim profilleri tek seyrek WLS sıçramasında fiziksel puls
kaçırmamak için lock'u ancak `128 tick = 1.024 us` üstünde bırakır; watchdog,
quality holdover ve slew clamp daha büyük arızaları ayrıca sınırlar.
### NAV solve periyodu değiştiğinde loop dinamiği
`Ki` zaten ayrık-zaman katsayısıdır; aynı sayıyı 200, 500 ve 1000 ms'de
kullanmak aynı loop değildir. Basitçe `Ki`'yi periyotla çarpmak da uzun
periyotlarda mevcut RTL'nin gerçek kutuplarını yeterince doğru korumaz.
Firmware ve Python model şu uyumlu yöntemi kullanır:
1. Kartta doğrulanmış 200 ms Q2.46 katsayılarından RTL'nin iki ayrık kutbu
çıkarılır.
2. Yeni `T` periyodu için her kutup `T / 0.2` kuvvetine yükseltilir.
3. Hedef kutuplardan, RTL'nin önce-integral-sonra-proportional commit sırasına
uygun yeni `Kp` ve `Ki` geri hesaplanır.
Bu yöntem 200 ms'de eski katsayıları bit düzeyinde aynen üretir; 500 ve
1000 ms'de aynı gerçek-zaman damping ve settling davranışını korur:
| NAV periyodu | profil | Kp Q2.46 | Ki Q2.46 |
|---:|---|---:|---:|
| 200 ms | acquire / precision / robust | 764018 / 222838 / 84891 | 64006 / 5445 / 640 |
| 500 ms | acquire / precision / robust | 615780 / 209783 / 82980 | 127492 / 12804 / 1564 |
| 1000 ms | acquire / precision / robust | 447388 / 190239 / 79923 | 178038 / 23149 / 3011 |
Toplam döngü yalnız RTL PI'den oluşmadığı için yazılım clock-bias, Doppler
drift ve fallback drift kazançları da gerçek geçen süreyle
`alpha_T = 1 - (1 - alpha_200ms)^(T/0.2)` biçiminde uygulanır. Quality/profile
histerezisi, lock ve filter-settle sayaçları saniye cinsinden ölçeklenir;
bağımsız örnek güvenliği için sırasıyla en az 2/2/3 update korunur. Reference
watchdog `max(2 s, 3*T)` olur.
Desteklenen build değeri `GPS_NAV_SOLVE_TIMER_TICKS=N` için N, 300 bitlik
LNAV subframe'i bölmelidir. Doğrulanan örnekler N=10, 25 ve 50'dir. N=40,
HOW anchor yenilendiğinde timer fazını kaydırdığı için compile-time reddedilir.
`POSTPONE_TOW=180 ms` NAV solve periyodundan bağımsızdır ve değişmez.
Örnek firmware build'leri:
```bash
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=10 all # 200 ms, varsayılan
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=25 all # 500 ms
make -C gps_system RISCV_TOOLCHAIN=/tools/toolchains/rvtc-medany \
GPS_NAV_SOLVE_TIMER_TICKS=50 all # 1000 ms
```
Linux stability sweep'i aynı periyotları doğrudan kabul eder:
```bash
python3 -m pps_end_to_end_validation.stability_analysis \
--update-period-ms 500
```
Kutup eşlemesi deterministik loop dinamiğini korur; daha seyrek WLS ölçümü
aynı gürültü performansını garanti etmez. Örnek sayısı azalınca aliasing,
outlier yakalama gecikmesi ve mutlak faz wander'ı değişebilir. Bu nedenle her
kart/periyot kombinasyonu trusted PPS ile ayrıca ölçülmelidir.
## Adaptif PPS senaryo matrisi
Tam fixed-point/holdover matrisi:
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,
soc/software/pps_python/test_pps_adaptive.py
+56
View File
@@ -6,6 +6,7 @@ import unittest
from pps_python.adaptive_pps import (
ACQUIRE_PROFILE,
FixedPointAdaptivePps,
Observation,
Profile,
PROFILES,
@@ -59,6 +60,61 @@ def noisy_observations(
class AdaptivePpsTests(unittest.TestCase):
def test_period_aware_coefficients_match_firmware(self):
expected = {
0.2: {
"acquire": (764018, 64006),
Profile.PRECISION: (222838, 5445),
Profile.ROBUST: (84891, 640),
},
0.5: {
"acquire": (615780, 127492),
Profile.PRECISION: (209783, 12804),
Profile.ROBUST: (82980, 1564),
},
1.0: {
"acquire": (447388, 178038),
Profile.PRECISION: (190239, 23149),
Profile.ROBUST: (79923, 3011),
},
}
for period_s, coefficients in expected.items():
self.assertEqual(
ACQUIRE_PROFILE.fixed_coefficients(period_s)[:2],
coefficients["acquire"],
)
for profile in (Profile.PRECISION, Profile.ROBUST):
self.assertEqual(
PROFILES[profile].fixed_coefficients(period_s)[:2],
coefficients[profile],
)
def test_wall_clock_hysteresis_and_settling_scale_with_period(self):
model_500ms = FixedPointAdaptivePps(update_period_s=0.5)
self.assertEqual(model_500ms.selector.upgrade_updates, 4)
self.assertEqual(model_500ms.selector.degrade_updates, 2)
self.assertEqual(
model_500ms.profiles[Profile.PRECISION].software_settle_updates,
5,
)
self.assertEqual(
model_500ms.profiles[Profile.ROBUST].lock_acquire_updates,
4,
)
model_1s = FixedPointAdaptivePps(update_period_s=1.0)
self.assertEqual(model_1s.selector.upgrade_updates, 2)
self.assertEqual(model_1s.selector.degrade_updates, 2)
self.assertEqual(
model_1s.profiles[Profile.PRECISION].software_settle_updates,
3,
)
self.assertEqual(
model_1s.profiles[Profile.ROBUST].lock_acquire_updates,
2,
)
self.assertEqual(model_1s.reference_watchdog_sec, 3.0)
def test_board_calibrated_doppler_predictor_reduces_pps_error(self):
"""Genesys2 WLS statistics: predictor-on must beat fallback-only."""
quality_metrics = QualityMetrics(