feat(pps): add adaptive disciplined timing loop

2026-07-25 17:56:41 +03:00
parent 0f6b3482c5
commit ecbb8d0b68
39 changed files with 4505 additions and 96 deletions
@@ -5,7 +5,7 @@ TARGET := gps_system
 # directory previously compiled every GPS_LOG call out and produced a silent
 # UART image.
 GPS_DEBUG ?= 1
-GPS_DEBUG_LEVEL ?= 4
+GPS_DEBUG_LEVEL ?= 1
 GPS_BUILD_ID ?= $(shell git describe --always --dirty --abbrev=12 2>/dev/null || printf unknown)

 # The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
@@ -71,6 +71,8 @@ APP_SRCS := \
 	src/gps/gps_time_anchor.c \
 	src/gps/gps_nav_epoch.c \
 	src/gps/gps_pps.c \
+	src/gps/gps_pps_quality.c \
+	src/gps/gps_pps_time_filter.c \
 	src/gps/gps_tracking_measurement.c \
 	src/gps/linked_list.c \
 	src/gps/NavEquations.c \
@@ -79,6 +81,7 @@ APP_SRCS := \
 BSP_SRCS := init.c uart.c plic.c irq.c spi_flash.c pmod_spi.c

 GPS_NAV_SOLVE_TIMER_TICKS ?= 10
+GPS_PPS_STATUS_LOG_INTERVAL ?= 10

 include ../common/app.mk

@@ -90,7 +93,8 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
 	-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
 	-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
 	-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
-	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
+	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS) \
+	-DGPS_PPS_STATUS_LOG_INTERVAL=$(GPS_PPS_STATUS_LOG_INTERVAL)
 LDLIBS += -lm

 ifeq ($(GPS_DEBUG),1)
@@ -1173,7 +1173,8 @@ Bu timer/scheduler yavasligi degildir. Firmware start ediyor; tamamlanma icin RF
  darbesi uretir. Bu olay `ABS_SAMPLE` ve `REM_CODE_PHASE` latch'iyle aynidir;
  subframe-ready PPS timestamp'i olusturmaz.
 - `leastSquaresSTD()` ayni basarili epoch'un `[x, y, z, b]` sonucunu verir.
-  Minimum corrected-ABS kanalinin receive-time ankraji su sekilde kurulur:
+  Ilk cozumde en erken corrected-ABS kanali zaman datum'u secilir; ayni SVID
+  coherent snapshot'ta kaldigi surece referans korunur. Receive-time ankraji:

 ```text
 T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c
@@ -1182,12 +1183,108 @@ T_rx = TOW_ref + POSTPONE_TOW + STARTOFFSET/1000 - b/c
 - `POSTPONE_TOW=0.160 s`, takip eden subframe'dan parity icin beklenen sekiz
  bitin gecikmesidir ve yazilim tarafinda tam bir kez uygulanir. PPS RTL bu
  degeri tekrar eklemez.
- Bu GPS zamani, ayni minimum-ABS kanalinin 125 MHz capture counter'i ile atomik
+- Bu GPS zamani, ayni kalici referans kanalinin 125 MHz capture counter'i ile atomik
  APB mailbox uzerinden eslenir. Ilk cozum coarse-align, sonraki cozumler Q2.46
  dusuk bant genislikli PI rate discipline yapar. WLS/APB calisma suresi faz
  hatasi olarak kullanilmaz.
+- Tracking capture olayi integer RF sample kenarinda olustugu halde WLS,
+  `ABS_SAMPLE - REM_CODE_PHASE/1.023e6` alt-sample zamanini kullanir. Yazilim
+  mailbox `EPOCH_COUNTER` degerinden ayni fractional sample gecikmesini
+  `round(fraction * 125e6/16.367981e6)` ile cikarir. Boylece WLS zamani ile
+  hardware epoch'u ayni code boundary'ye referans olur; 125 MHz integer
+  mailbox yuvarlamasindan en fazla 4 ns kalir.
+- HOW/sample sureklilik kontrolu kartin fiziksel `16.367981 MHz` RF frekansini
+  kullanir. Periyodik nav-solve TOW'u receiver propagation/range-rate
+  degisiminden uretilmez; sample farki yalniz olay numarasini dogrular ve TOW
+  bilinen 200 ms transmit-time grid'ine snap edilir.
+- Donanimdaki kontrol filtresi PI loop'un kendisidir; ayri bir phase-IIR veya
+  D terimi yoktur. Yazilim `TDOP/GDOP`, formal clock-bias sigma, kullanilan
+  uydu sayisi, C/N0, chi-square/FDE ve Doppler velocity residual'ina gore
+  `acquire`, `precision`, `robust` ve `holdover` profillerini histerezisle
+  secer. Profil gecisinde `Kp`, `Ki`, `max-slew` ve v2.2 lock esikleri bir
+  sonraki mailbox commit'inde birlikte yakalanir; normal geciste integrator
+  korunur. Lock almak icin bes ardisik residual `64 ns` icinde olmalidir;
+  uretim release esigi tek seyrek WLS/capture sicrama pulse kaybettirmesin
+  diye `1.024 us` tutulur. Watchdog, quality holdover ve `20 ppm` slew clamp
+  buyuk arizalari ayri olarak sinirlar.
+- Yazilim filtresi PI'nin yerine gecmez. Yalnizca 5 Hz WLS clock-bias
+  olcumunde Doppler clock-drift prediction, bounded innovation ve aykiri
+  olcum reddi yapar. Ilk yerlesmede commit'ler coarse ve PPS invalid tutulur.
+  Periyodik background watchdog iki saniye yeni referans gelmezse cikis
+  validligini holdover ile kapatir; fakat ayni referans geri gelirse Doppler
+  prediction ve donanim PI state'i 30 saniyeye kadar korunur. Referans SVID
+  degisimi veya 30 saniyeyi asan bosluk coarse re-anchor yapar. PPS v2.2
+  holdover aktifken normal PI commit'ini uygulamadigi icin recovery'de butun
+  shadow word'ler once yazilir, holdover commit toggle'inin hemen oncesinde
+  birakilir ve ilk taze olcum PI'ya girer. Fiziksel PPS held rate ile
+  kesintisiz yurur; `STATUS.VALID` PI apply'dan yalniz bounded
+  synchronizer/commit-pipeline gecikmesi kadar once gorunebilir.
+- PPS v2.2 `LOOP_CONFIG` register'i `0x88` adresindedir. Eski v2.1 bitstream
+  algilanirsa yazilim bu register'a dokunmaz; dinamik Q2.46 `Kp/Ki/max-slew`
+  yazimi mevcut register'larla calismaya devam eder.
 - 125 MHz fiziksel kenar grid'i 8 ns'dir. Generated VCU118 ve Genesys2 top'lari
  `gps_pps` output'unu expose eder. VCU118 icin PPS PMOD1/J53.8 (`R29`),
  Genesys2 icin PMOD JC10 (`AK28`) pinine atanmistir. Board testinde
  Gray/PCLK capture pipeline sabit offset'i ve
  clock-tree/I/O gecikmesi olculup kalibre edilmelidir.
+
+### PPS Sentetik Dogrulama (2026-07-25)
+
+Python modeli ve Linux C testi ayni Q32/Q46 PI, kalite histerezisi, Doppler
+prediction, holdover ve lock politikasini kullanir. Ayrica donanim phase-IIR
+kullanilmaz. Test matrisi temiz/320 ns WLS noise, 650 ns zayif geometri,
+baslangicta 1.1 us noise sonra temiz sinyal, aralikli 900 ns noise burstleri,
+uydu kaybi ve geri gelmesi, 15 saniyelik state-korumali ve 30 saniye ustu
+coarse WLS kesintisi, Doppler kaybi, multipath
+basamagi, profil sicramalari, +18 ppm slew siniri ve +25 ppm sinir disi
+osilatoru kapsar.
+
+Monte Carlo sonucunda hedeflenen senaryolarin tamami lock/recovery sagladi.
+Normal kart-noise senaryosunda 50/50 kosu lock oldu ve 125 MHz edge
+quantization dahil bir saniyelik jitter p95 `13.62 ns` bulundu. Baslangicta
+yuksek noise sonra temiz sinyal senaryosunda 30/30, noise-burst senaryosunda
+30/30, iki uydu-kaybi araligi bulunan senaryoda 30/30 recovery goruldu.
+`+18 ppm` 30/30 yerlesirken `+25 ppm`, `20 ppm` clamp disinda oldugu icin
+0/10 lock uretti; yani saturation yanlis-valid olusturmadi. Bunlar sentetik
+loop/jitter sonuclaridir; mutlak UTC/GPS faz dogrulugu icin karttaki sabit
+capture/I/O offset'i ve gercek multipath halen osiloskop/time-interval olcumu
+gerektirir.
+
+### PPS Kart Dogrulamasi (2026-07-25, Genesys2)
+
+- Kartta okunan RTL surumu `0x00020200`; dinamik `LOOP_CONFIG=0x08800800`
+  mailbox'i ve diagnostic snapshot calisti.
+- Eski `16.368 MHz` zaman donusumu 6 saniyede yaklasik `6.97 us` hata
+  uretiyordu. Fiziksel `16.367981 MHz` donusumu ve 200 ms transmit-grid snap
+  sonrasinda 6 saniyelik mikro-saniye sawtooth kayboldu.
+- Fractional capture duzeltmesi kartta epoch'a gore `0..64 ns` uygulandi
+  (son deger 8 ns grid'ine yuvarlanir). Duzeltmeden onceki uzun lock
+  penceresinde internal commit residual'i yaklasik `20.9 ns` median mutlak ve
+  `62.8 ns` p95 idi. Duzeltmeden sonraki precision penceresinde median mutlak
+  residual yaklasik `17 ns`, RMS `26 ns` ve p95 yaklasik `55 ns` oldu.
+- Ilk kart yaziliminda iki saniyeyi asan normal coherent-WLS scheduling
+  bosluklari filtreyi coarse resetliyordu; bu PPS'in periyodik olarak
+  4--5 update invalid kalmasina yol acti. Esigi validlik watchdog'undan
+  ayirdiktan sonra `seq=10551 -> 10561` araliginda `pulse=2174 -> 2186`
+  ile yaklasik 12 saniyelik gercek kart boslugu goruldu. Buna ragmen
+  `mode=tracking`, `locked=1`, status `0x80f` ve PI integrator kesintisiz
+  kaldi; coarse commit veya faz basamagi olusmadi.
+- Her WLS commit'inin loglandigi `pps-fullrate` tanisinda `seq=16090..17631`
+  arasinda 1514 commit izlendi. Ilk lock'tan sonra coarse veya unlock olmadi.
+  `seq>=16250` icin 1355 locked residual'in median mutlak degeri `17.29 ns`,
+  RMS'i `34.17 ns`, p95'i `52.76 ns`, p99'u `67.24 ns` oldu. Bu sayilar
+  fiziksel PPS pin jitter'i degil, 5 Hz WLS commit residual'idir.
+- Ayni tanida quality'nin robust'a dustugu tek bir commit'te `750.93 ns`
+  residual goruldu ve bir sonraki 200 ms commit'te normale dondu. Robust
+  `1.024 us` release esigi gereksiz pulse kaybini onledi. Daha once precision
+  `512 ns` release ile gorulen tek kisa unlock nedeniyle uretim precision
+  esigi de `1.024 us` yapildi; acquisition kosulu bes ardisik `<=64 ns`
+  olarak kaldi. Son `pps-final` kart okumasinda `LOOP_CONFIG=0x05800800`,
+  `CONTROL=0x3`, valid/locked status `0x80f` goruldu ve kontrollu holdover
+  denemesinden sonra ilk normal commit ile control/valid geri geldi.
+- Bu `hw_phase`, 5 Hz WLS reference commit residual'idir ve dogrudan fiziksel
+  1PPS kenar jitter'i degildir. Ayni pencerede proportional rate terimi
+  yaklasik `9.6 ppb RMS` idi; sentetik 125 MHz edge modeli kart-noise
+  senaryosunda bir saniyelik jitter p95 `13.62 ns` veriyor. `15--20 ns`
+  fiziksel hedef makuldur fakat pin uzerinde scope/TIC ile GPS-disiplinli
+  harici referansa karsi olculmeden saglandi diye raporlanmamalidir.
@@ -9,6 +9,7 @@
 		typedef struct gps_nav_solution {
 			double ecef_m[3];
 			double clock_bias_m;
+			double clock_bias_variance_m2;
 			double velocity_ecef_mps[3];
 			double velocity_covariance_ecef_mps2[3][3];
 			double clock_drift_mps;
@@ -37,8 +37,21 @@
 	#define WN_INDEX        2
 	#define POSITION_VECTOR_COL 3
 	#define MAX_24_BIT_INT  0x00800000
-	// ADC sample frekansi
-	#define SAMPLES_PER_CODE    16368.0
+	/*
+	 * RF ADC sample rate used for time and pseudorange conversion.
+	 *
+	 * The acquisition RTL buffers an integer 16368 samples per nominal C/A
+	 * code period, but the Genesys2/VCU118 RF reference is 16.367981 MHz.
+	 * Using 16368.000 samples/ms as a frequency creates a 1.161 ppm timing
+	 * ramp which is reset by every six-second HOW anchor (about 6.97 us
+	 * peak-to-peak).  Keep the physical clock rate distinct from the integer
+	 * acquisition-buffer length.
+	 */
+	#ifndef GPS_RF_SAMPLE_RATE_HZ
+	#define GPS_RF_SAMPLE_RATE_HZ 16367981.0
+	#endif
+	#define GPS_RF_SAMPLES_PER_MILLISECOND (GPS_RF_SAMPLE_RATE_HZ / 1000.0)
+	#define SAMPLES_PER_CODE GPS_RF_SAMPLES_PER_MILLISECOND

 	#define CODE_FREQ_BASIS 1.023E+6
 	
@@ -4,11 +4,40 @@
 #include <stdbool.h>
 #include <stdint.h>

+#include "gps/gps_pps_quality.h"
+#include "gps/gps_pps_time_filter.h"
+
 #define GPS_PPS_CLOCK_HZ 125000000u

+typedef struct {
+	uint32_t status;
+	uint32_t active_sequence;
+	uint32_t pulse_count;
+	int64_t phase_error_q32;
+	int64_t integrator_q32;
+	int64_t rate_word_q32;
+} gps_pps_diagnostics_t;
+
 void gps_pps_init(unsigned int nav_solve_tick);
+void gps_pps_service(void);
 bool gps_pps_is_locked(void);
+bool gps_pps_set_loop_profile(gps_pps_profile_t profile);
+void gps_pps_set_holdover(bool holdover);
+void gps_pps_reset_time_filter(void);
+bool gps_pps_read_diagnostics(gps_pps_diagnostics_t* diagnostics);
 bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* sequence);
-bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiver_tow_sec);
+gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
+	uint64_t epoch_counter,
+	double measured_clock_bias_m,
+	bool clock_drift_valid,
+	double measured_clock_drift_mps,
+	double tdop,
+	double* filtered_clock_bias_m_out,
+	double* innovation_m_out);
+bool gps_pps_commit_solution(
+	uint64_t epoch_counter,
+	int gps_week,
+	double receiver_tow_sec,
+	bool force_coarse);

 #endif
@@ -0,0 +1,62 @@
+#ifndef GPS_PPS_QUALITY_H
+#define GPS_PPS_QUALITY_H
+
+#include <stdbool.h>
+
+typedef enum {
+	GPS_PPS_PROFILE_HOLDOVER = 0,
+	GPS_PPS_PROFILE_ROBUST,
+	GPS_PPS_PROFILE_PRECISION,
+	GPS_PPS_PROFILE_ACQUIRE
+} gps_pps_profile_t;
+
+typedef struct {
+	double tdop;
+	double gdop;
+	double clock_bias_sigma_m;
+	double mean_cn0_dbhz;
+	double minimum_cn0_dbhz;
+	double chi_square_ratio;
+	double velocity_residual_rms_mps;
+	unsigned int used_satellites;
+	bool solution_valid;
+	bool residual_test_valid;
+	bool velocity_valid;
+	bool fde_applied;
+} gps_pps_quality_metrics_t;
+
+typedef struct {
+	gps_pps_profile_t profile;
+	gps_pps_profile_t candidate;
+	unsigned int candidate_updates;
+} gps_pps_quality_selector_t;
+
+typedef struct {
+	double natural_frequency_hz;
+	double damping;
+	double max_slew_ppm;
+	unsigned int lock_acquire_ticks;
+	unsigned int lock_release_ticks;
+	unsigned int lock_acquire_updates;
+	double software_bias_gain;
+	unsigned int software_settle_updates;
+} gps_pps_loop_profile_t;
+
+void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector);
+
+/*
+ * Select a bounded loop profile from timing geometry, code/carrier quality
+ * and integrity. Upgrades require sustained evidence and degradations require
+ * three consecutive observations. A single rejected WLS epoch is skipped
+ * without resetting the loop; sustained loss enters holdover, while the
+ * independent reference watchdog remains the final stale-data guard.
+ */
+gps_pps_profile_t gps_pps_quality_selector_update(
+	gps_pps_quality_selector_t* selector,
+	const gps_pps_quality_metrics_t* metrics,
+	bool* profile_changed_out);
+
+gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile);
+const char* gps_pps_profile_name(gps_pps_profile_t profile);
+
+#endif
@@ -0,0 +1,82 @@
+#ifndef GPS_PPS_TIME_FILTER_H
+#define GPS_PPS_TIME_FILTER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+typedef enum {
+	GPS_PPS_TIME_FILTER_REJECTED = 0,
+	GPS_PPS_TIME_FILTER_COARSE,
+	GPS_PPS_TIME_FILTER_TRACKING
+} gps_pps_time_filter_result_t;
+
+typedef struct {
+	uint64_t epoch_counter;
+	uint64_t drift_anchor_counter;
+	double clock_bias_m;
+	double clock_drift_mps;
+	double previous_measurement_m;
+	double drift_anchor_measurement_m;
+	unsigned int accepted_updates;
+	bool initialized;
+	bool drift_initialized;
+} gps_pps_time_filter_t;
+
+void gps_pps_time_filter_init(gps_pps_time_filter_t* filter);
+
+/*
+ * Keep the current WLS datum satellite while it remains in the coherent
+ * snapshot. Fall back to the earliest capture only when that satellite is
+ * gone. Stable selection avoids artificial clock-bias jumps as satellites
+ * enter the solution.
+ */
+bool gps_pps_select_reference(
+	const unsigned int satellite_ids[],
+	const double capture_times[],
+	unsigned int count,
+	bool preferred_valid,
+	unsigned int preferred_id,
+	unsigned int* reference_index_out);
+
+/*
+ * The position WLS observations are formed from capture_i - min(capture).
+ * That subtraction removes the common receiver-clock offset and drift before
+ * WLS. The fitted clock-bias datum is STARTOFFSET*c - reference_range (plus
+ * the reference satellite clock correction), so its derivative is
+ * -reference_range_rate + c*reference_satellite_clock_drift.
+ */
+bool gps_pps_reference_bias_drift(
+	const double receiver_ecef_m[3],
+	const double receiver_velocity_ecef_mps[3],
+	const double reference_satellite_ecef_m[3],
+	const double reference_satellite_velocity_ecef_mps[3],
+	double reference_satellite_clock_drift_sps,
+	double* clock_bias_drift_mps_out);
+
+/*
+ * Smooth the pseudorange WLS receiver-clock bias without lagging its large
+ * linear oscillator drift.  The Doppler WLS clock-drift state predicts the
+ * bias at the new capture epoch; only the residual bias noise is low-passed.
+ * If Doppler is temporarily unavailable, a bounded, slower finite-difference
+ * drift estimate keeps the filter usable in the robust profile.
+ *
+ * COARSE is returned during initial settling, a reference-datum change, or
+ * after a gap too long for bounded Doppler prediction. The independent
+ * reference watchdog may suppress PPS validity during a shorter gap without
+ * destroying filter/PI state. The caller must keep PPS output invalid while
+ * COARSE is returned.
+ */
+gps_pps_time_filter_result_t gps_pps_time_filter_update(
+	gps_pps_time_filter_t* filter,
+	uint64_t epoch_counter,
+	double counter_frequency_hz,
+	double measured_clock_bias_m,
+	bool clock_drift_valid,
+	double measured_clock_drift_mps,
+	double tdop,
+	double bias_gain,
+	unsigned int settle_updates,
+	double* filtered_clock_bias_m_out,
+	double* innovation_m_out);
+
+#endif
@@ -36,6 +36,40 @@ bool gps_time_from_sample_anchor(int anchor_week,
                                 double* event_tow_sec_out,
                                 double* elapsed_sec_out);

+/*
+ * Convert a periodic tracking event to GPS transmit time. The RF sample
+ * interval identifies and validates the event number; the result is then
+ * snapped to the known navigation-event grid. This removes the
+ * satellite-range-rate term present in receive-time sample intervals.
+ */
+bool gps_time_from_periodic_sample_anchor(
+	int anchor_week,
+	double anchor_tow_sec,
+	uint32_t anchor_raw_sample,
+	double anchor_corrected_sample,
+	uint32_t event_raw_sample,
+	double event_corrected_sample,
+	double samples_per_second,
+	double max_age_sec,
+	double event_period_sec,
+	double max_snap_error_sec,
+	int* event_week_out,
+	double* event_tow_sec_out,
+	double* elapsed_sec_out);
+
+/*
+ * Refer a counter captured at the first RF sample edge after a code boundary
+ * back to that fractional code boundary. fractional_sample_delay is
+ * raw_abs_sample - corrected_abs_sample and must be in [0, 1].
+ */
+bool gps_time_correct_capture_counter(
+	uint64_t capture_counter,
+	double fractional_sample_delay,
+	double samples_per_second,
+	double counter_frequency_hz,
+	uint64_t* corrected_counter_out,
+	double* applied_correction_sec_out);
+
 /* Validate that two decoded HOW/sample anchors describe the same monotonic
 * receiver timeline.  This keeps a single transient HOW from replacing a
 * healthy anchor while still permitting normal week rollover. */
@@ -1208,6 +1208,28 @@ static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveS
 				covariance_between_axes(position_covariance, enu_axes[row], enu_axes[col]);
 		}
 	}
+	double clock_covariance_scale = 1.0;
+	if(degrees_of_freedom > 0u)
+	{
+		clock_covariance_scale =
+			fmax(1.0, weighted_residual_sum / (double)degrees_of_freedom);
+	}
+	solution_out->clock_bias_variance_m2 =
+		clock_covariance_scale * position_covariance->matrix[3][3];
+	if(!isfinite(solution_out->clock_bias_variance_m2) ||
+	   (solution_out->clock_bias_variance_m2 < 0.0))
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "clock-bias covariance rejected variance=%e scale=%e\r\n",
+		        solution_out->clock_bias_variance_m2, clock_covariance_scale);
+		free_matrix(position_covariance);
+		free_matrix(A);
+		free_matrix(posVec);
+		free(absoluteWeights);
+		free(phi_lambda_h);
+		free(topcent);
+		return false;
+	}
 	free_matrix(position_covariance);
 	free_matrix(A);

@@ -92,6 +92,7 @@ static gps_external_irq_handler_t gps_external_irq_handler = NULL;
 #define GPS_NAV_HOLDOFF_LOCAL_TICKS \
 	(((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u)
 #define GPS_TRACK_DOPPLER_SMOOTH_ALPHA 0.05
+#define GPS_NAV_TOW_SNAP_TOLERANCE_SEC 0.005

 _Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS <
               (GPS_NAV_SOLVE_PERIOD_MS / 2u),
@@ -1161,7 +1162,7 @@ static bool gps_how_candidate_consistent(
 		first_raw_sample, first_corrected_sample,
 		second_week, second_tow_sec,
 		second_raw_sample, second_corrected_sample,
-		SAMPLES_PER_CODE * 1000.0,
+		GPS_RF_SAMPLE_RATE_HZ,
 		GPS_HOW_CONTINUITY_MAX_GAP_SEC,
 		GPS_HOW_CONTINUITY_TOLERANCE_SEC);
 }
@@ -1388,17 +1389,20 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 		int tow_week = 0;
 		double tow_sec = 0.0;
 		double tow_delta_sec = 0.0;
-		if(!gps_time_from_sample_anchor(node->gps_how_tow.week,
-		                                node->gps_how_tow.sec,
-		                                node->gps_how_abs_sample,
-		                                node->gps_how_corrected_abs_sample,
-		                                event->abs_sample_raw,
-		                                event->corrected_abs_sample,
-		                                SAMPLES_PER_CODE * 1000.0,
-		                                GPS_HOW_ANCHOR_MAX_AGE_SEC,
-		                                &tow_week,
-		                                &tow_sec,
-		                                &tow_delta_sec))
+		if(!gps_time_from_periodic_sample_anchor(
+			   node->gps_how_tow.week,
+			   node->gps_how_tow.sec,
+			   node->gps_how_abs_sample,
+			   node->gps_how_corrected_abs_sample,
+			   event->abs_sample_raw,
+			   event->corrected_abs_sample,
+			   GPS_RF_SAMPLE_RATE_HZ,
+			   GPS_HOW_ANCHOR_MAX_AGE_SEC,
+			   (double)GPS_NAV_SOLVE_PERIOD_MS / 1000.0,
+			   GPS_NAV_TOW_SNAP_TOLERANCE_SEC,
+			   &tow_week,
+			   &tow_sec,
+			   &tow_delta_sec))
 		{
 			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
 			        "nav solve rejected by stale/invalid HOW anchor: trk=%u prn=%u anchor_tow=%.3f anchor_abs=%u event_abs=%u\r\n",
@@ -1912,6 +1916,7 @@ void gps_background_task(void)
 	gps_process_isr_events();
 	if(gps_take_timer_maintenance_pending())
 	{
+		gps_pps_service();
 		gps_reconcile_stopped_tracking_channels();
 		gps_mark_stale_measurements();
 		gps_check_acq_stuck_busy();
@@ -10,6 +10,7 @@
 #include "gps/gps_interrupt_handlers.h"
 #include "gps/gps_nav_epoch.h"
 #include "gps/gps_pps.h"
+#include "gps/gps_time_anchor.h"
 #include "gps/defines.h"
 #include <bsp/irq.h>

@@ -61,6 +62,17 @@ volatile gps_ephem_ll_t* tracking_node = NULL;
 static volatile bool gps_initial_position_valid = false;
 static volatile bool gps_ionoutc_valid = false;
 static ionoutc_t gps_last_ionoutc;
+static gps_pps_quality_selector_t gps_pps_quality_selector;
+static gps_pps_profile_t gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER;
+static unsigned int gps_pps_status_log_decimator;
+static bool gps_pps_reference_svid_valid;
+static unsigned int gps_pps_reference_svid;
+static bool gps_pps_filter_reference_svid_valid;
+static unsigned int gps_pps_filter_reference_svid;
+
+#if !defined(GPS_PPS_STATUS_LOG_INTERVAL) || (GPS_PPS_STATUS_LOG_INTERVAL < 1)
+#error "GPS_PPS_STATUS_LOG_INTERVAL must be at least one"
+#endif

 volatile uint64_t gps_systick_ticks = 0;

@@ -658,6 +670,7 @@ static unsigned int gps_collect_completed_epoch_locked(
 	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
 	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
+	double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT],
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
 	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
 	uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
@@ -690,6 +703,9 @@ static unsigned int gps_collect_completed_epoch_locked(
 		snapshot_ips[count] = ip_no;
 		snapshot_pps_counters[count] =
 			epoch->pps_counter[ip_no];
+		snapshot_pps_sample_delays[count] =
+			(double)epoch->raw_sample[ip_no] -
+			epoch->corrected_abs_sample[ip_no];
 		snapshot_pps_sequences[count] =
 			epoch->pps_capture_sequence[ip_no];
 		snapshot_local_timestamps[count] =
@@ -707,6 +723,7 @@ static unsigned int gps_select_coherent_nav_snapshot(
 	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
 	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
+	double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT],
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
 	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
 	uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
@@ -754,6 +771,8 @@ static unsigned int gps_select_coherent_nav_snapshot(
 			snapshot_ips[destination] = snapshot_ips[source];
 			snapshot_pps_counters[destination] =
 				snapshot_pps_counters[source];
+			snapshot_pps_sample_delays[destination] =
+				snapshot_pps_sample_delays[source];
 			snapshot_pps_sequences[destination] =
 				snapshot_pps_sequences[source];
 			snapshot_local_timestamps[destination] =
@@ -799,12 +818,15 @@ static uint64_t gps_nav_median_local_timestamp(

 static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 	                                   const uint64_t pps_counters[GPS_TRCK_IP_COUNT],
+	                                   const double pps_sample_delays[GPS_TRCK_IP_COUNT],
 	                                   const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT],
 	                                   const gps_tracking_measurement_t* measurements,
 	                                   unsigned int satellite_count,
 	                                   gps_nav_solution_t* solution_out)
 {
-	if((ephemeris == NULL) || (measurements == NULL) ||
+	if((ephemeris == NULL) || (pps_counters == NULL) ||
+	   (pps_sample_delays == NULL) ||
+	   (pps_capture_sequences == NULL) || (measurements == NULL) ||
 	   (solution_out == NULL) ||
 	   (satellite_count < GPS_NAV_MIN_WLS_SATELLITES))
 	{
@@ -829,6 +851,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 	ionoutc_t ionoutc_snapshot;
 	const ionoutc_t* ionoutc = gps_get_ionoutc(&ionoutc_snapshot) ? &ionoutc_snapshot : NULL;
 	bool ok = false;
+	bool pps_clock_bias_drift_valid = false;
+	double pps_clock_bias_drift_mps = 0.0;
 	gps_nav_solution_t solution;
 	memset(&solution, 0, sizeof(solution));
 	memset(solution_out, 0, sizeof(*solution_out));
@@ -866,16 +890,30 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 		             satellitePositions, velocities);
 	}

-	double minimum = psTVLtime[0];
-	unsigned int minimum_index = 0u;
-	for(unsigned int i = 1u ; i < satellite_count ; i++)
+	unsigned int satellite_ids[GPS_TRCK_IP_COUNT];
+	for(unsigned int i = 0u; i < satellite_count; i++)
 	{
-		if(psTVLtime[i] < minimum)
-		{
-			minimum = psTVLtime[i];
-			minimum_index = i;
-		}
+		satellite_ids[i] = ephemeris[i].SVID;
 	}
+	unsigned int reference_index = 0u;
+	if(!gps_pps_select_reference(
+		   satellite_ids, psTVLtime, satellite_count,
+		   gps_pps_reference_svid_valid, gps_pps_reference_svid,
+		   &reference_index))
+	{
+		goto cleanup;
+	}
+	double reference_capture_time = psTVLtime[reference_index];
+	uint64_t pps_reference_counter = 0u;
+	double pps_capture_correction_sec = 0.0;
+	bool pps_reference_counter_valid =
+		gps_time_correct_capture_counter(
+			pps_counters[reference_index],
+			pps_sample_delays[reference_index],
+			GPS_RF_SAMPLE_RATE_HZ,
+			(double)GPS_PPS_CLOCK_HZ,
+			&pps_reference_counter,
+			&pps_capture_correction_sec);

 	preciseTxTimes = create_matrix(1, satellite_count, "preciseTxTimes");
 	localPseudoRanges = create_matrix(1, satellite_count, "localPseudoRanges");
@@ -894,7 +932,8 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,

 	for(unsigned int i = 0 ; i < satellite_count ; i++)
 	{
-		preciseTxTimes->matrix[0][i] = psTVLtime[i] - minimum + STARTOFFSET;
+		preciseTxTimes->matrix[0][i] =
+			psTVLtime[i] - reference_capture_time + STARTOFFSET;
 		localPseudoRanges->matrix[0][i] = preciseTxTimes->matrix[0][i] * (SPEED_OF_LIGHT / 1000.0);

 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "psTVLtime=%.10lf preciseTx=%.10lf pseudo=%.10lf clockCorr=%.10lf corrected=%.10lf\r\n",
@@ -957,6 +996,14 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 				     solution.velocity_ecef_mps[1] +
 				     solution.velocity_ecef_mps[2] *
 				     solution.velocity_ecef_mps[2]);
+			pps_clock_bias_drift_valid =
+				gps_pps_reference_bias_drift(
+					solution.ecef_m,
+					solution.velocity_ecef_mps,
+					satellite_positions_array[reference_index],
+					satellite_velocities_array[reference_index],
+					clockDrift[reference_index],
+					&pps_clock_bias_drift_mps);
 		}
 	}
 	if(ok && solution.fde_applied)
@@ -971,27 +1018,221 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 		}
 	}
 	bool pps_reference_used = ok &&
-		((solution.used_row_mask & (1u << minimum_index)) != 0u);
-	if(ok && solution.solution_valid && pps_reference_used &&
-	   (pps_capture_sequences[minimum_index] != 0u))
+		((solution.used_row_mask & (1u << reference_index)) != 0u);
+	if(ok && solution.solution_valid && pps_reference_used)
 	{
+		gps_pps_reference_svid = ephemeris[reference_index].SVID;
+		gps_pps_reference_svid_valid = true;
+	}
+	else if(ok && !pps_reference_used)
+	{
+		/* Select a new datum on the next WLS solve if FDE rejected this one. */
+		gps_pps_reference_svid_valid = false;
+	}
+	gps_pps_quality_metrics_t pps_quality = {0};
+	pps_quality.solution_valid = ok && solution.solution_valid;
+	if(pps_quality.solution_valid)
+	{
+		double cn0_sum = 0.0;
+		double minimum_cn0 = INFINITY;
+		unsigned int cn0_count = 0u;
+		for(unsigned int row = 0u; row < satellite_count; row++)
+		{
+			if(((solution.used_row_mask & (1u << row)) != 0u) &&
+			   isfinite(solution.cn0_dbhz[row]))
+			{
+				cn0_sum += solution.cn0_dbhz[row];
+				minimum_cn0 = fmin(minimum_cn0, solution.cn0_dbhz[row]);
+				cn0_count++;
+			}
+		}
+		pps_quality.tdop = solution.tdop;
+		pps_quality.gdop = solution.gdop;
+		pps_quality.clock_bias_sigma_m =
+			sqrt(solution.clock_bias_variance_m2);
+		pps_quality.mean_cn0_dbhz =
+			(cn0_count != 0u) ? (cn0_sum / (double)cn0_count) : NAN;
+		pps_quality.minimum_cn0_dbhz =
+			(cn0_count != 0u) ? minimum_cn0 : NAN;
+		pps_quality.chi_square_ratio = solution.chi_square_ratio;
+		pps_quality.velocity_residual_rms_mps =
+			solution.velocity_valid ?
+				solution.velocity_residual_rms_mps : 0.0;
+		pps_quality.used_satellites = solution.used_satellite_count;
+		pps_quality.residual_test_valid = solution.residual_test_valid;
+		pps_quality.velocity_valid = solution.velocity_valid;
+		pps_quality.fde_applied = solution.fde_applied;
+	}
+	bool pps_profile_changed = false;
+	/*
+	 * An invalid WLS epoch carries no usable quality evidence and is not fed
+	 * into the profile selector.  It is also never committed below.  This
+	 * lets isolated chi-square rejects pass without resetting a converged
+	 * loop; continuous loss is independently caught by gps_pps_service()'s
+	 * two-second reference watchdog.
+	 */
+	gps_pps_profile_t pps_profile = gps_pps_quality_selector.profile;
+	if(pps_quality.solution_valid)
+	{
+		pps_profile = gps_pps_quality_selector_update(
+			&gps_pps_quality_selector, &pps_quality,
+			&pps_profile_changed);
+	}
+	if(pps_profile == GPS_PPS_PROFILE_HOLDOVER)
+	{
+		gps_pps_set_holdover(true);
+	}
+	gps_pps_profile_t desired_loop_profile =
+		((pps_profile != GPS_PPS_PROFILE_HOLDOVER) &&
+		 !gps_pps_is_locked()) ?
+			GPS_PPS_PROFILE_ACQUIRE : pps_profile;
+	if((desired_loop_profile != GPS_PPS_PROFILE_HOLDOVER) &&
+	   (desired_loop_profile != gps_pps_applied_profile) &&
+	   gps_pps_set_loop_profile(desired_loop_profile))
+	{
+		gps_pps_applied_profile = desired_loop_profile;
+	}
+	if(pps_profile_changed)
+	{
+		GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
+		        "PPS quality profile=%s tdop=%.2lf gdop=%.2lf "
+		        "clock_sigma=%.2lf cn0_mean=%.1lf cn0_min=%.1lf "
+		        "chi2_ratio=%.3lf velocity_rms=%.3lf\r\n",
+		        gps_pps_profile_name(pps_profile),
+		        pps_quality.tdop, pps_quality.gdop,
+		        pps_quality.clock_bias_sigma_m,
+		        pps_quality.mean_cn0_dbhz,
+		        pps_quality.minimum_cn0_dbhz,
+		        pps_quality.chi_square_ratio,
+		        pps_quality.velocity_residual_rms_mps);
+	}
+
+	if(ok && solution.solution_valid && pps_reference_used &&
+	   (pps_capture_sequences[reference_index] != 0u) &&
+	   pps_reference_counter_valid &&
+	   (pps_profile != GPS_PPS_PROFILE_HOLDOVER))
+	{
+		unsigned int reference_svid = ephemeris[reference_index].SVID;
+		if(!gps_pps_filter_reference_svid_valid ||
+		   (gps_pps_filter_reference_svid != reference_svid))
+		{
+			gps_pps_reset_time_filter();
+			gps_pps_filter_reference_svid = reference_svid;
+			gps_pps_filter_reference_svid_valid = true;
+			GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
+			        "PPS WLS datum re-anchor ref_svid=%u\r\n",
+			        reference_svid);
+		}
+		double filtered_clock_bias_m = 0.0;
+		double clock_innovation_m = 0.0;
+		gps_pps_time_filter_result_t filter_result =
+			gps_pps_filter_clock_bias(
+				pps_reference_counter,
+				solution.clock_bias_m,
+				pps_clock_bias_drift_valid,
+				pps_clock_bias_drift_mps,
+				solution.tdop,
+				&filtered_clock_bias_m,
+				&clock_innovation_m);
+		if(filter_result == GPS_PPS_TIME_FILTER_REJECTED)
+		{
+			GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
+			        "PPS clock observation rejected profile=%s bias=%.3lf "
+			        "drift=%.3lf tdop=%.2lf\r\n",
+			        gps_pps_profile_name(pps_profile),
+			        solution.clock_bias_m,
+			        solution.clock_drift_mps,
+			        solution.tdop);
+			goto pps_anchor_done;
+		}
 		/* The synthetic STARTOFFSET datum is absorbed by WLS b.  Removing b/c
 		 * recovers the reference satellite's physical flight time; POSTPONE_TOW
 		 * is applied exactly once here, just as it is in satpos(). */
-		double receiver_tow = ephemeris[minimum_index].tow.sec + POSTPONE_TOW +
-		                      (STARTOFFSET / 1000.0) - (solution.clock_bias_m / SPEED_OF_LIGHT);
-		if(!gps_pps_commit_solution(pps_counters[minimum_index],
-		                            ephemeris[minimum_index].tow.week,
-		                            receiver_tow))
+		double receiver_tow = ephemeris[reference_index].tow.sec + POSTPONE_TOW +
+		                      (STARTOFFSET / 1000.0) -
+		                      (filtered_clock_bias_m / SPEED_OF_LIGHT);
+		/*
+		 * Keep holdover asserted throughout initial settling and re-anchor
+		 * commits. A coarse commit is accepted while held and clears lock.
+		 * gps_pps_commit_solution() releases holdover immediately before a
+		 * normal mailbox request so the first coherent recovery update is
+		 * applied by PPS v2.2 without stepping the held physical timebase.
+		 */
+		if(filter_result == GPS_PPS_TIME_FILTER_COARSE)
+		{
+			gps_pps_set_holdover(true);
+		}
+		if(!gps_pps_commit_solution(pps_reference_counter,
+		                            ephemeris[reference_index].tow.week,
+		                            receiver_tow,
+		                            filter_result == GPS_PPS_TIME_FILTER_COARSE))
 		{
 			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "PPS anchor skipped: mailbox busy or invalid time\r\n");
 		}
+		gps_pps_status_log_decimator++;
+		if(gps_pps_status_log_decimator >= GPS_PPS_STATUS_LOG_INTERVAL)
+		{
+			gps_pps_status_log_decimator = 0u;
+			gps_pps_diagnostics_t pps_diagnostics;
+			bool pps_diagnostics_valid =
+				gps_pps_read_diagnostics(&pps_diagnostics);
+			double hardware_phase_ns = pps_diagnostics_valid ?
+				((double)pps_diagnostics.phase_error_q32 /
+				 4294967296.0) *
+				(1000000000.0 / (double)GPS_PPS_CLOCK_HZ) : NAN;
+			double rate_command_ppm = pps_diagnostics_valid ?
+				((double)pps_diagnostics.rate_word_q32 /
+				 4294967296.0) * 1000000.0 : NAN;
+			double integrator_ppm = pps_diagnostics_valid ?
+				((double)pps_diagnostics.integrator_q32 /
+				 4294967296.0) * 1000000.0 : NAN;
+			GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
+			        "PPS profile=%s mode=%s locked=%u tdop=%.2lf gdop=%.2lf "
+			        "clock_sigma=%.2lf cn0=%.1lf/%.1lf innovation=%.2lfm "
+			        "raw_bias=%.3lfm filtered_bias=%.3lfm "
+			        "clock_drift=%.3lfmps bias_drift=%.3lfmps ref_svid=%u "
+			        "capture_corr=%.2lfns "
+			        "hw_phase=%.2lfns rate_cmd=%.3lfppm integ=%.3lfppm "
+			        "pulse=%u seq=%u status=0x%03x\r\n",
+			        gps_pps_profile_name(pps_profile),
+			        (filter_result == GPS_PPS_TIME_FILTER_COARSE) ?
+			            "coarse" : "tracking",
+			        gps_pps_is_locked() ? 1u : 0u,
+			        pps_quality.tdop, pps_quality.gdop,
+			        pps_quality.clock_bias_sigma_m,
+			        pps_quality.mean_cn0_dbhz,
+			        pps_quality.minimum_cn0_dbhz,
+			        clock_innovation_m,
+			        solution.clock_bias_m,
+			        filtered_clock_bias_m,
+			        solution.clock_drift_mps,
+			        pps_clock_bias_drift_mps,
+			        ephemeris[reference_index].SVID,
+			        pps_capture_correction_sec * 1.0e9,
+			        hardware_phase_ns, rate_command_ppm, integrator_ppm,
+			        pps_diagnostics_valid ?
+			            pps_diagnostics.pulse_count : 0u,
+			        pps_diagnostics_valid ?
+			            pps_diagnostics.active_sequence : 0u,
+			        pps_diagnostics_valid ?
+			            pps_diagnostics.status : 0u);
+		}
+	}
+	else if(ok && solution.solution_valid && pps_reference_used &&
+	        (pps_capture_sequences[reference_index] != 0u) &&
+	        !pps_reference_counter_valid)
+	{
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+		        "PPS anchor rejected: invalid fractional capture correction "
+		        "ref_svid=%u sample_delay=%.9lf\r\n",
+		        ephemeris[reference_index].SVID,
+		        pps_sample_delays[reference_index]);
 	}
 	else if(ok && solution.solution_valid && !pps_reference_used)
 	{
 		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
 		        "PPS anchor protected: FDE excluded pseudorange reference row=%u SVID=%u\r\n",
-		        minimum_index, ephemeris[minimum_index].SVID);
+		        reference_index, ephemeris[reference_index].SVID);
 	}
 	else if(ok && !solution.solution_valid)
 	{
@@ -999,6 +1240,7 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 		        "PPS/last_solution protected from invalid best-effort NAVSOL quality=%.1lf ratio=%.3lf\r\n",
 		        solution.quality_score, solution.chi_square_ratio);
 	}
+pps_anchor_done:

 cleanup:
 	if(preciseTxTimes != NULL) free_matrix(preciseTxTimes);
@@ -1140,6 +1382,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
 	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
+	double snapshot_pps_sample_delays[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
 	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT];
@@ -1186,6 +1429,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 			candidate_count = gps_collect_completed_epoch_locked(
 				&completed_epoch,
 				snapshot, snapshot_ips, snapshot_pps_counters,
+				snapshot_pps_sample_delays,
 				snapshot_pps_sequences,
 				snapshot_local_timestamps, snapshot_raw_samples,
 				snapshot_measurements);
@@ -1211,6 +1455,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 	{
 		satellite_count = gps_select_coherent_nav_snapshot(
 			snapshot, snapshot_ips, snapshot_pps_counters,
+			snapshot_pps_sample_delays,
 			snapshot_pps_sequences,
 			snapshot_local_timestamps, snapshot_raw_samples,
 			snapshot_measurements,
@@ -1267,6 +1512,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 		memset(&nav_solution, 0, sizeof(nav_solution));
 		uint64_t nav_software_start_cycles = gps_read_cycle_counter();
 		bool nav_ok = nav_software_from_snapshot(snapshot, snapshot_pps_counters,
+		                                         snapshot_pps_sample_delays,
 		                                         snapshot_pps_sequences,
 		                                         snapshot_measurements,
 		                                         satellite_count,
@@ -1486,6 +1732,13 @@ void gps_startup_initialization(unsigned int nsp_tick)
 	        GPS_NAV_ENABLE_FDE);
 	gps_init_acq_tables();
 	gps_pps_init(nsp_tick);
+	gps_pps_quality_selector_init(&gps_pps_quality_selector);
+	gps_pps_applied_profile = GPS_PPS_PROFILE_HOLDOVER;
+	gps_pps_status_log_decimator = 0u;
+	gps_pps_reference_svid_valid = false;
+	gps_pps_reference_svid = 0u;
+	gps_pps_filter_reference_svid_valid = false;
+	gps_pps_filter_reference_svid = 0u;
 	gps_ionoutc_valid = false;
 	gps_last_ionoutc = (ionoutc_t){0};
 	gps_nav_epoch_reject_total = 0u;
@@ -1,4 +1,5 @@
 #include "gps/gps_pps.h"
+#include "gps/gps_macros.h"

 #include <bsp/soc.h>
 #include <math.h>
@@ -39,6 +40,7 @@ typedef struct {
 	volatile uint32_t version;              /* 0x7c */
 	volatile uint32_t kp_q46_hi;            /* 0x80 */
 	volatile uint32_t ki_q46_hi;            /* 0x84 */
+	volatile uint32_t loop_config;          /* 0x88, PPS v2.2+ */
 } gps_pps_regs_t;

 #if defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 201112L)
@@ -46,23 +48,44 @@ _Static_assert(offsetof(gps_pps_regs_t, capture_counter_lo) == 0x14, "PPS captur
 _Static_assert(offsetof(gps_pps_regs_t, epoch_counter_lo) == 0x20, "PPS epoch offset mismatch");
 _Static_assert(offsetof(gps_pps_regs_t, commit) == 0x44, "PPS commit offset mismatch");
 _Static_assert(offsetof(gps_pps_regs_t, kp_q46_hi) == 0x80, "PPS coefficient offset mismatch");
+_Static_assert(offsetof(gps_pps_regs_t, loop_config) == 0x88, "PPS loop-config offset mismatch");
 #endif

 enum {
 	GPS_PPS_CONTROL_ENABLE = 1u << 0,
 	GPS_PPS_CONTROL_TRACK = 1u << 1,
+	GPS_PPS_CONTROL_HOLDOVER = 1u << 2,
 	GPS_PPS_CONTROL_CLEAR_STATUS = 1u << 3,
 	GPS_PPS_STATUS_ALIGNED = 1u << 1,
 	GPS_PPS_STATUS_COMMIT_BUSY = 1u << 4,
+	GPS_PPS_STATUS_DIAG_BUSY = 1u << 10,
 	GPS_PPS_STATUS_LOCKED = 1u << 11,
 	GPS_PPS_COMMIT_VALID = 1u << 0,
 	GPS_PPS_COMMIT_COARSE = 1u << 1
 };

+#define GPS_PPS_VERSION_ADAPTIVE_LOOP 0x00020200u
+#define GPS_PPS_REFERENCE_TIMEOUT_TICKS (2u * GPS_PPS_CLOCK_HZ)
+
 static volatile gps_pps_regs_t* const gps_pps =
 	(volatile gps_pps_regs_t*)BSP_GPS_PPS_BASE;
 static uint32_t gps_pps_solution_sequence;
+static uint32_t gps_pps_version;
+static unsigned int gps_pps_nav_solve_tick;
+static gps_pps_profile_t gps_pps_active_loop_profile;
 static bool gps_pps_force_coarse;
+static bool gps_pps_reference_committed;
+static bool gps_pps_holdover;
+static uint64_t gps_pps_last_reference_counter;
+static gps_pps_time_filter_t gps_pps_time_filter;
+
+static uint64_t gps_pps_read_u64(
+	volatile uint32_t* lo, volatile uint32_t* hi)
+{
+	uint32_t low_word = *lo;
+	uint32_t high_word = *hi;
+	return ((uint64_t)high_word << 32) | low_word;
+}

 static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int64_t value)
 {
@@ -71,29 +94,86 @@ static void gps_pps_write_q46(volatile uint32_t* lo, volatile uint32_t* hi, int6
 	*hi = (uint32_t)(encoded >> 32);
 }

-void gps_pps_init(unsigned int nav_solve_tick)
+static void gps_pps_write_loop_profile(gps_pps_profile_t profile)
 {
 	const double pi = 3.14159265358979323846;
-	const double natural_frequency_hz = 0.035;
-	const double damping = 0.9;
-	const double update_period_sec = (nav_solve_tick != 0u) ?
-		((double)nav_solve_tick * 0.020) : 0.200;
-	const double wn = 2.0 * pi * natural_frequency_hz;
+	gps_pps_loop_profile_t parameters = gps_pps_loop_profile(profile);
+	const double update_period_sec = (gps_pps_nav_solve_tick != 0u) ?
+		((double)gps_pps_nav_solve_tick * 0.020) : 0.200;
+	const double wn = 2.0 * pi * parameters.natural_frequency_hz;
 	const double q46 = 70368744177664.0;
-	int64_t kp_q46 = (int64_t)llround((2.0 * damping * wn / GPS_PPS_CLOCK_HZ) * q46);
+	int64_t kp_q46 = (int64_t)llround(
+		(2.0 * parameters.damping * wn / GPS_PPS_CLOCK_HZ) * q46);
 	int64_t ki_q46 = (int64_t)llround((wn * wn * update_period_sec / GPS_PPS_CLOCK_HZ) * q46);
+	uint32_t max_slew_q32 = (uint32_t)llround(
+		parameters.max_slew_ppm * 1.0e-6 * 4294967296.0);

-	gps_pps->control = 0u;
 	gps_pps_write_q46(&gps_pps->kp_q46_lo, &gps_pps->kp_q46_hi, kp_q46);
 	gps_pps_write_q46(&gps_pps->ki_q46_lo, &gps_pps->ki_q46_hi, ki_q46);
-	gps_pps->max_slew_q32 = 85899u; /* 20 ppm of one Q32 tick per local clock. */
+	gps_pps->max_slew_q32 = max_slew_q32;
+	if(gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP)
+	{
+		gps_pps->loop_config =
+			((parameters.lock_acquire_ticks & 0xffu) << 8) |
+			((parameters.lock_release_ticks & 0xffu) << 16) |
+			((parameters.lock_acquire_updates & 0xffu) << 24);
+	}
+}
+
+void gps_pps_init(unsigned int nav_solve_tick)
+{
+	gps_pps_nav_solve_tick = nav_solve_tick;
+	gps_pps->control = 0u;
+	gps_pps_version = gps_pps->version;
+	gps_pps_write_loop_profile(GPS_PPS_PROFILE_ROBUST);
+	gps_pps_active_loop_profile = GPS_PPS_PROFILE_ROBUST;
 	/* A GDB CPU reset does not reset the tracking-clock PPS state.  Continue
 	 * its mailbox sequence and force the first new solution to re-anchor the
 	 * timebase instead of feeding a new epoch into the previous PI state. */
-	gps_pps_solution_sequence = gps_pps->active_sequence;
+	gps_pps_diagnostics_t initial_diagnostics;
+	if(gps_pps_read_diagnostics(&initial_diagnostics))
+	{
+		gps_pps_solution_sequence = initial_diagnostics.active_sequence;
+	}
+	else
+	{
+		gps_pps_solution_sequence = gps_pps->active_sequence;
+	}
 	gps_pps_force_coarse = true;
+	gps_pps_reference_committed = false;
+	gps_pps_holdover = true;
+	gps_pps_last_reference_counter = 0u;
+	gps_pps_time_filter_init(&gps_pps_time_filter);
 	gps_pps->control = GPS_PPS_CONTROL_ENABLE | GPS_PPS_CONTROL_TRACK |
+	                   GPS_PPS_CONTROL_HOLDOVER |
 	                   GPS_PPS_CONTROL_CLEAR_STATUS;
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
+	        "PPS init rtl=0x%08x adaptive=%u loop_config=0x%08x "
+	        "clock_hz=%u update_ticks=%u\r\n",
+	        gps_pps_version,
+	        gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ? 1u : 0u,
+	        gps_pps_version >= GPS_PPS_VERSION_ADAPTIVE_LOOP ?
+	            gps_pps->loop_config : 0u,
+	        GPS_PPS_CLOCK_HZ, gps_pps_nav_solve_tick);
+}
+
+void gps_pps_service(void)
+{
+	if(!gps_pps_reference_committed || gps_pps_holdover)
+	{
+		return;
+	}
+
+	/* Reading LO snapshots HI in hardware, so this is a coherent 64-bit
+	 * free-running counter read over the 32-bit APB interface. */
+	uint32_t now_lo = gps_pps->raw_counter_lo;
+	uint32_t now_hi = gps_pps->raw_counter_hi;
+	uint64_t now = ((uint64_t)now_hi << 32) | now_lo;
+	if((now - gps_pps_last_reference_counter) >
+	   (uint64_t)GPS_PPS_REFERENCE_TIMEOUT_TICKS)
+	{
+		gps_pps_set_holdover(true);
+	}
 }

 bool gps_pps_is_locked(void)
@@ -101,6 +181,70 @@ bool gps_pps_is_locked(void)
 	return (gps_pps->status & GPS_PPS_STATUS_LOCKED) != 0u;
 }

+bool gps_pps_read_diagnostics(gps_pps_diagnostics_t* diagnostics)
+{
+	if((diagnostics == NULL) ||
+	   ((gps_pps->status & GPS_PPS_STATUS_DIAG_BUSY) != 0u))
+	{
+		return false;
+	}
+
+	gps_pps->diag_snapshot = 1u;
+	bool diag_started = false;
+	for(unsigned int attempt = 0u; attempt < 1000u; attempt++)
+	{
+		uint32_t status = gps_pps->status;
+		if((status & GPS_PPS_STATUS_DIAG_BUSY) != 0u)
+		{
+			diag_started = true;
+		}
+		/*
+		 * The 125 MHz snapshot/ack can complete between two CPU MMIO
+		 * reads.  In that legitimate case DIAG_BUSY is never observed.
+		 * Sixteen ordered APB reads are far longer than the request,
+		 * snapshot, and two-flop return path; accepting a clear status
+		 * after that delay still guarantees that the frozen group is new.
+		 */
+		else if(diag_started || (attempt >= 16u))
+		{
+			diagnostics->status = status;
+			diagnostics->active_sequence = gps_pps->active_sequence;
+			diagnostics->pulse_count = gps_pps->pulse_count;
+			diagnostics->phase_error_q32 = (int64_t)gps_pps_read_u64(
+				&gps_pps->phase_error_lo, &gps_pps->phase_error_hi);
+			diagnostics->integrator_q32 = (int64_t)gps_pps_read_u64(
+				&gps_pps->integrator_lo, &gps_pps->integrator_hi);
+			diagnostics->rate_word_q32 = (int64_t)gps_pps_read_u64(
+				&gps_pps->rate_word_lo, &gps_pps->rate_word_hi);
+			return true;
+		}
+	}
+	return false;
+}
+
+bool gps_pps_set_loop_profile(gps_pps_profile_t profile)
+{
+	if((profile == GPS_PPS_PROFILE_HOLDOVER) ||
+	   ((gps_pps->status & GPS_PPS_STATUS_COMMIT_BUSY) != 0u))
+	{
+		return false;
+	}
+	gps_pps_write_loop_profile(profile);
+	gps_pps_active_loop_profile = profile;
+	return true;
+}
+
+void gps_pps_set_holdover(bool holdover)
+{
+	uint32_t control = GPS_PPS_CONTROL_ENABLE | GPS_PPS_CONTROL_TRACK;
+	if(holdover)
+	{
+		control |= GPS_PPS_CONTROL_HOLDOVER;
+	}
+	gps_pps->control = control;
+	gps_pps_holdover = holdover;
+}
+
 bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* sequence)
 {
 	if((channel >= BSP_GPS_TRCK_COUNT) || (counter == NULL) || (sequence == NULL))
@@ -123,7 +267,37 @@ bool gps_pps_read_capture(unsigned int channel, uint64_t* counter, uint32_t* seq
 	return true;
 }

-bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiver_tow_sec)
+gps_pps_time_filter_result_t gps_pps_filter_clock_bias(
+	uint64_t epoch_counter,
+	double measured_clock_bias_m,
+	bool clock_drift_valid,
+	double measured_clock_drift_mps,
+	double tdop,
+	double* filtered_clock_bias_m_out,
+	double* innovation_m_out)
+{
+	gps_pps_loop_profile_t parameters =
+		gps_pps_loop_profile(gps_pps_active_loop_profile);
+	return gps_pps_time_filter_update(
+		&gps_pps_time_filter, epoch_counter, (double)GPS_PPS_CLOCK_HZ,
+		measured_clock_bias_m, clock_drift_valid,
+		measured_clock_drift_mps, tdop,
+		parameters.software_bias_gain,
+		parameters.software_settle_updates,
+		filtered_clock_bias_m_out, innovation_m_out);
+}
+
+void gps_pps_reset_time_filter(void)
+{
+	gps_pps_time_filter_init(&gps_pps_time_filter);
+	gps_pps_force_coarse = true;
+}
+
+bool gps_pps_commit_solution(
+	uint64_t epoch_counter,
+	int gps_week,
+	double receiver_tow_sec,
+	bool force_coarse)
 {
 	if(!isfinite(receiver_tow_sec))
 	{
@@ -160,7 +334,7 @@ bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiv
 	uint64_t gps_seconds = ((uint64_t)(unsigned int)gps_week * 604800ULL) +
 	                       (uint64_t)whole_tow;
 	uint32_t next_sequence = gps_pps_solution_sequence + 1u;
-	bool coarse = gps_pps_force_coarse ||
+	bool coarse = force_coarse || gps_pps_force_coarse ||
 	              ((gps_pps->status & GPS_PPS_STATUS_ALIGNED) == 0u);

 	gps_pps->epoch_counter_lo = (uint32_t)epoch_counter;
@@ -169,9 +343,24 @@ bool gps_pps_commit_solution(uint64_t epoch_counter, int gps_week, double receiv
 	gps_pps->gps_seconds_hi = (uint32_t)(gps_seconds >> 32);
 	gps_pps->gps_fraction_q32 = (uint32_t)fraction_q32;
 	gps_pps->epoch_sequence = next_sequence;
+	/*
+	 * PPS v2.2 deliberately rejects a normal PI update while HOLDOVER is
+	 * asserted.  Release it only after every shadow word is in place and
+	 * immediately before toggling COMMIT, so the first coherent post-gap
+	 * observation is applied instead of merely acknowledged.  The physical
+	 * PPS already runs from the held rate word, so this does not introduce a
+	 * phase step; only the validity status precedes the PI apply by the
+	 * bounded mailbox pipeline latency.
+	 */
+	if(!coarse && gps_pps_holdover)
+	{
+		gps_pps_set_holdover(false);
+	}
 	gps_pps->commit = GPS_PPS_COMMIT_VALID |
 	                  (coarse ? GPS_PPS_COMMIT_COARSE : 0u);
 	gps_pps_solution_sequence = next_sequence;
 	gps_pps_force_coarse = false;
+	gps_pps_reference_committed = true;
+	gps_pps_last_reference_counter = epoch_counter;
 	return true;
 }
@@ -0,0 +1,194 @@
+#include "gps/gps_pps_quality.h"
+
+#include <limits.h>
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+#define GPS_PPS_PROFILE_UPGRADE_UPDATES   8u
+#define GPS_PPS_PROFILE_DEGRADE_UPDATES   3u
+
+static bool gps_pps_metrics_finite(const gps_pps_quality_metrics_t* metrics)
+{
+	return (metrics != NULL) &&
+	       isfinite(metrics->tdop) &&
+	       isfinite(metrics->gdop) &&
+	       isfinite(metrics->clock_bias_sigma_m) &&
+	       isfinite(metrics->mean_cn0_dbhz) &&
+	       isfinite(metrics->minimum_cn0_dbhz) &&
+	       isfinite(metrics->velocity_residual_rms_mps);
+}
+
+static gps_pps_profile_t gps_pps_classify_quality(
+	const gps_pps_quality_metrics_t* metrics)
+{
+	if(!gps_pps_metrics_finite(metrics) ||
+	   !metrics->solution_valid ||
+	   (metrics->used_satellites < 4u) ||
+	   (metrics->tdop < 0.0) ||
+	   (metrics->gdop < 0.0) ||
+	   (metrics->clock_bias_sigma_m < 0.0))
+	{
+		return GPS_PPS_PROFILE_HOLDOVER;
+	}
+
+	bool residual_ok =
+		metrics->residual_test_valid ?
+			(isfinite(metrics->chi_square_ratio) &&
+			 (metrics->chi_square_ratio <= 1.0)) :
+			(metrics->used_satellites == 4u);
+	bool robust =
+		residual_ok &&
+		(metrics->tdop <= 8.0) &&
+		(metrics->gdop <= 12.0) &&
+		(metrics->clock_bias_sigma_m <= 100.0) &&
+		(metrics->mean_cn0_dbhz >= 22.0) &&
+		(metrics->minimum_cn0_dbhz >= 14.0) &&
+		(!metrics->velocity_valid ||
+		 (metrics->velocity_residual_rms_mps <= 3.0));
+	if(!robust)
+	{
+		return GPS_PPS_PROFILE_HOLDOVER;
+	}
+
+	bool precision =
+		(metrics->used_satellites >= 6u) &&
+		metrics->residual_test_valid &&
+		!metrics->fde_applied &&
+		(metrics->tdop <= 3.0) &&
+		(metrics->gdop <= 6.0) &&
+		(metrics->clock_bias_sigma_m <= 35.0) &&
+		(metrics->mean_cn0_dbhz >= 30.0) &&
+		(metrics->minimum_cn0_dbhz >= 20.0) &&
+		(metrics->chi_square_ratio <= 0.75) &&
+		metrics->velocity_valid &&
+		(metrics->velocity_residual_rms_mps <= 1.0);
+	return precision ? GPS_PPS_PROFILE_PRECISION : GPS_PPS_PROFILE_ROBUST;
+}
+
+void gps_pps_quality_selector_init(gps_pps_quality_selector_t* selector)
+{
+	if(selector != NULL)
+	{
+		memset(selector, 0, sizeof(*selector));
+		selector->profile = GPS_PPS_PROFILE_HOLDOVER;
+		selector->candidate = GPS_PPS_PROFILE_HOLDOVER;
+	}
+}
+
+gps_pps_profile_t gps_pps_quality_selector_update(
+	gps_pps_quality_selector_t* selector,
+	const gps_pps_quality_metrics_t* metrics,
+	bool* profile_changed_out)
+{
+	if(profile_changed_out != NULL)
+	{
+		*profile_changed_out = false;
+	}
+	if(selector == NULL)
+	{
+		return GPS_PPS_PROFILE_HOLDOVER;
+	}
+
+	gps_pps_profile_t target =
+		gps_pps_classify_quality(metrics);
+
+	if(target == selector->profile)
+	{
+		selector->candidate = target;
+		selector->candidate_updates = 0u;
+		return selector->profile;
+	}
+	if(target != selector->candidate)
+	{
+		selector->candidate = target;
+		selector->candidate_updates = 1u;
+	}
+	else if(selector->candidate_updates != UINT_MAX)
+	{
+		selector->candidate_updates++;
+	}
+
+	unsigned int required_updates =
+		(target > selector->profile) ?
+			GPS_PPS_PROFILE_UPGRADE_UPDATES :
+			GPS_PPS_PROFILE_DEGRADE_UPDATES;
+	if(selector->candidate_updates >= required_updates)
+	{
+		selector->profile = target;
+		selector->candidate_updates = 0u;
+		if(profile_changed_out != NULL)
+		{
+			*profile_changed_out = true;
+		}
+	}
+	return selector->profile;
+}
+
+gps_pps_loop_profile_t gps_pps_loop_profile(gps_pps_profile_t profile)
+{
+	switch(profile)
+	{
+		case GPS_PPS_PROFILE_ACQUIRE:
+			return (gps_pps_loop_profile_t){
+				.natural_frequency_hz = 0.120,
+				.damping = 0.9,
+				.max_slew_ppm = 20.0,
+				.lock_acquire_ticks = 8u,
+				.lock_release_ticks = 128u,
+				.lock_acquire_updates = 5u,
+				.software_bias_gain = 1.0 / 16.0,
+				.software_settle_updates = 12u
+			};
+		case GPS_PPS_PROFILE_PRECISION:
+			return (gps_pps_loop_profile_t){
+				.natural_frequency_hz = 0.035,
+				.damping = 0.9,
+				.max_slew_ppm = 20.0,
+				.lock_acquire_ticks = 8u,
+				.lock_release_ticks = 128u,
+				.lock_acquire_updates = 5u,
+				.software_bias_gain = 1.0 / 32.0,
+				.software_settle_updates = 12u
+			};
+		case GPS_PPS_PROFILE_ROBUST:
+			return (gps_pps_loop_profile_t){
+				.natural_frequency_hz = 0.012,
+				.damping = 1.0,
+				.max_slew_ppm = 20.0,
+				.lock_acquire_ticks = 8u,
+				.lock_release_ticks = 128u,
+				.lock_acquire_updates = 8u,
+				.software_bias_gain = 1.0 / 32.0,
+				.software_settle_updates = 16u
+			};
+		case GPS_PPS_PROFILE_HOLDOVER:
+		default:
+			return (gps_pps_loop_profile_t){
+				.natural_frequency_hz = 0.0,
+				.damping = 1.0,
+				.max_slew_ppm = 20.0,
+				.lock_acquire_ticks = 8u,
+				.lock_release_ticks = 128u,
+				.lock_acquire_updates = 8u,
+				.software_bias_gain = 1.0 / 32.0,
+				.software_settle_updates = 16u
+			};
+	}
+}
+
+const char* gps_pps_profile_name(gps_pps_profile_t profile)
+{
+	switch(profile)
+	{
+		case GPS_PPS_PROFILE_ACQUIRE:
+			return "acquire";
+		case GPS_PPS_PROFILE_PRECISION:
+			return "precision";
+		case GPS_PPS_PROFILE_ROBUST:
+			return "robust";
+		case GPS_PPS_PROFILE_HOLDOVER:
+		default:
+			return "holdover";
+	}
+}
@@ -0,0 +1,309 @@
+#include "gps/gps_pps_time_filter.h"
+
+#include <math.h>
+#include <stddef.h>
+#include <string.h>
+
+/*
+ * Board data showed hundreds of nanoseconds of epoch-to-epoch pseudorange
+ * clock-bias noise even with seven satellites, while Doppler clock drift was
+ * stable.  A 1/32 residual gain gives roughly a 6.4 s time constant at the
+ * nominal 5 Hz solve rate.  The innovation clamp limits one accepted WLS epoch
+ * to less than 32 ns of reference movement.
+ */
+#define GPS_PPS_FILTER_DRIFT_GAIN         (1.0 / 4.0)
+#define GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN (1.0 / 16.0)
+#define GPS_PPS_FILTER_MAX_DRIFT_MPS       10000.0
+#define GPS_PPS_FILTER_MAX_TDOP           8.0
+#define GPS_PPS_FILTER_MIN_INTERVAL_SEC   0.040
+#define GPS_PPS_FILTER_MAX_PREDICTION_GAP_SEC 30.000
+#define GPS_PPS_FILTER_INNOVATION_CLAMP_M 300.0
+#define GPS_PPS_FILTER_HARD_OUTLIER_M     1500.0
+#define GPS_PPS_SPEED_OF_LIGHT_MPS         299792458.0
+
+static double clamp_double(double value, double limit)
+{
+	if(value > limit)
+	{
+		return limit;
+	}
+	if(value < -limit)
+	{
+		return -limit;
+	}
+	return value;
+}
+
+void gps_pps_time_filter_init(gps_pps_time_filter_t* filter)
+{
+	if(filter != NULL)
+	{
+		memset(filter, 0, sizeof(*filter));
+	}
+}
+
+bool gps_pps_select_reference(
+	const unsigned int satellite_ids[],
+	const double capture_times[],
+	unsigned int count,
+	bool preferred_valid,
+	unsigned int preferred_id,
+	unsigned int* reference_index_out)
+{
+	if((satellite_ids == NULL) || (capture_times == NULL) ||
+	   (reference_index_out == NULL) || (count == 0u))
+	{
+		return false;
+	}
+
+	unsigned int earliest_index = 0u;
+	if(!isfinite(capture_times[0]))
+	{
+		return false;
+	}
+	for(unsigned int index = 0u; index < count; index++)
+	{
+		if(!isfinite(capture_times[index]))
+		{
+			return false;
+		}
+		if(preferred_valid && (satellite_ids[index] == preferred_id))
+		{
+			*reference_index_out = index;
+			return true;
+		}
+		if(capture_times[index] < capture_times[earliest_index])
+		{
+			earliest_index = index;
+		}
+	}
+
+	*reference_index_out = earliest_index;
+	return true;
+}
+
+bool gps_pps_reference_bias_drift(
+	const double receiver_ecef_m[3],
+	const double receiver_velocity_ecef_mps[3],
+	const double reference_satellite_ecef_m[3],
+	const double reference_satellite_velocity_ecef_mps[3],
+	double reference_satellite_clock_drift_sps,
+	double* clock_bias_drift_mps_out)
+{
+	if((receiver_ecef_m == NULL) ||
+	   (receiver_velocity_ecef_mps == NULL) ||
+	   (reference_satellite_ecef_m == NULL) ||
+	   (reference_satellite_velocity_ecef_mps == NULL) ||
+	   (clock_bias_drift_mps_out == NULL) ||
+	   !isfinite(reference_satellite_clock_drift_sps))
+	{
+		return false;
+	}
+
+	double delta_m[3];
+	double range_squared_m2 = 0.0;
+	for(unsigned int component = 0u; component < 3u; component++)
+	{
+		if(!isfinite(receiver_ecef_m[component]) ||
+		   !isfinite(receiver_velocity_ecef_mps[component]) ||
+		   !isfinite(reference_satellite_ecef_m[component]) ||
+		   !isfinite(reference_satellite_velocity_ecef_mps[component]))
+		{
+			return false;
+		}
+		delta_m[component] =
+			reference_satellite_ecef_m[component] -
+			receiver_ecef_m[component];
+		range_squared_m2 += delta_m[component] * delta_m[component];
+	}
+	double range_m = sqrt(range_squared_m2);
+	if(!isfinite(range_m) || (range_m < 1.0))
+	{
+		return false;
+	}
+
+	double geometric_range_rate_mps = 0.0;
+	for(unsigned int component = 0u; component < 3u; component++)
+	{
+		double relative_velocity_mps =
+			reference_satellite_velocity_ecef_mps[component] -
+			receiver_velocity_ecef_mps[component];
+		geometric_range_rate_mps +=
+			(delta_m[component] / range_m) *
+			relative_velocity_mps;
+	}
+	/*
+	 * capture_i - min(capture) removes the receiver clock before position
+	 * WLS. The remaining bias datum is -range(reference), with the broadcast
+	 * satellite-clock correction added to the observation.
+	 */
+	double bias_drift_mps =
+		-geometric_range_rate_mps +
+		GPS_PPS_SPEED_OF_LIGHT_MPS *
+		reference_satellite_clock_drift_sps;
+	if(!isfinite(bias_drift_mps))
+	{
+		return false;
+	}
+	*clock_bias_drift_mps_out = bias_drift_mps;
+	return true;
+}
+
+static gps_pps_time_filter_result_t gps_pps_time_filter_reanchor(
+	gps_pps_time_filter_t* filter,
+	uint64_t epoch_counter,
+	double measured_clock_bias_m,
+	bool clock_drift_valid,
+	double measured_clock_drift_mps,
+	double* filtered_clock_bias_m_out,
+	double* innovation_m_out)
+{
+	filter->epoch_counter = epoch_counter;
+	filter->drift_anchor_counter = epoch_counter;
+	filter->clock_bias_m = measured_clock_bias_m;
+	filter->clock_drift_mps =
+		clock_drift_valid ? measured_clock_drift_mps : 0.0;
+	filter->previous_measurement_m = measured_clock_bias_m;
+	filter->drift_anchor_measurement_m = measured_clock_bias_m;
+	filter->accepted_updates = 1u;
+	filter->initialized = true;
+	filter->drift_initialized = clock_drift_valid;
+	*filtered_clock_bias_m_out = measured_clock_bias_m;
+	*innovation_m_out = 0.0;
+	return GPS_PPS_TIME_FILTER_COARSE;
+}
+
+gps_pps_time_filter_result_t gps_pps_time_filter_update(
+	gps_pps_time_filter_t* filter,
+	uint64_t epoch_counter,
+	double counter_frequency_hz,
+	double measured_clock_bias_m,
+	bool clock_drift_valid,
+	double measured_clock_drift_mps,
+	double tdop,
+	double bias_gain,
+	unsigned int settle_updates,
+	double* filtered_clock_bias_m_out,
+	double* innovation_m_out)
+{
+	if((filter == NULL) || (filtered_clock_bias_m_out == NULL) ||
+	   (innovation_m_out == NULL) ||
+	   !isfinite(counter_frequency_hz) || (counter_frequency_hz <= 0.0) ||
+	   !isfinite(measured_clock_bias_m) ||
+	   !isfinite(tdop) || (tdop < 0.0) || (tdop > GPS_PPS_FILTER_MAX_TDOP) ||
+	   !isfinite(bias_gain) || (bias_gain <= 0.0) || (bias_gain > 1.0) ||
+	   (settle_updates == 0u) ||
+	   (clock_drift_valid && !isfinite(measured_clock_drift_mps)))
+	{
+		return GPS_PPS_TIME_FILTER_REJECTED;
+	}
+
+	if(!filter->initialized)
+	{
+		return gps_pps_time_filter_reanchor(
+			filter, epoch_counter, measured_clock_bias_m,
+			clock_drift_valid, measured_clock_drift_mps,
+			filtered_clock_bias_m_out, innovation_m_out);
+	}
+
+	if(epoch_counter <= filter->epoch_counter)
+	{
+		return GPS_PPS_TIME_FILTER_REJECTED;
+	}
+	uint64_t elapsed_ticks = epoch_counter - filter->epoch_counter;
+	double elapsed_sec = (double)elapsed_ticks / counter_frequency_hz;
+	if(!isfinite(elapsed_sec) ||
+	   (elapsed_sec < GPS_PPS_FILTER_MIN_INTERVAL_SEC))
+	{
+		return GPS_PPS_TIME_FILTER_REJECTED;
+	}
+	/*
+	 * The independent two-second reference watchdog suppresses PPS validity
+	 * during a missing-WLS interval.  Do not discard the Doppler prediction
+	 * or force a phase-stepping coarse commit for the shorter scheduling gaps
+	 * observed on the board.  Reference-satellite changes are reset by the
+	 * caller; only a genuinely long same-datum gap is re-anchored here.
+	 */
+	if(elapsed_sec > GPS_PPS_FILTER_MAX_PREDICTION_GAP_SEC)
+	{
+		return gps_pps_time_filter_reanchor(
+			filter, epoch_counter, measured_clock_bias_m,
+			clock_drift_valid, measured_clock_drift_mps,
+			filtered_clock_bias_m_out, innovation_m_out);
+	}
+
+	double measurement_delta_m =
+		measured_clock_bias_m - filter->previous_measurement_m;
+	if(!clock_drift_valid &&
+	   (fabs(measurement_delta_m) >
+	    GPS_PPS_FILTER_MAX_DRIFT_MPS * elapsed_sec +
+	    GPS_PPS_FILTER_HARD_OUTLIER_M))
+	{
+		return GPS_PPS_TIME_FILTER_REJECTED;
+	}
+	double anchor_elapsed_sec =
+		(double)(epoch_counter - filter->drift_anchor_counter) /
+		counter_frequency_hz;
+	double fallback_drift_mps = clamp_double(
+		(anchor_elapsed_sec > 0.0) ?
+			((measured_clock_bias_m -
+			  filter->drift_anchor_measurement_m) /
+			 anchor_elapsed_sec) :
+			(measurement_delta_m / elapsed_sec),
+		GPS_PPS_FILTER_MAX_DRIFT_MPS);
+	if(!filter->drift_initialized)
+	{
+		/* This happens only while PPS is still coarse.  It prevents the
+		 * hundreds-of-metres/second receiver-clock ramp from being mistaken
+		 * for timing phase when Doppler velocity is temporarily absent. */
+		filter->clock_drift_mps = fallback_drift_mps;
+		filter->drift_initialized = true;
+	}
+
+	double predicted_clock_bias_m =
+		filter->clock_bias_m + filter->clock_drift_mps * elapsed_sec;
+	double innovation_m = measured_clock_bias_m - predicted_clock_bias_m;
+	if(!isfinite(predicted_clock_bias_m) || !isfinite(innovation_m) ||
+	   (fabs(innovation_m) > GPS_PPS_FILTER_HARD_OUTLIER_M))
+	{
+		return GPS_PPS_TIME_FILTER_REJECTED;
+	}
+
+	filter->clock_bias_m =
+		predicted_clock_bias_m +
+		bias_gain *
+		clamp_double(innovation_m, GPS_PPS_FILTER_INNOVATION_CLAMP_M);
+	if(clock_drift_valid)
+	{
+		filter->clock_drift_mps +=
+			GPS_PPS_FILTER_DRIFT_GAIN *
+			(measured_clock_drift_mps - filter->clock_drift_mps);
+	}
+	else
+	{
+		if(filter->accepted_updates < settle_updates)
+		{
+			/* While PPS output is deliberately invalid/coarse, use the slope
+			 * from the re-anchor point so a single noisy first difference
+			 * cannot leave a long clock-ramp transient. */
+			filter->clock_drift_mps = fallback_drift_mps;
+		}
+		else
+		{
+			filter->clock_drift_mps +=
+				GPS_PPS_FILTER_FALLBACK_DRIFT_GAIN *
+				(fallback_drift_mps - filter->clock_drift_mps);
+		}
+	}
+	filter->previous_measurement_m = measured_clock_bias_m;
+	filter->epoch_counter = epoch_counter;
+	if(filter->accepted_updates != UINT32_MAX)
+	{
+		filter->accepted_updates++;
+	}
+
+	*filtered_clock_bias_m_out = filter->clock_bias_m;
+	*innovation_m_out = innovation_m;
+	return (filter->accepted_updates < settle_updates) ?
+		GPS_PPS_TIME_FILTER_COARSE : GPS_PPS_TIME_FILTER_TRACKING;
+}
@@ -109,6 +109,109 @@ bool gps_time_from_sample_anchor(int anchor_week,
 	return true;
 }

+bool gps_time_from_periodic_sample_anchor(
+	int anchor_week,
+	double anchor_tow_sec,
+	uint32_t anchor_raw_sample,
+	double anchor_corrected_sample,
+	uint32_t event_raw_sample,
+	double event_corrected_sample,
+	double samples_per_second,
+	double max_age_sec,
+	double event_period_sec,
+	double max_snap_error_sec,
+	int* event_week_out,
+	double* event_tow_sec_out,
+	double* elapsed_sec_out)
+{
+	if((event_week_out == NULL) || (event_tow_sec_out == NULL) ||
+	   !(event_period_sec > 0.0) || !(max_snap_error_sec >= 0.0) ||
+	   !isfinite(event_period_sec) || !isfinite(max_snap_error_sec))
+	{
+		return false;
+	}
+
+	int sample_week = 0;
+	double sample_tow_sec = 0.0;
+	double sample_elapsed_sec = 0.0;
+	if(!gps_time_from_sample_anchor(
+		   anchor_week, anchor_tow_sec,
+		   anchor_raw_sample, anchor_corrected_sample,
+		   event_raw_sample, event_corrected_sample,
+		   samples_per_second, max_age_sec,
+		   &sample_week, &sample_tow_sec, &sample_elapsed_sec))
+	{
+		return false;
+	}
+	(void)sample_week;
+	(void)sample_tow_sec;
+
+	double event_index = round(sample_elapsed_sec / event_period_sec);
+	double periodic_elapsed_sec = event_index * event_period_sec;
+	if(!isfinite(event_index) || !isfinite(periodic_elapsed_sec) ||
+	   (event_index < 0.0) ||
+	   (fabs(periodic_elapsed_sec - sample_elapsed_sec) >
+	    max_snap_error_sec))
+	{
+		return false;
+	}
+
+	int event_week = anchor_week;
+	double event_tow_sec = gps_time_normalize_week(
+		anchor_tow_sec + periodic_elapsed_sec, &event_week);
+	*event_week_out = event_week;
+	*event_tow_sec_out = event_tow_sec;
+	if(elapsed_sec_out != NULL)
+	{
+		*elapsed_sec_out = periodic_elapsed_sec;
+	}
+	return true;
+}
+
+bool gps_time_correct_capture_counter(
+	uint64_t capture_counter,
+	double fractional_sample_delay,
+	double samples_per_second,
+	double counter_frequency_hz,
+	uint64_t* corrected_counter_out,
+	double* applied_correction_sec_out)
+{
+	if((corrected_counter_out == NULL) ||
+	   !isfinite(fractional_sample_delay) ||
+	   !isfinite(samples_per_second) ||
+	   !isfinite(counter_frequency_hz) ||
+	   (fractional_sample_delay < 0.0) ||
+	   (fractional_sample_delay > 1.0 + 1.0e-6) ||
+	   !(samples_per_second > 0.0) ||
+	   !(counter_frequency_hz > 0.0))
+	{
+		return false;
+	}
+
+	double correction_ticks_exact =
+		fractional_sample_delay *
+		(counter_frequency_hz / samples_per_second);
+	if(!isfinite(correction_ticks_exact) ||
+	   (correction_ticks_exact < 0.0) ||
+	   (correction_ticks_exact > (double)INT64_MAX))
+	{
+		return false;
+	}
+
+	uint64_t correction_ticks = (uint64_t)llround(correction_ticks_exact);
+	if(correction_ticks > capture_counter)
+	{
+		return false;
+	}
+	*corrected_counter_out = capture_counter - correction_ticks;
+	if(applied_correction_sec_out != NULL)
+	{
+		*applied_correction_sec_out =
+			(double)correction_ticks / counter_frequency_hz;
+	}
+	return true;
+}
+
 bool gps_time_anchors_consistent(int first_week,
                                 double first_tow_sec,
                                 uint32_t first_raw_sample,
@@ -8,6 +8,9 @@ TARGETS := test_imu_calibration test_gps_nav_epoch \
 	test_gps_ephemeris_cache test_gps_receiver_hardware_sim
 TARGETS += test_gps_tracking_measurement
 TARGETS += test_gps_satellite_velocity
+TARGETS += test_gps_pps_time_filter
+TARGETS += test_gps_pps_quality
+TARGETS += test_gps_pps_adaptive_loop

 .PHONY: all run sanitize clean

@@ -50,6 +53,19 @@ test_gps_satellite_velocity: test_gps_satellite_velocity.c \
 		-Wno-unused-variable -Wno-sign-compare \
 		-Ifake_bsp -I$(ROOT)/header $^ -lm -o $@

+test_gps_pps_time_filter: test_gps_pps_time_filter.c \
+		$(ROOT)/src/gps/gps_pps_time_filter.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
+test_gps_pps_quality: test_gps_pps_quality.c \
+		$(ROOT)/src/gps/gps_pps_quality.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
+test_gps_pps_adaptive_loop: test_gps_pps_adaptive_loop.c \
+		$(ROOT)/src/gps/gps_pps_time_filter.c \
+		$(ROOT)/src/gps/gps_pps_quality.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
 run: $(TARGETS)
 	./test_imu_calibration
 	./test_gps_nav_epoch
@@ -61,6 +77,9 @@ run: $(TARGETS)
 	./test_gps_receiver_hardware_sim
 	./test_gps_tracking_measurement
 	./test_gps_satellite_velocity
+	./test_gps_pps_time_filter
+	./test_gps_pps_quality
+	./test_gps_pps_adaptive_loop

 sanitize:
 	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
@@ -77,6 +96,13 @@ sanitize:
 		-o test_gps_nav_epoch_coalescing_san
 	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
 		UBSAN_OPTIONS=halt_on_error=1 ./test_gps_nav_epoch_coalescing_san
+	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I$(ROOT)/header test_gps_time_anchor.c \
+		$(ROOT)/src/gps/gps_time_anchor.c -lm \
+		-o test_gps_time_anchor_san
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./test_gps_time_anchor_san
 	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
 		-fsanitize=address,undefined -fno-omit-frame-pointer \
 		-I$(ROOT)/header test_gps_acq_manager.c \
@@ -122,11 +148,36 @@ sanitize:
 	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
 		UBSAN_OPTIONS=halt_on_error=1 \
 		./test_gps_satellite_velocity_san
+	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I$(ROOT)/header test_gps_pps_time_filter.c \
+		$(ROOT)/src/gps/gps_pps_time_filter.c -lm \
+		-o test_gps_pps_time_filter_san
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_time_filter_san
+	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I$(ROOT)/header test_gps_pps_quality.c \
+		$(ROOT)/src/gps/gps_pps_quality.c -lm \
+		-o test_gps_pps_quality_san
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_quality_san
+	$(CC) -O1 -g -std=c11 -Wall -Wextra -Werror \
+		-fsanitize=address,undefined -fno-omit-frame-pointer \
+		-I$(ROOT)/header test_gps_pps_adaptive_loop.c \
+		$(ROOT)/src/gps/gps_pps_time_filter.c \
+		$(ROOT)/src/gps/gps_pps_quality.c -lm \
+		-o test_gps_pps_adaptive_loop_san
+	ASAN_OPTIONS=detect_leaks=1:halt_on_error=1 \
+		UBSAN_OPTIONS=halt_on_error=1 ./test_gps_pps_adaptive_loop_san

 clean:
 	rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro \
 		test_gps_nav_epoch_san test_gps_nav_epoch_coalescing_san \
+		test_gps_time_anchor_san \
 		test_gps_acq_manager_san test_gps_ephemeris_cache_san \
 		test_gps_receiver_hardware_sim_san \
 		test_gps_tracking_measurement_san \
-		test_gps_satellite_velocity_san
+		test_gps_satellite_velocity_san \
+		test_gps_pps_time_filter_san test_gps_pps_quality_san \
+		test_gps_pps_adaptive_loop_san
@@ -0,0 +1,542 @@
+#include <assert.h>
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "gps/gps_pps_quality.h"
+#include "gps/gps_pps_time_filter.h"
+
+#define TRACKING_HZ 125000000.0
+#define UPDATE_PERIOD_SEC 0.2
+#define SPEED_OF_LIGHT_MPS 299792458.0
+#define Q32_SCALE 4294967296.0
+#define Q46_SCALE 70368744177664.0
+
+typedef struct {
+	int64_t phase_q32;
+	int64_t integrator_q32;
+	int64_t rate_q32;
+	bool aligned;
+	bool locked;
+	unsigned int good_updates;
+} synthetic_rtl_t;
+
+typedef struct {
+	uint32_t state;
+} synthetic_rng_t;
+
+typedef enum {
+	SYNTHETIC_STEADY = 0,
+	SYNTHETIC_STARTUP_NOISY_THEN_CLEAN,
+	SYNTHETIC_NOISE_BURSTS,
+	SYNTHETIC_SATELLITE_LOSS,
+	SYNTHETIC_DOPPLER_LOSS,
+	SYNTHETIC_GEOMETRY_DEGRADE,
+	SYNTHETIC_REFERENCE_RANGE_RATE
+} synthetic_pattern_t;
+
+static double synthetic_uniform(synthetic_rng_t* rng)
+{
+	rng->state = rng->state * 1664525u + 1013904223u;
+	return ((double)(rng->state >> 8) + 0.5) / 16777216.0;
+}
+
+static double synthetic_normal(synthetic_rng_t* rng)
+{
+	double sum = 0.0;
+	for(unsigned int sample = 0u; sample < 12u; sample++)
+	{
+		sum += synthetic_uniform(rng);
+	}
+	return sum - 6.0;
+}
+
+static int64_t clamp_i128(__int128 value, int64_t limit)
+{
+	if(value > limit)
+	{
+		return limit;
+	}
+	if(value < -(__int128)limit)
+	{
+		return -limit;
+	}
+	return (int64_t)value;
+}
+
+static int64_t seconds_to_phase_q32(double seconds)
+{
+	return (int64_t)llround(seconds * TRACKING_HZ * Q32_SCALE);
+}
+
+static double phase_q32_to_seconds(int64_t phase_q32)
+{
+	return (double)phase_q32 / (TRACKING_HZ * Q32_SCALE);
+}
+
+static void synthetic_rtl_advance(synthetic_rtl_t* rtl, double oscillator_ppm)
+{
+	int64_t raw_ticks = (int64_t)llround(UPDATE_PERIOD_SEC * TRACKING_HZ);
+	int64_t oscillator_q32 = (int64_t)llround(
+		oscillator_ppm * 1.0e-6 * Q32_SCALE);
+	rtl->phase_q32 += raw_ticks * (oscillator_q32 + rtl->rate_q32);
+}
+
+static void profile_coefficients(
+	gps_pps_loop_profile_t profile,
+	int64_t* kp_q46,
+	int64_t* ki_q46,
+	int64_t* max_slew_q32)
+{
+	const double pi = 3.14159265358979323846;
+	double wn = 2.0 * pi * profile.natural_frequency_hz;
+	*kp_q46 = (int64_t)llround(
+		(2.0 * profile.damping * wn / TRACKING_HZ) * Q46_SCALE);
+	*ki_q46 = (int64_t)llround(
+		(wn * wn * UPDATE_PERIOD_SEC / TRACKING_HZ) * Q46_SCALE);
+	*max_slew_q32 = (int64_t)llround(
+		profile.max_slew_ppm * 1.0e-6 * Q32_SCALE);
+}
+
+static void synthetic_rtl_coarse(synthetic_rtl_t* rtl, int64_t reference_q32)
+{
+	bool was_locked = rtl->locked;
+	memset(rtl, 0, sizeof(*rtl));
+	rtl->phase_q32 = reference_q32;
+	rtl->aligned = true;
+	assert(!was_locked || !rtl->locked);
+}
+
+static void synthetic_rtl_commit(
+	synthetic_rtl_t* rtl,
+	int64_t reference_q32,
+	gps_pps_loop_profile_t profile)
+{
+	int64_t raw_phase_q32 = reference_q32 - rtl->phase_q32;
+
+	int64_t kp_q46;
+	int64_t ki_q46;
+	int64_t max_slew_q32;
+	profile_coefficients(profile, &kp_q46, &ki_q46, &max_slew_q32);
+	int64_t proportional_q32 =
+		(int64_t)(((__int128)raw_phase_q32 * kp_q46) >> 46);
+	__int128 integrator_candidate =
+		(__int128)rtl->integrator_q32 +
+		(((__int128)raw_phase_q32 * ki_q46) >> 46);
+	rtl->integrator_q32 =
+		clamp_i128(integrator_candidate, max_slew_q32);
+	rtl->rate_q32 = clamp_i128(
+		(__int128)rtl->integrator_q32 + proportional_q32,
+		max_slew_q32);
+
+	int64_t acquire_q32 =
+		(int64_t)profile.lock_acquire_ticks << 32;
+	int64_t release_q32 =
+		(int64_t)profile.lock_release_ticks << 32;
+	if(llabs(raw_phase_q32) > release_q32)
+	{
+		rtl->locked = false;
+		rtl->good_updates = 0u;
+	}
+	else if(!rtl->locked)
+	{
+		if(llabs(raw_phase_q32) <= acquire_q32)
+		{
+			rtl->good_updates++;
+			if(rtl->good_updates >= profile.lock_acquire_updates)
+			{
+				rtl->locked = true;
+				rtl->good_updates = 0u;
+			}
+		}
+		else
+		{
+			rtl->good_updates = 0u;
+		}
+	}
+}
+
+static gps_pps_quality_metrics_t precision_quality(void)
+{
+	return (gps_pps_quality_metrics_t){
+		.tdop = 1.7,
+		.gdop = 3.2,
+		.clock_bias_sigma_m = 18.0,
+		.mean_cn0_dbhz = 38.0,
+		.minimum_cn0_dbhz = 27.0,
+		.chi_square_ratio = 0.35,
+		.velocity_residual_rms_mps = 0.25,
+		.used_satellites = 7u,
+		.solution_valid = true,
+		.residual_test_valid = true,
+		.velocity_valid = true,
+		.fde_applied = false
+	};
+}
+
+static gps_pps_quality_metrics_t robust_quality(bool velocity_valid)
+{
+	gps_pps_quality_metrics_t quality = {
+		.tdop = 5.0,
+		.gdop = 8.0,
+		.clock_bias_sigma_m = 65.0,
+		.mean_cn0_dbhz = 27.0,
+		.minimum_cn0_dbhz = 18.0,
+		.chi_square_ratio = 0.85,
+		.velocity_residual_rms_mps = velocity_valid ? 1.8 : 0.0,
+		.used_satellites = 5u,
+		.solution_valid = true,
+		.residual_test_valid = true,
+		.velocity_valid = velocity_valid,
+		.fde_applied = false
+	};
+	return quality;
+}
+
+static void run_noisy_scenario(
+	unsigned int seed,
+	bool with_gap,
+	double oscillator_ppm,
+	double noise_sigma_ns,
+	bool clock_drift_valid,
+	bool use_robust_quality,
+	bool hard_outliers,
+	unsigned int update_count,
+	synthetic_pattern_t pattern,
+	double* first_lock_sec_out,
+	double* one_second_jitter_rms_sec_out,
+	unsigned int* coarse_count_out,
+	unsigned int* rejected_count_out,
+	bool* locked_after_recovery_out)
+{
+	assert(update_count <= 1500u);
+	synthetic_rng_t rng = {.state = seed};
+	gps_pps_quality_selector_t selector;
+	gps_pps_quality_selector_init(&selector);
+	gps_pps_time_filter_t filter;
+	gps_pps_time_filter_init(&filter);
+	synthetic_rtl_t rtl = {0};
+	gps_pps_profile_t quality_profile = GPS_PPS_PROFILE_HOLDOVER;
+	double first_lock_sec = NAN;
+	unsigned int coarse_count = 0u;
+	unsigned int rejected_count = 0u;
+	double phase_history[1500] = {0.0};
+	bool lock_history[1500] = {false};
+
+	for(unsigned int update = 0u; update < update_count; update++)
+	{
+		double time_sec = UPDATE_PERIOD_SEC * (double)update;
+		synthetic_rtl_advance(&rtl, oscillator_ppm);
+		phase_history[update] = phase_q32_to_seconds(rtl.phase_q32);
+		lock_history[update] = rtl.locked;
+		if(with_gap && (time_sec >= 35.0) && (time_sec < 50.0))
+		{
+			/* The firmware reference watchdog deasserts PPS validity after
+			 * two seconds without a successful commit. */
+			if(time_sec >= 37.0)
+			{
+				lock_history[update] = false;
+			}
+			continue;
+		}
+
+		bool sample_drift_valid = clock_drift_valid;
+		bool sample_robust_quality = use_robust_quality;
+		bool sample_solution_valid = true;
+		double sample_noise_sigma_ns = noise_sigma_ns;
+		double reference_range_rate_mps = 0.0;
+		switch(pattern)
+		{
+			case SYNTHETIC_STARTUP_NOISY_THEN_CLEAN:
+				sample_robust_quality = time_sec < 55.0;
+				sample_noise_sigma_ns =
+					(time_sec < 55.0) ? 1100.0 : 120.0;
+				break;
+			case SYNTHETIC_NOISE_BURSTS:
+			{
+				bool burst =
+					((time_sec >= 55.0) && (time_sec < 78.0)) ||
+					((time_sec >= 135.0) && (time_sec < 151.0)) ||
+					((time_sec >= 205.0) && (time_sec < 214.0));
+				sample_robust_quality = burst;
+				sample_noise_sigma_ns = burst ? 900.0 : 150.0;
+				break;
+			}
+			case SYNTHETIC_SATELLITE_LOSS:
+				sample_solution_valid =
+					!(((time_sec >= 55.0) && (time_sec < 78.0)) ||
+					  ((time_sec >= 145.0) && (time_sec < 151.0)));
+				break;
+			case SYNTHETIC_DOPPLER_LOSS:
+				sample_drift_valid =
+					!((time_sec >= 65.0) && (time_sec < 105.0));
+				break;
+			case SYNTHETIC_GEOMETRY_DEGRADE:
+				sample_robust_quality =
+					(time_sec >= 60.0) && (time_sec < 145.0);
+				sample_noise_sigma_ns =
+					sample_robust_quality ? 650.0 : 200.0;
+				break;
+			case SYNTHETIC_REFERENCE_RANGE_RATE:
+				reference_range_rate_mps = 130.0;
+				break;
+			case SYNTHETIC_STEADY:
+			default:
+				break;
+		}
+
+		bool changed = false;
+		gps_pps_quality_metrics_t quality = sample_robust_quality ?
+			robust_quality(sample_drift_valid) : precision_quality();
+		quality.velocity_valid = sample_drift_valid;
+		if(!sample_solution_valid)
+		{
+			quality.solution_valid = false;
+			quality.used_satellites = 2u;
+			quality.velocity_valid = false;
+		}
+		quality_profile = gps_pps_quality_selector_update(
+			&selector, &quality, &changed);
+		if(quality_profile == GPS_PPS_PROFILE_HOLDOVER)
+		{
+			lock_history[update] = false;
+			continue;
+		}
+		gps_pps_profile_t loop_profile =
+			rtl.locked ? quality_profile : GPS_PPS_PROFILE_ACQUIRE;
+		gps_pps_loop_profile_t parameters =
+			gps_pps_loop_profile(loop_profile);
+
+		double true_bias_m =
+			1200.0 +
+			(-reference_range_rate_mps) * time_sec;
+		double noise_sec =
+			synthetic_normal(&rng) * sample_noise_sigma_ns * 1.0e-9;
+		if((update % 37u) == 0u)
+		{
+			noise_sec +=
+				(synthetic_uniform(&rng) < 0.5 ? -1.0 : 1.0) *
+				(hard_outliers ? 6.0e-6 : 900.0e-9);
+		}
+		double measured_bias_m =
+			true_bias_m - noise_sec * SPEED_OF_LIGHT_MPS;
+		double filtered_bias_m = 0.0;
+		double innovation_m = 0.0;
+		gps_pps_time_filter_result_t result = gps_pps_time_filter_update(
+			&filter,
+			(uint64_t)llround(time_sec * TRACKING_HZ),
+			TRACKING_HZ,
+			measured_bias_m,
+			sample_drift_valid,
+			-reference_range_rate_mps +
+				synthetic_normal(&rng) * 0.25,
+			quality.tdop,
+			parameters.software_bias_gain,
+			parameters.software_settle_updates,
+			&filtered_bias_m,
+			&innovation_m);
+		if(result == GPS_PPS_TIME_FILTER_REJECTED)
+		{
+			rejected_count++;
+			continue;
+		}
+		assert(isfinite(innovation_m));
+		int64_t reference_q32 = seconds_to_phase_q32(
+			-(filtered_bias_m - true_bias_m) / SPEED_OF_LIGHT_MPS);
+		if((result == GPS_PPS_TIME_FILTER_COARSE) || !rtl.aligned)
+		{
+			synthetic_rtl_coarse(&rtl, reference_q32);
+			coarse_count++;
+		}
+		else
+		{
+			synthetic_rtl_commit(&rtl, reference_q32, parameters);
+		}
+		if(rtl.locked && !isfinite(first_lock_sec))
+		{
+			first_lock_sec = time_sec;
+		}
+		phase_history[update] = phase_q32_to_seconds(rtl.phase_q32);
+		lock_history[update] = rtl.locked;
+	}
+
+	double jitter_square_sum = 0.0;
+	unsigned int jitter_count = 0u;
+	for(unsigned int update = (update_count / 2u) + 5u;
+	    update < update_count;
+	    update++)
+	{
+		if(lock_history[update] && lock_history[update - 5u])
+		{
+			double difference =
+				phase_history[update] - phase_history[update - 5u];
+			jitter_square_sum += difference * difference;
+			jitter_count++;
+		}
+	}
+	*first_lock_sec_out = first_lock_sec;
+	*one_second_jitter_rms_sec_out =
+		(jitter_count != 0u) ?
+			sqrt(jitter_square_sum / (double)jitter_count) : NAN;
+	*coarse_count_out = coarse_count;
+	*rejected_count_out = rejected_count;
+	*locked_after_recovery_out = lock_history[update_count - 1u];
+}
+
+static void test_noisy_fixed_point_loop(void)
+{
+	for(unsigned int seed = 1u; seed <= 20u; seed++)
+	{
+		double first_lock_sec;
+		double jitter_rms_sec;
+		unsigned int coarse_count;
+		unsigned int rejected_count;
+		bool locked_after_recovery;
+		run_noisy_scenario(
+			seed, false, 1.65, 320.0, true, false, false, 600u,
+			SYNTHETIC_STEADY,
+			&first_lock_sec, &jitter_rms_sec, &coarse_count,
+			&rejected_count, &locked_after_recovery);
+		assert(isfinite(first_lock_sec));
+		assert(first_lock_sec < 15.0);
+		assert(jitter_rms_sec < 20.0e-9);
+		assert(coarse_count >= 10u);
+		assert(rejected_count == 0u);
+		assert(locked_after_recovery);
+	}
+}
+
+static void test_missing_wls_holds_and_resumes_without_reanchor(void)
+{
+	double first_lock_sec;
+	double jitter_rms_sec;
+	unsigned int coarse_count;
+	unsigned int rejected_count;
+	bool locked_after_recovery;
+	run_noisy_scenario(
+		31u, true, 1.65, 320.0, true, false, false, 600u,
+		SYNTHETIC_STEADY,
+		&first_lock_sec, &jitter_rms_sec, &coarse_count,
+		&rejected_count, &locked_after_recovery);
+	assert(first_lock_sec < 15.0);
+	/* Initial settling emits repeated coarse commits; the bounded 15-second
+	 * gap must not add another reset of the already converged PI state. */
+	assert(coarse_count == 11u);
+	assert(rejected_count == 0u);
+	assert(locked_after_recovery);
+	assert(jitter_rms_sec < 20.0e-9);
+}
+
+static void test_embedded_corner_case_matrix(void)
+{
+	for(unsigned int scenario = 0u; scenario < 4u; scenario++)
+	{
+		double first_lock_sec;
+		double jitter_rms_sec;
+		unsigned int coarse_count;
+		unsigned int rejected_count;
+		bool locked_after_recovery;
+		bool no_doppler = scenario == 1u;
+		bool hard_outliers = scenario == 2u;
+		double oscillator_ppm = (scenario == 3u) ? 18.0 : 1.65;
+		double noise_sigma_ns = (scenario == 0u) ? 650.0 : 320.0;
+		run_noisy_scenario(
+			100u + scenario,
+			false,
+			oscillator_ppm,
+			noise_sigma_ns,
+			!no_doppler,
+			(scenario == 0u) || no_doppler,
+			hard_outliers,
+			1200u,
+			SYNTHETIC_STEADY,
+			&first_lock_sec,
+			&jitter_rms_sec,
+			&coarse_count,
+			&rejected_count,
+			&locked_after_recovery);
+		assert(isfinite(first_lock_sec));
+		assert(first_lock_sec < 35.0);
+		assert(isfinite(jitter_rms_sec));
+		assert(jitter_rms_sec < 20.0e-9);
+		assert(coarse_count >= 10u);
+		assert(hard_outliers ? (rejected_count > 0u) :
+		                      (rejected_count == 0u));
+		assert(locked_after_recovery);
+	}
+
+	double first_lock_sec;
+	double jitter_rms_sec;
+	unsigned int coarse_count;
+	unsigned int rejected_count;
+	bool locked_after_recovery;
+	run_noisy_scenario(
+		200u, false, 25.0, 50.0, true, false, false, 600u,
+		SYNTHETIC_STEADY,
+		&first_lock_sec, &jitter_rms_sec, &coarse_count,
+		&rejected_count, &locked_after_recovery);
+	assert(!isfinite(first_lock_sec));
+	assert(!isfinite(jitter_rms_sec));
+	assert(!locked_after_recovery);
+}
+
+static void test_time_varying_environment_matrix(void)
+{
+	const synthetic_pattern_t patterns[] = {
+		SYNTHETIC_STARTUP_NOISY_THEN_CLEAN,
+		SYNTHETIC_NOISE_BURSTS,
+		SYNTHETIC_SATELLITE_LOSS,
+		SYNTHETIC_DOPPLER_LOSS,
+		SYNTHETIC_GEOMETRY_DEGRADE,
+		SYNTHETIC_REFERENCE_RANGE_RATE
+	};
+	for(unsigned int pattern_index = 0u;
+	    pattern_index < (sizeof(patterns) / sizeof(patterns[0]));
+	    pattern_index++)
+	{
+		for(unsigned int seed = 1u; seed <= 12u; seed++)
+		{
+			double first_lock_sec;
+			double jitter_rms_sec;
+			unsigned int coarse_count;
+			unsigned int rejected_count;
+			bool locked_after_recovery;
+			run_noisy_scenario(
+				300u + 31u * pattern_index + seed,
+				false,
+				(patterns[pattern_index] == SYNTHETIC_NOISE_BURSTS) ?
+					-2.5 : 1.65,
+				320.0,
+				true,
+				false,
+				false,
+				1500u,
+				patterns[pattern_index],
+				&first_lock_sec,
+				&jitter_rms_sec,
+				&coarse_count,
+				&rejected_count,
+				&locked_after_recovery);
+			assert(isfinite(first_lock_sec));
+			assert(first_lock_sec < 25.0);
+			assert(isfinite(jitter_rms_sec));
+			assert(jitter_rms_sec < 20.0e-9);
+			assert(coarse_count >= 10u);
+			assert(rejected_count == 0u);
+			assert(locked_after_recovery);
+		}
+	}
+}
+
+int main(void)
+{
+	test_noisy_fixed_point_loop();
+	test_missing_wls_holds_and_resumes_without_reanchor();
+	test_embedded_corner_case_matrix();
+	test_time_varying_environment_matrix();
+	puts("gps_pps_adaptive_loop tests passed");
+	return 0;
+}
@@ -0,0 +1,173 @@
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+
+#include "gps/gps_pps_quality.h"
+
+static gps_pps_quality_metrics_t precision_metrics(void)
+{
+	return (gps_pps_quality_metrics_t){
+		.tdop = 1.7,
+		.gdop = 3.2,
+		.clock_bias_sigma_m = 18.0,
+		.mean_cn0_dbhz = 38.0,
+		.minimum_cn0_dbhz = 29.0,
+		.chi_square_ratio = 0.35,
+		.velocity_residual_rms_mps = 0.25,
+		.used_satellites = 7u,
+		.solution_valid = true,
+		.residual_test_valid = true,
+		.velocity_valid = true,
+		.fde_applied = false
+	};
+}
+
+static void test_hysteretic_profile_transitions(void)
+{
+	gps_pps_quality_selector_t selector;
+	gps_pps_quality_selector_init(&selector);
+	gps_pps_quality_metrics_t metrics = precision_metrics();
+	bool changed = false;
+
+	for(unsigned int update = 0u; update < 7u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+		assert(!changed);
+	}
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
+	assert(changed);
+
+	/* One DOP/CN0 excursion must not chatter the active loop profile. */
+	metrics.tdop = 4.5;
+	metrics.gdop = 7.0;
+	metrics.mean_cn0_dbhz = 27.0;
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
+	assert(!changed);
+	metrics = precision_metrics();
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
+
+	/* Sustained FDE operation degrades to the robust profile. */
+	metrics.fde_applied = true;
+	for(unsigned int update = 0u; update < 2u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
+	}
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
+	assert(changed);
+
+	/* One invalid WLS epoch is skipped; sustained invalidity holds over. */
+	metrics.solution_valid = false;
+	for(unsigned int update = 0u; update < 2u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
+		assert(!changed);
+	}
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+	assert(changed);
+}
+
+static void test_alternating_quality_does_not_upgrade(void)
+{
+	gps_pps_quality_selector_t selector;
+	gps_pps_quality_selector_init(&selector);
+	gps_pps_quality_metrics_t good = precision_metrics();
+	gps_pps_quality_metrics_t poor = good;
+	poor.tdop = 9.0;
+
+	for(unsigned int update = 0u; update < 40u; update++)
+	{
+		const gps_pps_quality_metrics_t* metrics =
+			(update & 1u) ? &poor : &good;
+		bool changed = false;
+		assert(gps_pps_quality_selector_update(
+			&selector, metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+		assert(!changed);
+	}
+}
+
+static void test_profile_parameters_are_ordered(void)
+{
+	gps_pps_loop_profile_t robust =
+		gps_pps_loop_profile(GPS_PPS_PROFILE_ROBUST);
+	gps_pps_loop_profile_t precision =
+		gps_pps_loop_profile(GPS_PPS_PROFILE_PRECISION);
+	gps_pps_loop_profile_t acquire =
+		gps_pps_loop_profile(GPS_PPS_PROFILE_ACQUIRE);
+	assert(robust.natural_frequency_hz < precision.natural_frequency_hz);
+	assert(precision.natural_frequency_hz < acquire.natural_frequency_hz);
+	assert(robust.lock_acquire_updates > precision.lock_acquire_updates);
+	assert(robust.lock_release_ticks >= precision.lock_release_ticks);
+}
+
+static void test_quality_boundaries_and_recovery(void)
+{
+	gps_pps_quality_selector_t selector;
+	gps_pps_quality_selector_init(&selector);
+	gps_pps_quality_metrics_t metrics = precision_metrics();
+	bool changed = false;
+
+	/* Exactly-on-threshold values remain usable in robust mode. */
+	metrics.used_satellites = 5u;
+	metrics.tdop = 8.0;
+	metrics.gdop = 12.0;
+	metrics.clock_bias_sigma_m = 100.0;
+	metrics.mean_cn0_dbhz = 22.0;
+	metrics.minimum_cn0_dbhz = 14.0;
+	metrics.chi_square_ratio = 1.0;
+	metrics.velocity_residual_rms_mps = 3.0;
+	for(unsigned int update = 0u; update < 8u; update++)
+	{
+		gps_pps_quality_selector_update(&selector, &metrics, &changed);
+	}
+	assert(selector.profile == GPS_PPS_PROFILE_ROBUST);
+
+	/* Non-finite metrics and loss below four satellites use fast hysteresis. */
+	metrics.tdop = NAN;
+	for(unsigned int update = 0u; update < 2u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_ROBUST);
+	}
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+	assert(changed);
+
+	/* A single good epoch is not enough to leave holdover. */
+	metrics = precision_metrics();
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+	metrics.used_satellites = 3u;
+	for(unsigned int update = 0u; update < 3u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+	}
+
+	/* Recovery cannot expose PPS until eight consecutive good epochs. */
+	metrics = precision_metrics();
+	for(unsigned int update = 0u; update < 7u; update++)
+	{
+		assert(gps_pps_quality_selector_update(
+			&selector, &metrics, &changed) == GPS_PPS_PROFILE_HOLDOVER);
+	}
+	assert(gps_pps_quality_selector_update(
+		&selector, &metrics, &changed) == GPS_PPS_PROFILE_PRECISION);
+}
+
+int main(void)
+{
+	test_hysteretic_profile_transitions();
+	test_alternating_quality_does_not_upgrade();
+	test_profile_parameters_are_ordered();
+	test_quality_boundaries_and_recovery();
+	puts("gps_pps_quality tests passed");
+	return 0;
+}
@@ -0,0 +1,215 @@
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+
+#include "gps/gps_pps_time_filter.h"
+
+#define COUNTER_HZ 125000000.0
+
+static void test_drift_prediction_rejects_bias_noise(void)
+{
+	static const double noise_m[] = {
+		0.0, 90.0, -120.0, 70.0, -45.0, 130.0, -80.0, 55.0,
+		-105.0, 35.0, 85.0, -65.0, 115.0, -95.0, 50.0, -30.0
+	};
+	gps_pps_time_filter_t filter;
+	gps_pps_time_filter_init(&filter);
+
+	double squared_error_sum = 0.0;
+	unsigned int tracked = 0u;
+	for(unsigned int update = 0u; update < 160u; update++)
+	{
+		double time_sec = 0.2 * (double)update;
+		double true_bias_m = 1200.0 + 475.0 * time_sec;
+		double measured_bias_m =
+			true_bias_m + noise_m[update % (sizeof(noise_m) / sizeof(noise_m[0]))];
+		double filtered_bias_m = 0.0;
+		double innovation_m = 0.0;
+		gps_pps_time_filter_result_t result = gps_pps_time_filter_update(
+			&filter,
+			(uint64_t)llround(time_sec * COUNTER_HZ),
+			COUNTER_HZ,
+			measured_bias_m,
+			true,
+			475.0 + ((update & 1u) ? 0.3 : -0.3),
+			1.7,
+			1.0 / 32.0,
+			16u,
+			&filtered_bias_m,
+			&innovation_m);
+		assert(result != GPS_PPS_TIME_FILTER_REJECTED);
+		assert(isfinite(innovation_m));
+		if(result == GPS_PPS_TIME_FILTER_TRACKING)
+		{
+			double error_m = filtered_bias_m - true_bias_m;
+			squared_error_sum += error_m * error_m;
+			tracked++;
+		}
+	}
+	assert(tracked > 100u);
+	assert(sqrt(squared_error_sum / (double)tracked) < 12.0);
+}
+
+static void test_quality_gap_and_outlier_handling(void)
+{
+	gps_pps_time_filter_t filter;
+	gps_pps_time_filter_init(&filter);
+	double filtered = 0.0;
+	double innovation = 0.0;
+
+	assert(gps_pps_time_filter_update(
+		&filter, 0u, COUNTER_HZ, 100.0, true, 10.0, 1.0, 1.0 / 32.0, 1u,
+		&filtered, &innovation) == GPS_PPS_TIME_FILTER_COARSE);
+	assert(gps_pps_time_filter_update(
+		&filter, 25000000u, COUNTER_HZ, 102.0, true, 10.0, 12.0, 1.0 / 32.0, 1u,
+		&filtered, &innovation) == GPS_PPS_TIME_FILTER_REJECTED);
+
+	uint64_t saved_epoch = filter.epoch_counter;
+	double saved_bias = filter.clock_bias_m;
+	assert(gps_pps_time_filter_update(
+		&filter, 25000000u, COUNTER_HZ, 5000.0, true, 10.0, 1.0, 1.0 / 32.0, 1u,
+		&filtered, &innovation) == GPS_PPS_TIME_FILTER_REJECTED);
+	assert(filter.epoch_counter == saved_epoch);
+	assert(filter.clock_bias_m == saved_bias);
+	assert(gps_pps_time_filter_update(
+		&filter, 0u, COUNTER_HZ, 100.0, true, 10.0, 1.0,
+		1.0 / 32.0, 1u, &filtered, &innovation) ==
+		GPS_PPS_TIME_FILTER_REJECTED);
+	assert(filter.epoch_counter == saved_epoch);
+	assert(filter.clock_bias_m == saved_bias);
+
+	assert(gps_pps_time_filter_update(
+		&filter, 375000000u, COUNTER_HZ, 130.0, true, 10.0, 1.0, 1.0 / 32.0, 1u,
+		&filtered, &innovation) == GPS_PPS_TIME_FILTER_TRACKING);
+	assert(fabs(filtered - 130.0) < 1.0e-12);
+
+	/* A gap beyond the bounded Doppler-prediction horizon must still reset
+	 * the filter and require a safe coarse alignment. */
+	assert(gps_pps_time_filter_update(
+		&filter, 4250000000ULL, COUNTER_HZ, 440.0, true, 10.0,
+		1.0, 1.0 / 32.0, 1u, &filtered, &innovation) ==
+		GPS_PPS_TIME_FILTER_COARSE);
+	assert(fabs(filtered - 440.0) < 1.0e-12);
+}
+
+static void test_bias_ramp_without_doppler_uses_bounded_fallback(void)
+{
+	static const double noise_m[] = {
+		80.0, -110.0, 45.0, -25.0, 125.0, -70.0, 35.0, -95.0,
+		60.0, -40.0, 105.0, -85.0, 20.0, -15.0, 75.0, -55.0
+	};
+	gps_pps_time_filter_t filter;
+	gps_pps_time_filter_init(&filter);
+
+	double squared_error_sum = 0.0;
+	unsigned int tracked = 0u;
+	for(unsigned int update = 0u; update < 500u; update++)
+	{
+		double time_sec = 0.2 * (double)update;
+		double true_bias_m = 1200.0 + 475.0 * time_sec;
+		double measured_bias_m =
+			true_bias_m +
+			noise_m[update % (sizeof(noise_m) / sizeof(noise_m[0]))];
+		double filtered_bias_m = 0.0;
+		double innovation_m = 0.0;
+		gps_pps_time_filter_result_t result = gps_pps_time_filter_update(
+			&filter,
+			(uint64_t)llround(time_sec * COUNTER_HZ),
+			COUNTER_HZ,
+			measured_bias_m,
+			false,
+			0.0,
+			5.0,
+			1.0 / 32.0,
+			16u,
+			&filtered_bias_m,
+			&innovation_m);
+		assert(result != GPS_PPS_TIME_FILTER_REJECTED);
+		if((result == GPS_PPS_TIME_FILTER_TRACKING) && (update >= 200u))
+		{
+			double error_m = filtered_bias_m - true_bias_m;
+			squared_error_sum += error_m * error_m;
+			tracked++;
+		}
+	}
+	assert(tracked > 250u);
+	assert(filter.drift_initialized);
+	assert(fabs(filter.clock_drift_mps) < 10000.0);
+	assert(sqrt(squared_error_sum / (double)tracked) < 45.0);
+}
+
+static void test_reference_geometry_defines_bias_drift(void)
+{
+	const double receiver_position_m[3] = {0.0, 0.0, 0.0};
+	const double receiver_velocity_mps[3] = {10.0, -2.0, 1.0};
+	const double satellite_position_m[3] = {20200000.0, 0.0, 0.0};
+	const double satellite_velocity_mps[3] = {140.0, 30.0, -10.0};
+	double bias_drift_mps = 0.0;
+
+	/* LOS is +X, so WLS datum drift is -(140 - 10) = -130 m/s. */
+	assert(gps_pps_reference_bias_drift(
+		receiver_position_m,
+		receiver_velocity_mps,
+		satellite_position_m,
+		satellite_velocity_mps,
+		0.0,
+		&bias_drift_mps));
+	assert(fabs(bias_drift_mps + 130.0) < 1.0e-12);
+
+	/* Broadcast satellite-clock drift is part of the position observation. */
+	assert(gps_pps_reference_bias_drift(
+		receiver_position_m,
+		receiver_velocity_mps,
+		satellite_position_m,
+		satellite_velocity_mps,
+		1.0e-9,
+		&bias_drift_mps));
+	assert(fabs(
+		bias_drift_mps -
+		(-130.0 + 299792458.0 * 1.0e-9)) < 1.0e-12);
+
+	double invalid_satellite_position[3] = {0.0, 0.0, 0.0};
+	assert(!gps_pps_reference_bias_drift(
+		receiver_position_m,
+		receiver_velocity_mps,
+		invalid_satellite_position,
+		satellite_velocity_mps,
+		0.0,
+		&bias_drift_mps));
+}
+
+static void test_reference_selection_is_stable(void)
+{
+	const unsigned int ids[] = {3u, 8u, 17u};
+	const double captures[] = {102.0, 100.0, 104.0};
+	unsigned int index = 99u;
+
+	assert(gps_pps_select_reference(
+		ids, captures, 3u, false, 0u, &index));
+	assert(index == 1u);
+
+	/* A newly earlier satellite must not move an existing WLS datum. */
+	assert(gps_pps_select_reference(
+		ids, captures, 3u, true, 3u, &index));
+	assert(index == 0u);
+
+	/* Loss of the preferred satellite selects the earliest remaining one. */
+	assert(gps_pps_select_reference(
+		ids, captures, 3u, true, 25u, &index));
+	assert(index == 1u);
+
+	double invalid_captures[] = {102.0, NAN, 104.0};
+	assert(!gps_pps_select_reference(
+		ids, invalid_captures, 3u, false, 0u, &index));
+}
+
+int main(void)
+{
+	test_drift_prediction_rejects_bias_noise();
+	test_quality_gap_and_outlier_handling();
+	test_bias_ramp_without_doppler_uses_bounded_fallback();
+	test_reference_geometry_defines_bias_drift();
+	test_reference_selection_is_stable();
+	puts("gps_pps_time_filter tests passed");
+	return 0;
+}
@@ -1,10 +1,11 @@
 #include "gps/gps_time_anchor.h"
+#include "gps/Navigation.h"

 #include <math.h>
 #include <stdint.h>
 #include <stdio.h>

-#define SAMPLE_RATE_HZ 16368000.0
+#define SAMPLE_RATE_HZ GPS_RF_SAMPLE_RATE_HZ

 static int failures;

@@ -99,6 +100,89 @@ static void invalid_inputs_are_rejected(void)
 	      "out-of-range HOW was accepted");
 }

+static void physical_rf_rate_avoids_six_second_sawtooth(void)
+{
+	const double elapsed_sec = 6.0;
+	const double samples = GPS_RF_SAMPLE_RATE_HZ * elapsed_sec;
+	const double legacy_elapsed_sec = samples / 16368000.0;
+	const double corrected_elapsed_sec = samples / GPS_RF_SAMPLE_RATE_HZ;
+
+	CHECK(fabs((legacy_elapsed_sec - elapsed_sec) * 1.0e9) > 6900.0,
+	      "legacy 16.368 MHz conversion did not expose HOW sawtooth");
+	CHECK(fabs((corrected_elapsed_sec - elapsed_sec) * 1.0e9) < 0.001,
+	      "physical RF rate left a six-second timing ramp");
+}
+
+static void periodic_event_time_removes_receive_range_rate(void)
+{
+	const uint32_t anchor = 700000000u;
+	const double physical_elapsed_sec = 5.8 + 8.0e-6;
+	const uint32_t event =
+		anchor + (uint32_t)llround(physical_elapsed_sec * SAMPLE_RATE_HZ);
+	int week = 0;
+	double tow_sec = 0.0;
+	double elapsed_sec = 0.0;
+
+	CHECK(gps_time_from_periodic_sample_anchor(
+		      380, 469362.0, anchor, (double)anchor,
+		      event, (double)event, SAMPLE_RATE_HZ, 120.0,
+		      0.2, 0.005, &week, &tow_sec, &elapsed_sec),
+	      "periodic NAV event with range-rate delay was rejected");
+	CHECK(week == 380, "periodic NAV event changed GPS week");
+	CHECK(fabs(tow_sec - 469367.8) < 1.0e-9,
+	      "periodic NAV event retained receive-time range-rate error");
+	CHECK(fabs(elapsed_sec - 5.8) < 1.0e-12,
+	      "periodic NAV event did not report snapped elapsed time");
+
+	const uint32_t off_grid_event =
+		anchor + (uint32_t)llround(5.82 * SAMPLE_RATE_HZ);
+	CHECK(!gps_time_from_periodic_sample_anchor(
+		       380, 469362.0, anchor, (double)anchor,
+		       off_grid_event, (double)off_grid_event,
+		       SAMPLE_RATE_HZ, 120.0, 0.2, 0.005,
+		       &week, &tow_sec, &elapsed_sec),
+		      "off-grid NAV event was accepted");
+}
+
+static void fractional_code_phase_corrects_capture_counter(void)
+{
+	const uint64_t capture_counter = 1000000u;
+	uint64_t corrected_counter = 0u;
+	double correction_sec = 0.0;
+
+	CHECK(gps_time_correct_capture_counter(
+		      capture_counter, 0.75, SAMPLE_RATE_HZ, 125000000.0,
+		      &corrected_counter, &correction_sec),
+	      "valid fractional code phase did not correct PPS capture");
+	/* 0.75 RF sample is 45.82 ns, rounded onto the 8 ns PPS counter grid. */
+	CHECK(corrected_counter == capture_counter - 6u,
+	      "fractional code phase used the wrong capture-counter correction");
+	CHECK(fabs(correction_sec - 48.0e-9) < 1.0e-15,
+	      "capture correction did not report the applied integer tick");
+
+	CHECK(gps_time_correct_capture_counter(
+		      capture_counter, 0.0, SAMPLE_RATE_HZ, 125000000.0,
+		      &corrected_counter, &correction_sec) &&
+	      (corrected_counter == capture_counter),
+	      "zero code residue changed the capture counter");
+	CHECK(!gps_time_correct_capture_counter(
+		       capture_counter, -0.1, SAMPLE_RATE_HZ, 125000000.0,
+		       &corrected_counter, NULL),
+	      "negative fractional sample delay was accepted");
+	CHECK(!gps_time_correct_capture_counter(
+		       capture_counter, 1.1, SAMPLE_RATE_HZ, 125000000.0,
+		       &corrected_counter, NULL),
+	      "capture correction larger than one RF sample was accepted");
+	CHECK(!gps_time_correct_capture_counter(
+		       2u, 1.0, SAMPLE_RATE_HZ, 125000000.0,
+		       &corrected_counter, NULL),
+	      "capture-counter underflow was accepted");
+	CHECK(!gps_time_correct_capture_counter(
+		       UINT64_MAX, 1.0, 1.0, 1.0e30,
+		       &corrected_counter, NULL),
+	      "capture correction beyond llround range was accepted");
+}
+
 int main(void)
 {
 	normal_and_skipped_subframes_are_consistent();
@@ -106,6 +190,9 @@ int main(void)
 	temporary_week_can_upgrade_on_first_subframe_one();
 	captured_future_jump_is_rejected_and_recovers();
 	invalid_inputs_are_rejected();
+	physical_rf_rate_avoids_six_second_sawtooth();
+	periodic_event_time_removes_receive_range_rate();
+	fractional_code_phase_corrects_capture_counter();
 	if(failures == 0)
 	{
 		puts("gps_time_anchor tests passed");