feat(pps): add adaptive disciplined timing loop

soc/software/gps_system/header/gps/NavEquations.h

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

soc/software/gps_system/header/gps/Navigation.h

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

soc/software/gps_system/header/gps/gps_pps.h

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

soc/software/gps_system/header/gps/gps_pps_quality.h

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

soc/software/gps_system/header/gps/gps_pps_time_filter.h

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

soc/software/gps_system/header/gps/gps_time_anchor.h

@@ -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. */