feat(pps): add adaptive disciplined timing loop

soc/software/gps_system/tests/Makefile

@@ -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

soc/software/gps_system/tests/test_gps_pps_adaptive_loop.c

@@ -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;
+}

soc/software/gps_system/tests/test_gps_pps_quality.c

@@ -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;
+}

soc/software/gps_system/tests/test_gps_pps_time_filter.c

@@ -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;
+}

soc/software/gps_system/tests/test_gps_time_anchor.c

@@ -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");