Add GPS integrity checks and GPS IMU ESKF fusion tools

soc/software/gps_system/Makefile

@@ -1,9 +1,15 @@
 TARGET := gps_system
 
+# The current Genesys2 Rocket/fabric domain that clocks the UART is 50 MHz.
+# Keep the requested 115200 baud physical rather than producing 57600 baud
+# with the common 100 MHz fallback divider.
+BSP_UART_CLOCK_HZ ?= 50000000
+
 APP_SRCS := \
 	gps_test.c \
 	src/gps_ip.c \
 	src/gps_interrupt_handlers.c \
+	src/gps_time_anchor.c \
 	src/gps_pps.c \
 	src/linked_list.c \
 	src/NavEquations.c \
@@ -17,3 +23,7 @@ LDLIBS += -lm
 ifeq ($(GPS_DEBUG),1)
 CFLAGS += -DDEBUG -DGPS_DEBUG_LEVEL=$(GPS_DEBUG_LEVEL)
 endif
+
+ifeq ($(GPS_NAV_DIAGNOSTICS),1)
+CFLAGS += -DGPS_NAV_JUMP_DIAGNOSTICS=1
+endif

soc/software/gps_system/gps_test.c

@@ -9,7 +9,7 @@
 int main()
 {
 		
-	gps_startup_initialization(10u);
+	gps_startup_initialization(15);
 
 	acq_start_loop();
 

soc/software/gps_system/header/NavEquations.h

@@ -5,6 +5,56 @@
 		#include "gps_ip_defines.h"
 		#include "matrixLib.h"
 		
+		typedef struct {
+			double ecef_m[3];
+			double clock_bias_m;
+			double latitude_deg;
+			double longitude_deg;
+			double height_m;
+			double covariance_enu_m2[3][3];
+			double hacc_m;
+			double vacc_m;
+			double gdop;
+			double pdop;
+			double hdop;
+			double vdop;
+			double tdop;
+			double residual_rms_m;
+			double max_abs_residual_m;
+			double postfit_residual_m[GPS_TRCK_IP_COUNT];
+			double elevation_deg[GPS_TRCK_IP_COUNT];
+			double measurement_sigma_m[GPS_TRCK_IP_COUNT];
+			double full_postfit_residual_m[GPS_TRCK_IP_COUNT];
+			double full_elevation_deg[GPS_TRCK_IP_COUNT];
+			double full_measurement_sigma_m[GPS_TRCK_IP_COUNT];
+			double weighted_residual_rms_m;
+			double weighted_residual_sum;
+			double chi_square_threshold;
+			double chi_square_ratio;
+			double full_chi_square_ratio;
+			double postfit_position_correction_m;
+			double postfit_clock_correction_m;
+			double full_postfit_position_correction_m;
+			double full_postfit_clock_correction_m;
+			double quality_score;
+			unsigned int available_satellite_count;
+			unsigned int used_satellite_count;
+			unsigned int degrees_of_freedom;
+			uint32_t used_row_mask;
+			uint32_t excluded_row_mask;
+			uint64_t candidate_cycles;
+			uint64_t full_solution_cycles;
+			uint64_t fde_cycles;
+			uint64_t total_wls_cycles;
+			unsigned int iteration_count;
+			unsigned int fde_trial_count;
+			bool candidate_valid;
+			bool residual_test_valid;
+			bool chi_square_pass;
+			bool fde_applied;
+			bool solution_valid;
+		} gps_nav_solution_t;
+
 		    void satpos(gps_ephemeris_t* eph, double* pos, double* vel, double* clk, double* clockCorr);
 		    void computeRange(double* psTVLTime, unsigned char sat_no, range_t* rho, gps_ephemeris_t* eph, ionoutc_t* ionoutc, double* xyz, t_temp_pos* tempPos, double* clockCorr, mat_type *satellitePositions, mat_type * velocities);
 		    double check_t(long double tkl);
@@ -15,7 +65,7 @@
 			                     mat_type * az, mat_type * el,
 			                     const unsigned char* ura_indices,
 			                     const ionoutc_t* ionoutc, double gps_tow,
-			                     double solution_out[4]); // x/y/z and common clock bias in metres
+			                     gps_nav_solution_t* solution_out);
 		    void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot);
 		    void topocent(const double* XPtr, const double* dxPtr, int size, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);
 		    void togeod(double a, double finv, double X, double Y, double Z, t_phi_Lambda_h * phi_Lambda_h, t_topocentData* topcent);

soc/software/gps_system/header/gps_interrupt_handlers.h

@@ -3,6 +3,19 @@
  
 	#include "gps_ip_defines.h"
 
+	typedef struct {
+		uint32_t event_overflow_total;
+		uint32_t plic_drain_limit_total;
+		uint32_t plic_repeat_total;
+		uint32_t nav_epoch_coalesced_total;
+		uint32_t satellite_connected_total;
+		uint32_t satellite_lost_total;
+		uint32_t last_connected_prn;
+		uint32_t last_lost_prn;
+		uint64_t last_connected_tick;
+		uint64_t last_lost_tick;
+	} gps_runtime_diagnostics_t;
+
 	bool assign_tracking_from_result(const gps_acq_result_t* result);
 	void acq_intr_handler(unsigned char engine_id);
 	void gps_dispatch_irq(unsigned int irq_id);
@@ -10,6 +23,8 @@
 	void gps_process_isr_events(void);
 	void gps_acq_scheduler_run(void);
 	void gps_background_task(void);
+	bool gps_take_nav_solve_period_ready(void);
+	void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
 	void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no);
 
 

soc/software/gps_system/header/gps_ip_defines.h

@@ -45,11 +45,26 @@
 		#define GPS_ACQ_INTR_LINE(index)    (BSP_GPS_ACQ_IRQ + ((unsigned int)(index) * BSP_GPS_ACQ_IRQ_STRIDE))
 			#define GPS_EVENT_QUEUE_SIZE        64u
 			#define GPS_SYSTICK_HZ              20u
+			#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
+			#define GPS_NAV_SOLVE_TIMER_TICKS   50u
+			#define GPS_NAV_SOLVE_PERIOD_MS     (GPS_NAV_SOLVE_TIMER_TICKS * GPS_NAV_SOLVE_TIMER_TICK_MS)
+			/* Keep integrity statistics active, but do not let leave-one-out FDE
+			 * replace the full-WLS position until the real measurement-noise model
+			 * is calibrated.  The live stationary test produced a 22.7% exclusion
+			 * rate and substantially worse position dispersion. */
+			#define GPS_NAV_ENABLE_FDE          0u
+			#define GPS_PROFILE_CPU_HZ          50000000ULL
 			#define GPS_ACQ_RETRY_DELAY_TICKS   (GPS_SYSTICK_HZ / 2u)
 			#define GPS_ACQ_STUCK_WARN_TICKS    (GPS_SYSTICK_HZ * 2u)
 			#define GPS_NAV_STATUS_LOG_PERIOD_TICKS GPS_SYSTICK_HZ
 			#define GPS_TRK_MEASUREMENT_STALE_TICKS (GPS_SYSTICK_HZ * 3u)
 			#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
+			#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
+			#define GPS_HOW_ANCHOR_MAX_AGE_SEC         12.0
+
+			_Static_assert(GPS_NAV_SOLVE_PERIOD_MS == 1000u, "NAV solve period must be 1 second");
+			_Static_assert((GPS_NAV_ENABLE_FDE == 0u) || (GPS_NAV_ENABLE_FDE == 1u),
+			               "GPS_NAV_ENABLE_FDE must be zero or one");
 
 	#if (ACQ_ENGINE_COUNT < 1UL)
 		#error "ACQ_ENGINE_COUNT must be at least 1"
@@ -430,13 +445,19 @@
 		unsigned int gps_get_acq_intr_line(unsigned int engine_id);
 		bool gps_prn_valid(unsigned int prn_no);
 		uint64_t gps_get_systick(void);
+		static inline uint64_t gps_read_cycle_counter(void)
+		{
+			uint64_t cycles;
+			asm volatile("rdcycle %0" : "=r"(cycles));
+			return cycles;
+		}
 		bool gps_get_ionoutc(ionoutc_t* ionoutc);
 		void gps_background_task(void);
 		void gps_process_isr_events(void);
 		void gps_acq_scheduler_run(void);
 	// parse functions. Generic parse function.
 		unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
-		                             bool update_tow);
+		                             unsigned int* how_tow_sec_out);
 #ifdef EKF_ENABLE
 void calculate_position(Ekf* g_ekf);
 #else

soc/software/gps_system/header/gps_macros.h

@@ -7,8 +7,8 @@
 	 * Verbosity is cumulative:
 	 *   1: navigation result (position and DOP)
 	 *   2: tracked channel/PRN summary
-	 *   3: state transitions, warnings and errors
-	 *   4: per-epoch and low-level diagnostic detail
+	 *   3: state transitions, warnings/errors and integrity summary
+	 *   4: performance, NAV satellite and low-level diagnostic detail
 	 *
 	 * The aliases keep call sites descriptive without creating extra verbosity
 	 * levels. GPS_DEBUG_LEVEL should normally be in the range 1..4.

soc/software/gps_system/header/gps_time_anchor.h

@@ -0,0 +1,34 @@
+#ifndef GPS_TIME_ANCHOR_H_INCLUDED
+#define GPS_TIME_ANCHOR_H_INCLUDED
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define GPS_TIME_WEEK_SECONDS      604800.0
+#define GPS_TIME_HALF_WEEK_SECONDS 302400.0
+
+/* Resolve the week of a newly received HOW against the previous HOW anchor.
+ * The HOW contains seconds-of-week; the signed half-week rule makes the
+ * Saturday/Sunday rollover explicit without depending on a NAV timer. */
+int gps_time_resolve_how_week(bool previous_valid,
+                              int previous_week,
+                              double previous_tow_sec,
+                              int fallback_week,
+                              double how_tow_sec);
+
+/* Convert a receiver-sample timestamp to GPS time from a HOW/sample anchor.
+ * raw counters are modulo 2^32.  max_age_sec must keep the signed difference
+ * unambiguous (and also rejects a stale HOW anchor). */
+bool gps_time_from_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,
+                                 int* event_week_out,
+                                 double* event_tow_sec_out,
+                                 double* elapsed_sec_out);
+
+#endif

soc/software/gps_system/header/linked_list.h

@@ -3,17 +3,16 @@
 
 #include "gps_ip_defines.h"
 
-typedef enum {
-	GPS_TOW_SOURCE_HOW = 0,
-	GPS_TOW_SOURCE_NAV_SOLVE
-} gps_tow_source_t;
-
 typedef struct gps_ephem_ll {
 	volatile gps_ephemeris_t* data_p;
 	unsigned char tracking_ip_no;
 	struct gps_ephem_ll* next;
 	volatile unsigned int gps_subframe_mask;
-	volatile gps_tow_source_t gps_tow_source;
+	volatile gpstime_t gps_how_tow;
+	volatile uint32_t gps_how_abs_sample;
+	volatile double gps_how_corrected_abs_sample;
+	volatile uint64_t gps_how_tick;
+	volatile bool gps_how_anchor_valid;
 	volatile uint32_t gps_nav_solve_sequence;
 	volatile uint32_t gps_nav_solve_consumed_sequence;
 	volatile uint32_t gps_last_nav_solve_abs_sample;

soc/software/gps_system/src/NavEquations.c

@@ -4,6 +4,7 @@
 #include "gps_ip_defines.h"
 #include "gps.h"
 #include <math.h>
+#include <string.h>
 #include "defines.h"
 
 
@@ -370,6 +371,36 @@ static double covariance_along_axis(const mat_type* covariance, const double axi
 	return value;
 }
 
+static double covariance_between_axes(const mat_type* covariance,
+	                                  const double first_axis[3],
+	                                  const double second_axis[3])
+{
+	double value = 0.0;
+	for(int row = 0 ; row < 3 ; row++)
+	{
+		for(int col = 0 ; col < 3 ; col++)
+		{
+			value += first_axis[row] * covariance->matrix[row][col] * second_axis[col];
+		}
+	}
+	return value;
+}
+
+static double least_squares_chi_square_threshold_99(unsigned int degrees_of_freedom)
+{
+	/* 99th percentile for chi-square distributions with 1..5 DOF.  The
+	 * current hardware has at most nine GPS channels and four WLS states. */
+	static const double threshold[6] = {
+		0.0,
+		6.634896601,
+		9.210340372,
+		11.34486673,
+		13.27670414,
+		15.08627247
+	};
+	return (degrees_of_freedom < 6u) ? threshold[degrees_of_freedom] : 0.0;
+}
+
 static bool least_squares_solve_weighted_update(const mat_type* A,
                                                 const mat_type* omc,
                                                 const double* weights,
@@ -492,22 +523,118 @@ static double klobuchar_delay_meters(const ionoutc_t* ionoutc,
 	return obliquity * delay_seconds * SPEED_OF_LIGHT;
 }
 
+static bool least_squares_build_postfit(const mat_type* satpos,
+	                                    const mat_type* obs,
+	                                    int satellite_count,
+	                                    const unsigned char* ura_indices,
+	                                    const ionoutc_t* ionoutc,
+	                                    double gps_tow,
+	                                    const double state[4],
+	                                    mat_type* design,
+	                                    mat_type* az,
+	                                    mat_type* el,
+	                                    double* residuals_m,
+	                                    double* absolute_weights)
+{
+	if((satpos == NULL) || (obs == NULL) || (ura_indices == NULL) ||
+	   (state == NULL) || (design == NULL) || (az == NULL) || (el == NULL) ||
+	   (residuals_m == NULL) || (absolute_weights == NULL))
+	{
+		return false;
+	}
+
+	const double dtr = 0.01745329251994329576923690768489;
+	for(int i = 0 ; i < satellite_count ; i++)
+	{
+		double dx = satpos->matrix[i][0] - state[0];
+		double dy = satpos->matrix[i][1] - state[1];
+		double dz = satpos->matrix[i][2] - state[2];
+		double approximate_range = sqrt(dx*dx + dy*dy + dz*dz);
+		if(!isfinite(approximate_range) || (approximate_range <= 1.0))
+		{
+			return false;
+		}
+
+		double travel_time = approximate_range / SPEED_OF_LIGHT;
+		double omega_tau = OMEGA_EARTH * travel_time;
+		double cos_omega_tau = cos(omega_tau);
+		double sin_omega_tau = sin(omega_tau);
+		double rotated_satellite[3] = {
+			 cos_omega_tau * satpos->matrix[i][0] + sin_omega_tau * satpos->matrix[i][1],
+			-sin_omega_tau * satpos->matrix[i][0] + cos_omega_tau * satpos->matrix[i][1],
+			 satpos->matrix[i][2]
+		};
+		double difference[3] = {
+			rotated_satellite[0] - state[0],
+			rotated_satellite[1] - state[1],
+			rotated_satellite[2] - state[2]
+		};
+		double geometric_range = normVect(difference);
+		if(!isfinite(geometric_range) || (geometric_range <= 1.0))
+		{
+			return false;
+		}
+
+		t_phi_Lambda_h geodetic;
+		t_topocentData topocentric;
+		memset(&geodetic, 0, sizeof(geodetic));
+		memset(&topocentric, 0, sizeof(topocentric));
+		topocent(state, difference, 3, &geodetic, &topocentric);
+		az->matrix[0][i] = topocentric.azim;
+		el->matrix[0][i] = topocentric.elv;
+
+		double weight = least_squares_measurement_weight(ura_indices[i], topocentric.elv, true);
+		if(!isfinite(weight) || (weight <= 0.0))
+		{
+			return false;
+		}
+		absolute_weights[i] = weight;
+
+		double iono = klobuchar_delay_meters(ionoutc, gps_tow,
+		                                             geodetic.phi, geodetic.lambda,
+		                                             topocentric.azim, topocentric.elv);
+		double trop = 0.0;
+		if((topocentric.elv > 0.0) &&
+		   (topocentric.height >= -1000.0) && (topocentric.height <= 15000.0))
+		{
+			trop = tropo(sin(topocentric.elv * dtr), topocentric.height / 1000.0);
+			if(!isfinite(trop) || (trop < 0.0))
+			{
+				trop = 0.0;
+			}
+		}
+
+		residuals_m[i] = obs->matrix[0][i] - geometric_range - state[3] - trop - iono;
+		if(!isfinite(residuals_m[i]))
+		{
+			return false;
+		}
+		for(int component = 0 ; component < 3 ; component++)
+		{
+			design->matrix[i][component] = -difference[component] / geometric_range;
+		}
+		design->matrix[i][3] = 1.0;
+	}
+	return true;
+}
+
+static bool gps_wls_last_solution_valid = false;
+static double gps_wls_last_solution[4] = {0.0, 0.0, 0.0, 0.0};
+
 // Function leastSquaresSTD
 // Definition : initialize all matc cpp class variables.
 // Arguments : 
 // satpos - type mat_type*,  matc class type replacement
 // obs    - type mat_type*,  matc class type replacement 
-bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
-	                 mat_type * az, mat_type * el,
-	                 const unsigned char* ura_indices,
-	                 const ionoutc_t* ionoutc, double gps_tow,
-	                 double solution_out[4])
+static bool least_squares_candidate(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
+	                                mat_type * az, mat_type * el,
+	                                const unsigned char* ura_indices,
+	                                const ionoutc_t* ionoutc, double gps_tow,
+	                                const double seed[4], bool seed_valid,
+	                                gps_nav_solution_t* solution_out)
 {       //TODO computeRange i kapatmıştım, onu tekrar açmalıyım.
-    static bool last_solution_valid = false;
-    static double last_solution[4] = {0.0, 0.0, 0.0, 0.0};
-
 	if((satpos == NULL) || (obs == NULL) || (az == NULL) || (el == NULL) ||
-	   (ura_indices == NULL) ||
+	   (ura_indices == NULL) || (solution_out == NULL) ||
 	   (satpos->matrix == NULL) || (obs->matrix == NULL) ||
 	   (ActiveSatellitesNumber < MIN_NEEDED_SATELLITE) ||
 	   ((unsigned int)ActiveSatellitesNumber > GPS_TRCK_IP_COUNT))
@@ -516,6 +643,8 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
                 ActiveSatellitesNumber);
         return false;
     }
+	memset(solution_out, 0, sizeof(*solution_out));
+	uint64_t candidate_start_cycles = gps_read_cycle_counter();
 
 //BURASI MATLAB GIRDILERININ KULLANILARAK, LEAST SQUARE IN, CROSS-CHECK ETMEK ICIN KULLANILDIGI YER. KALDIRILABILIR
 //*****************************************************************************************************************
@@ -601,14 +730,14 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
         //initialize_vec(&obs->matrix[0][0], precisePseudoRanges);                  //TODO:Burası DEBUG da kontrol edilecek YUSUF, belki de hiç precisePseudoRanges olmamalı
         mat_type* omc                  = create_matrix(1, ActiveSatellitesNumber, "omc");                 //1x6 matrix for DEBUG
         mat_type* A                    = create_matrix(ActiveSatellitesNumber, 4, "A");                   //6x4 matrix for DEBUG
-	double* satWeights             = (double*)malloc(ActiveSatellitesNumber * sizeof(double));
-	if((posVec == NULL) || (omc == NULL) || (A == NULL) || (satWeights == NULL))
+	double* absoluteWeights        = (double*)malloc(ActiveSatellitesNumber * sizeof(double));
+	if((posVec == NULL) || (omc == NULL) || (A == NULL) || (absoluteWeights == NULL))
 	{
 		GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "least squares matrix allocation failed\r\n");
 		free_matrix(posVec);
 		free_matrix(omc);
 		free_matrix(A);
-		free(satWeights);
+		free(absoluteWeights);
 		free(phi_lambda_h);
 		free(topcent);
 		return false;
@@ -619,30 +748,31 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	initialize_all(0.0, A);
 	for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
 	{
-		satWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
+		absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], 0.0, false);
 	}
 
-	bool seeded_from_last_solution = last_solution_valid;
+	bool seeded_from_last_solution = seed_valid && (seed != NULL);
 	for(int i = 0 ; seeded_from_last_solution && (i <= POSITION_VECTOR_COL) ; i++)
 	{
-		if(!isfinite(last_solution[i]))
+		if(!isfinite(seed[i]))
 		{
 			seeded_from_last_solution = false;
-			last_solution_valid = false;
-			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "discarded non-finite cached position\r\n");
+			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "discarded non-finite WLS seed\r\n");
 		}
 	}
 	if(seeded_from_last_solution)
 	{
 		for(int i = 0 ; i <= POSITION_VECTOR_COL ; i++)
 		{
-			posVec->matrix[0][i] = last_solution[i];
+			posVec->matrix[0][i] = seed[i];
 		}
 	}
 	
+	unsigned int iteration_count = 0u;
 	//=============== Iteratively Find receiver position =================================================================
         for (unsigned iteration = 0; iteration < NUMBER_OF_ITERATIONS; iteration++)
 	{
+	iteration_count = iteration + 1u;
         mat_type* Rot_X                = create_matrix(1, 3, "Rot_X");                                    //1x3 matrix for DEBUG
         if(Rot_X == NULL)
         {
@@ -713,7 +843,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
                 //TODO: topocent e giren matrix lerin elemanları doğru yükleniyor mu diye bakılacak. !!!!!!!!!!!!!!!!!1
 		az->matrix[0][i] = topcent->azim;
 		el->matrix[0][i] = topcent->elv;
-		satWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true);
+		absoluteWeights[i] = least_squares_measurement_weight(ura_indices[i], topcent->elv, true);
 		iono = klobuchar_delay_meters(ionoutc, gps_tow,
 		                                  phi_lambda_h->phi, phi_lambda_h->lambda,
 		                                  topcent->azim, topcent->elv);
@@ -771,13 +901,18 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
                 //  for(int n = 0 ; n < posVec->rows*posVec->columns ; n++)
                 // posVec->matrix[0][n] = 0;             	// TODO rank 4'ten kucukse denklem cozulemez, dolayısıyla bır ust sevıyeye donulmesi gerekir..
 		//}
-		if(!least_squares_normalize_weights(satWeights, ActiveSatellitesNumber))
+		double solveWeights[GPS_TRCK_IP_COUNT];
+		for(int weight_index = 0 ; weight_index < ActiveSatellitesNumber ; weight_index++)
+		{
+			solveWeights[weight_index] = absoluteWeights[weight_index];
+		}
+		if(!least_squares_normalize_weights(solveWeights, ActiveSatellitesNumber))
 		{
 			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "weighted least squares rejected invalid URA/elevation weights\r\n");
 			goto least_squares_fail;
 		}
 		double xVec[4] = {0.0, 0.0, 0.0, 0.0};
-		bool weighted_ok = least_squares_solve_weighted_update(A, omc, satWeights, xVec);
+		bool weighted_ok = least_squares_solve_weighted_update(A, omc, solveWeights, xVec);
 		if(!weighted_ok)
 		{
 			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
@@ -805,9 +940,14 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 		#ifdef COUT_DEBUG
                   printf(" posVec[0]: %.12lf \n posVec[1]: %.12lf \n posVec[2]: %.12lf \n posVec[3]: %.12lf \n",posVec->matrix[0][0],posVec->matrix[0][1],posVec->matrix[0][2],posVec->matrix[0][3] );
 		#endif
-	                double norm = normVect(xVec);
+			double update_norm = sqrt((xVec[0] * xVec[0]) +
+			                          (xVec[1] * xVec[1]) +
+			                          (xVec[2] * xVec[2]) +
+			                          (xVec[3] * xVec[3]));
 
-			if(norm < 1e-4)
+			/* Position and receiver clock bias are all WLS states.  Ignoring the
+			 * clock term can stop with an unconverged common-mode residual. */
+			if(update_norm < 1e-4)
 			{
 				break;
 			}
@@ -827,7 +967,7 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 		        ecef_radius, X1, Y1, Z1);
 		free_matrix(A);
 		free_matrix(posVec);
-		free(satWeights);
+		free(absoluteWeights);
 		free(phi_lambda_h);
 		free(topcent);
 		return false;
@@ -841,29 +981,108 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 		        phi_lambda_h->phi, phi_lambda_h->lambda, phi_lambda_h->height);
 		free_matrix(A);
 		free_matrix(posVec);
-		free(satWeights);
+		free(absoluteWeights);
 		free(phi_lambda_h);
 		free(topcent);
 		return false;
 	}
 
-	/* DOP describes geometry, not the URA/elevation stochastic model used by
-	 * WLS. Build an unweighted normal matrix, then rotate its ECEF position
-	 * covariance into local ENU before reporting horizontal and vertical DOP. */
+	double residuals_m[GPS_TRCK_IP_COUNT];
+	if(!least_squares_build_postfit(satpos, obs, ActiveSatellitesNumber,
+	                                ura_indices, ionoutc, gps_tow,
+	                                posVec->matrix[0], A, az, el,
+	                                residuals_m, absoluteWeights))
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "post-fit residual construction failed\r\n");
+		free_matrix(A);
+		free_matrix(posVec);
+		free(absoluteWeights);
+		free(phi_lambda_h);
+		free(topcent);
+		return false;
+	}
+
+	double residual_square_sum = 0.0;
+	double absolute_weight_sum = 0.0;
+	double weighted_residual_sum = 0.0;
+	for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
+	{
+		residual_square_sum += residuals_m[i] * residuals_m[i];
+		absolute_weight_sum += absoluteWeights[i];
+		weighted_residual_sum += absoluteWeights[i] * residuals_m[i] * residuals_m[i];
+	}
+	if(!isfinite(residual_square_sum) || !isfinite(absolute_weight_sum) ||
+	   !isfinite(weighted_residual_sum) || (absolute_weight_sum <= 0.0))
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "post-fit residual statistics invalid\r\n");
+		free_matrix(A);
+		free_matrix(posVec);
+		free(absoluteWeights);
+		free(phi_lambda_h);
+		free(topcent);
+		return false;
+	}
+
+	unsigned int degrees_of_freedom = (unsigned int)ActiveSatellitesNumber - 4u;
+	double chi_square_threshold = least_squares_chi_square_threshold_99(degrees_of_freedom);
+	bool residual_test_valid = (degrees_of_freedom > 0u) && (chi_square_threshold > 0.0);
+	double chi_square_ratio = residual_test_valid
+	                        ? (weighted_residual_sum / chi_square_threshold)
+	                        : INFINITY;
+	bool chi_square_pass = residual_test_valid &&
+	                       (weighted_residual_sum <= chi_square_threshold);
+
+	/* Absolute weights retain the physical 1/m^2 scale needed by covariance.
+	 * The normalized weights above are used only by the numerical WLS solve. */
+	mat_type* covariance_normal = least_squares_weighted_normal_matrix(A, absoluteWeights);
+	mat_type* position_covariance = (covariance_normal != NULL)
+	                              ? calculate_matrix_inverse(covariance_normal) : NULL;
+	free_matrix(covariance_normal);
+	if(position_covariance == NULL)
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "position covariance skipped singular geometry\r\n");
+		free_matrix(A);
+		free_matrix(posVec);
+		free(absoluteWeights);
+		free(phi_lambda_h);
+		free(topcent);
+		return false;
+	}
+	double postfit_rhs[4] = {0.0, 0.0, 0.0, 0.0};
+	for(int col = 0 ; col < 4 ; col++)
+	{
+		for(int row = 0 ; row < ActiveSatellitesNumber ; row++)
+		{
+			postfit_rhs[col] += A->matrix[row][col] * absoluteWeights[row] * residuals_m[row];
+		}
+	}
+	double postfit_correction[4] = {0.0, 0.0, 0.0, 0.0};
+	for(int row = 0 ; row < 4 ; row++)
+	{
+		for(int col = 0 ; col < 4 ; col++)
+		{
+			postfit_correction[row] += position_covariance->matrix[row][col] * postfit_rhs[col];
+		}
+	}
+	solution_out->postfit_position_correction_m = normVect(postfit_correction);
+	solution_out->postfit_clock_correction_m = postfit_correction[3];
+
+	/* DOP describes geometry only, so it uses an unweighted normal matrix. */
 	double dop_weights[GPS_TRCK_IP_COUNT];
 	for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
 	{
 		dop_weights[i] = 1.0;
 	}
 	mat_type* dop_normal = least_squares_weighted_normal_matrix(A, dop_weights);
-	free_matrix(A);
 	mat_type* dop_covariance = (dop_normal != NULL) ? calculate_matrix_inverse(dop_normal) : NULL;
 	free_matrix(dop_normal);
 	if(dop_covariance == NULL)
 	{
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "DOP skipped singular geometry\r\n");
+		free_matrix(position_covariance);
+		free_matrix(A);
 		free_matrix(posVec);
-		free(satWeights);
+		free(absoluteWeights);
 		free(phi_lambda_h);
 		free(topcent);
 		return false;
@@ -897,8 +1116,10 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
 		        "DOP rejected invalid ENU geometry h2=%e v2=%e p2=%e t2=%e g2=%e\r\n",
 		        hdop2, vdop2, pdop2, tdop2, gdop2);
+		free_matrix(position_covariance);
+		free_matrix(A);
 		free_matrix(posVec);
-		free(satWeights);
+		free(absoluteWeights);
 		free(phi_lambda_h);
 		free(topcent);
 		return false;
@@ -909,37 +1130,85 @@ bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber
 	double hdop = sqrt(hdop2);
 	double vdop = sqrt(vdop2);
 	double tdop = sqrt(tdop2);
-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "DOP GDOP=%.10lf PDOP=%.10lf HDOP=%.10lf VDOP=%.10lf TDOP=%.10lf\r\n",
-	        gdop, pdop, hdop, vdop, tdop);
-	(void)gdop;
-	(void)pdop;
-	(void)hdop;
-	(void)vdop;
-	(void)tdop;
-	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
-	        "position lat=%.20lf lon=%.20lf height=%.20lf\r\n",
-	        phi_lambda_h->phi, phi_lambda_h->lambda, phi_lambda_h->height);
-	int UTMzone = findUtmZone(phi_lambda_h->phi, phi_lambda_h->lambda);
-	GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "UTM zone=%d\r\n", UTMzone);
-	(void)UTMzone;
 
-	for(int i = 0 ; i <= POSITION_VECTOR_COL ; i++)
+	const double* enu_axes[3] = {east, north, up};
+	for(int row = 0 ; row < 3 ; row++)
 	{
-		last_solution[i] = posVec->matrix[0][i];
-		if(solution_out != NULL)
+		for(int col = 0 ; col < 3 ; col++)
 		{
-			solution_out[i] = posVec->matrix[0][i];
+			solution_out->covariance_enu_m2[row][col] =
+				covariance_between_axes(position_covariance, enu_axes[row], enu_axes[col]);
 		}
 	}
-	last_solution_valid = true;
+	free_matrix(position_covariance);
+	free_matrix(A);
+
+	double horizontal_variance = solution_out->covariance_enu_m2[0][0] +
+	                             solution_out->covariance_enu_m2[1][1];
+	double vertical_variance = solution_out->covariance_enu_m2[2][2];
+	if(!isfinite(horizontal_variance) || !isfinite(vertical_variance) ||
+	   (horizontal_variance < 0.0) || (vertical_variance < 0.0))
+	{
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "position covariance rejected hvar=%e vvar=%e\r\n",
+		        horizontal_variance, vertical_variance);
+		free_matrix(posVec);
+		free(absoluteWeights);
+		free(phi_lambda_h);
+		free(topcent);
+		return false;
+	}
+
+	for(int component = 0 ; component < 3 ; component++)
+	{
+		solution_out->ecef_m[component] = posVec->matrix[0][component];
+	}
+	solution_out->clock_bias_m = posVec->matrix[0][3];
+	solution_out->latitude_deg = phi_lambda_h->phi;
+	solution_out->longitude_deg = phi_lambda_h->lambda;
+	solution_out->height_m = phi_lambda_h->height;
+	solution_out->hacc_m = sqrt(horizontal_variance);
+	solution_out->vacc_m = sqrt(vertical_variance);
+	solution_out->gdop = gdop;
+	solution_out->pdop = pdop;
+	solution_out->hdop = hdop;
+	solution_out->vdop = vdop;
+	solution_out->tdop = tdop;
+	solution_out->residual_rms_m = sqrt(residual_square_sum / (double)ActiveSatellitesNumber);
+	for(int i = 0 ; i < ActiveSatellitesNumber ; i++)
+	{
+		solution_out->postfit_residual_m[i] = residuals_m[i];
+		solution_out->elevation_deg[i] = el->matrix[0][i];
+		solution_out->measurement_sigma_m[i] = 1.0 / sqrt(absoluteWeights[i]);
+		if(fabs(residuals_m[i]) > solution_out->max_abs_residual_m)
+		{
+			solution_out->max_abs_residual_m = fabs(residuals_m[i]);
+		}
+	}
+	solution_out->weighted_residual_rms_m = sqrt(weighted_residual_sum / absolute_weight_sum);
+	solution_out->weighted_residual_sum = weighted_residual_sum;
+	solution_out->chi_square_threshold = chi_square_threshold;
+	solution_out->chi_square_ratio = chi_square_ratio;
+	solution_out->quality_score = residual_test_valid ? (100.0 / (1.0 + chi_square_ratio)) : 0.0;
+	solution_out->available_satellite_count = (unsigned int)ActiveSatellitesNumber;
+	solution_out->used_satellite_count = (unsigned int)ActiveSatellitesNumber;
+	solution_out->degrees_of_freedom = degrees_of_freedom;
+	solution_out->used_row_mask = (1u << (unsigned int)ActiveSatellitesNumber) - 1u;
+	solution_out->excluded_row_mask = 0u;
+	solution_out->candidate_valid = true;
+	solution_out->residual_test_valid = residual_test_valid;
+	solution_out->chi_square_pass = chi_square_pass;
+	solution_out->fde_applied = false;
+	solution_out->solution_valid = chi_square_pass;
+	solution_out->iteration_count = iteration_count;
+	solution_out->candidate_cycles = gps_read_cycle_counter() - candidate_start_cycles;
 
     	//free(phi_lambda_h, sizeof(t_phi_Lambda_h));
     	//free(topcent, sizeof(t_topocentData));;   //TODO: re-create by malloc
         free(phi_lambda_h);
 	free(topcent);
 	free_matrix(posVec);
-        free(satWeights);
+        free(absoluteWeights);
 //===============================================================================================
 return true;
 
@@ -947,12 +1216,227 @@ least_squares_fail:
         free_matrix(A);
         free_matrix(omc);
         free_matrix(posVec);
-        free(satWeights);
+        free(absoluteWeights);
         free(phi_lambda_h);
         free(topcent);
         return false;
 }
 
+bool leastSquaresSTD(mat_type* satpos, mat_type* obs, int ActiveSatellitesNumber,
+	                 mat_type * az, mat_type * el,
+	                 const unsigned char* ura_indices,
+	                 const ionoutc_t* ionoutc, double gps_tow,
+	                 gps_nav_solution_t* solution_out)
+{
+	if(solution_out == NULL)
+	{
+		return false;
+	}
+	memset(solution_out, 0, sizeof(*solution_out));
+	uint64_t total_wls_start_cycles = gps_read_cycle_counter();
+
+	gps_nav_solution_t full_solution;
+	if(!least_squares_candidate(satpos, obs, ActiveSatellitesNumber,
+	                            az, el, ura_indices, ionoutc, gps_tow,
+	                            gps_wls_last_solution,
+	                            gps_wls_last_solution_valid,
+	                            &full_solution))
+	{
+		return false;
+	}
+
+	gps_nav_solution_t selected_solution = full_solution;
+	bool passing_candidate_found = full_solution.chi_square_pass;
+	uint32_t full_row_mask = (1u << (unsigned int)ActiveSatellitesNumber) - 1u;
+	uint64_t fde_start_cycles = 0u;
+	unsigned int fde_trial_count = 0u;
+
+	/* Six available measurements leave only five after exclusion (one residual
+	 * DOF).  In that geometry several leave-one-out candidates can pass while
+	 * producing widely separated positions.  Keep the legacy full-WLS position
+	 * as an explicitly invalid best effort until at least two residual DOF remain
+	 * after exclusion (seven available measurements). */
+	if((GPS_NAV_ENABLE_FDE != 0u) && !full_solution.chi_square_pass &&
+	   (ActiveSatellitesNumber >= 7))
+	{
+		fde_start_cycles = gps_read_cycle_counter();
+		for(int excluded_row = 0 ; excluded_row < ActiveSatellitesNumber ; excluded_row++)
+		{
+			fde_trial_count++;
+			int subset_count = ActiveSatellitesNumber - 1;
+			mat_type* subset_satpos = create_matrix(subset_count, 3, "fdeSatpos");
+			mat_type* subset_obs = create_matrix(1, subset_count, "fdeObs");
+			mat_type* subset_az = create_matrix(1, subset_count, "fdeAz");
+			mat_type* subset_el = create_matrix(1, subset_count, "fdeEl");
+			unsigned char subset_ura[GPS_TRCK_IP_COUNT];
+			if((subset_satpos == NULL) || (subset_obs == NULL) ||
+			   (subset_az == NULL) || (subset_el == NULL))
+			{
+				free_matrix(subset_satpos);
+				free_matrix(subset_obs);
+				free_matrix(subset_az);
+				free_matrix(subset_el);
+				GPS_LOG(DEBUG_ERROR, GPS_LOG_CAT_MEM, "FDE subset allocation failed\r\n");
+				break;
+			}
+			initialize_all(0.0, subset_satpos);
+			initialize_all(0.0, subset_obs);
+			initialize_all(0.0, subset_az);
+			initialize_all(0.0, subset_el);
+
+			int subset_row = 0;
+			for(int original_row = 0 ; original_row < ActiveSatellitesNumber ; original_row++)
+			{
+				if(original_row == excluded_row)
+				{
+					continue;
+				}
+				for(int component = 0 ; component < 3 ; component++)
+				{
+					subset_satpos->matrix[subset_row][component] =
+						satpos->matrix[original_row][component];
+				}
+				subset_obs->matrix[0][subset_row] = obs->matrix[0][original_row];
+				subset_ura[subset_row] = ura_indices[original_row];
+				subset_row++;
+			}
+
+			double full_seed[4] = {
+				full_solution.ecef_m[0], full_solution.ecef_m[1],
+				full_solution.ecef_m[2], full_solution.clock_bias_m
+			};
+			gps_nav_solution_t candidate;
+			bool candidate_ok = least_squares_candidate(subset_satpos, subset_obs, subset_count,
+			                                                  subset_az, subset_el, subset_ura,
+			                                                  ionoutc, gps_tow,
+			                                                  full_seed, true, &candidate);
+			free_matrix(subset_satpos);
+			free_matrix(subset_obs);
+			free_matrix(subset_az);
+			free_matrix(subset_el);
+			if(!candidate_ok)
+			{
+				continue;
+			}
+
+			candidate.available_satellite_count = (unsigned int)ActiveSatellitesNumber;
+			candidate.used_row_mask = full_row_mask & ~(1u << (unsigned int)excluded_row);
+			candidate.excluded_row_mask = 1u << (unsigned int)excluded_row;
+			candidate.fde_applied = true;
+			double mapped_residuals[GPS_TRCK_IP_COUNT] = {0.0};
+			double mapped_elevations[GPS_TRCK_IP_COUNT] = {0.0};
+			double mapped_sigmas[GPS_TRCK_IP_COUNT] = {0.0};
+			subset_row = 0;
+			for(int original_row = 0 ; original_row < ActiveSatellitesNumber ; original_row++)
+			{
+				if(original_row != excluded_row)
+				{
+					mapped_residuals[original_row] = candidate.postfit_residual_m[subset_row];
+					mapped_elevations[original_row] = candidate.elevation_deg[subset_row];
+					mapped_sigmas[original_row] = candidate.measurement_sigma_m[subset_row];
+					subset_row++;
+				}
+			}
+			memcpy(candidate.postfit_residual_m, mapped_residuals, sizeof(mapped_residuals));
+			memcpy(candidate.elevation_deg, mapped_elevations, sizeof(mapped_elevations));
+			memcpy(candidate.measurement_sigma_m, mapped_sigmas, sizeof(mapped_sigmas));
+
+			bool choose_candidate = false;
+			if(candidate.chi_square_pass)
+			{
+				choose_candidate = !passing_candidate_found ||
+				                   (candidate.chi_square_ratio < selected_solution.chi_square_ratio);
+			}
+			else if(!passing_candidate_found &&
+			        (candidate.chi_square_ratio < selected_solution.chi_square_ratio))
+			{
+				/* Preserve the best statistical improvement as an explicitly invalid
+				 * best-effort navigation result. */
+				choose_candidate = true;
+			}
+			if(choose_candidate)
+			{
+				selected_solution = candidate;
+			}
+			if(candidate.chi_square_pass)
+			{
+				passing_candidate_found = true;
+			}
+		}
+	}
+
+	selected_solution.available_satellite_count = (unsigned int)ActiveSatellitesNumber;
+	selected_solution.full_chi_square_ratio = full_solution.chi_square_ratio;
+	selected_solution.full_postfit_position_correction_m =
+		full_solution.postfit_position_correction_m;
+	selected_solution.full_postfit_clock_correction_m =
+		full_solution.postfit_clock_correction_m;
+	memcpy(selected_solution.full_postfit_residual_m,
+	       full_solution.postfit_residual_m,
+	       sizeof(selected_solution.full_postfit_residual_m));
+	memcpy(selected_solution.full_elevation_deg,
+	       full_solution.elevation_deg,
+	       sizeof(selected_solution.full_elevation_deg));
+	memcpy(selected_solution.full_measurement_sigma_m,
+	       full_solution.measurement_sigma_m,
+	       sizeof(selected_solution.full_measurement_sigma_m));
+	selected_solution.full_solution_cycles = full_solution.candidate_cycles;
+	selected_solution.fde_trial_count = fde_trial_count;
+	selected_solution.fde_cycles = (fde_start_cycles != 0u)
+	                             ? (gps_read_cycle_counter() - fde_start_cycles) : 0u;
+	selected_solution.total_wls_cycles = gps_read_cycle_counter() - total_wls_start_cycles;
+	selected_solution.solution_valid = selected_solution.candidate_valid &&
+	                                   selected_solution.residual_test_valid &&
+	                                   selected_solution.chi_square_pass;
+	if(selected_solution.solution_valid)
+	{
+		gps_wls_last_solution[0] = selected_solution.ecef_m[0];
+		gps_wls_last_solution[1] = selected_solution.ecef_m[1];
+		gps_wls_last_solution[2] = selected_solution.ecef_m[2];
+		gps_wls_last_solution[3] = selected_solution.clock_bias_m;
+		gps_wls_last_solution_valid = true;
+	}
+	*solution_out = selected_solution;
+
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
+	        "GDOP=%.10lf\r\n", solution_out->gdop);
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_NAV,
+	        "position lat=%.20lf lon=%.20lf height=%.3lf\r\n",
+	        solution_out->latitude_deg, solution_out->longitude_deg,
+	        solution_out->height_m);
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
+	        "NAVSOL valid=%u integrity=%u fde=%u sats=%u/%u excluded=0x%03x "
+	        "residual_rms=%.3lf max_residual=%.3lf weighted_rms=%.3lf "
+	        "chi2=%.3lf dof=%u threshold=%.3lf ratio=%.3lf quality=%.1lf "
+	        "hacc=%.3lf vacc=%.3lf\r\n",
+	        solution_out->solution_valid ? 1u : 0u,
+	        solution_out->chi_square_pass ? 1u : 0u,
+	        solution_out->fde_applied ? 1u : 0u,
+	        solution_out->used_satellite_count,
+	        solution_out->available_satellite_count,
+	        solution_out->excluded_row_mask,
+	        solution_out->residual_rms_m,
+	        solution_out->max_abs_residual_m,
+	        solution_out->weighted_residual_rms_m,
+	        solution_out->weighted_residual_sum,
+	        solution_out->degrees_of_freedom,
+	        solution_out->chi_square_threshold,
+	        solution_out->chi_square_ratio,
+	        solution_out->quality_score,
+	        solution_out->hacc_m,
+	        solution_out->vacc_m);
+	for(int row = 0 ; row < ActiveSatellitesNumber ; row++)
+	{
+		if((solution_out->used_row_mask & (1u << (unsigned int)row)) != 0u)
+		{
+			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+			        "postfit row=%d residual=%.3lf m\r\n",
+			        row, solution_out->postfit_residual_m[row]);
+		}
+	}
+	return true;
+}
+
 void rotateSatelliteSTD(double const traveltime, const mat_type* X_sat, mat_type* X_sat_rot) // x_sat -> vector type
 {
 	double omegatau    = OMEGA_EARTH * traveltime;	

soc/software/gps_system/src/gps_interrupt_handlers.c

@@ -2,6 +2,7 @@
 #include "gps_interrupt_handlers.h"
 #include "Navigation.h"
 #include "gps_pps.h"
+#include "gps_time_anchor.h"
 #include <bsp/irq.h>
 #include <bsp/mmio.h>
 #include <bsp/plic.h>
@@ -68,6 +69,8 @@ static volatile unsigned int gps_plic_drain_limit_count = 0;
 static volatile unsigned int gps_plic_repeat_count = 0;
 static volatile bool gps_acq_schedule_pending = true;
 static volatile bool gps_timer_maintenance_pending = true;
+static volatile bool gps_nav_solve_period_pending = false;
+static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
 static unsigned int gps_acq_round_robin_prn = 0;
 
 #define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
@@ -98,45 +101,6 @@ static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phas
 	return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
 }
 
-static double gps_normalize_tow(double tow_sec, volatile int* week)
-{
-	while(tow_sec >= SECONDS_IN_WEEK)
-	{
-		tow_sec -= SECONDS_IN_WEEK;
-		if(week != NULL)
-		{
-			(*week)++;
-		}
-	}
-	while(tow_sec < 0.0)
-	{
-		tow_sec += SECONDS_IN_WEEK;
-		if(week != NULL)
-		{
-			(*week)--;
-		}
-	}
-	return tow_sec;
-}
-
-static bool gps_advance_tow_from_nav_timer(volatile gps_ephemeris_t* eph,
-	                                      uint32_t nav_solve_timer,
-	                                      uint32_t event_count,
-	                                      double* delta_sec_out)
-{
-	if((eph == NULL) || (nav_solve_timer == 0u) || (event_count == 0u))
-	{
-		return false;
-	}
-	double delta_sec = (double)event_count * (double)nav_solve_timer * 0.020;
-	eph->tow.sec = gps_normalize_tow(eph->tow.sec + delta_sec, &eph->tow.week);
-	if(delta_sec_out != NULL)
-	{
-		*delta_sec_out = delta_sec;
-	}
-	return true;
-}
-
 static uint64_t gps_systick_interval(void)
 {
 	uint64_t interval = BSP_CLINT_TIMEBASE_HZ / GPS_SYSTICK_HZ;
@@ -176,6 +140,7 @@ static bool gps_event_push_isr(const gps_isr_event_t* event)
 	if(next == gps_event_tail)
 	{
 		gps_event_overflow_count++;
+		gps_runtime_diagnostics.event_overflow_total++;
 		return false;
 	}
 
@@ -243,6 +208,52 @@ static bool gps_take_timer_maintenance_pending(void)
 	return pending;
 }
 
+bool gps_take_nav_solve_period_ready(void)
+{
+	bool tracking_shadow_pending = false;
+	bool acquisition_shadow_pending = false;
+	uint64_t irq_state = bsp_irq_save();
+	for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
+	{
+		if(trk_shadow[i].drop_pending || trk_shadow[i].nav_bits_pending ||
+		   trk_shadow[i].nav_solve_pending || trk_shadow[i].misc_pending)
+		{
+			tracking_shadow_pending = true;
+			break;
+		}
+	}
+	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
+	{
+		if(acq_shadow_pending[i])
+		{
+			acquisition_shadow_pending = true;
+			break;
+		}
+	}
+	bool pipeline_idle = (gps_event_head == gps_event_tail) &&
+	                     !tracking_shadow_pending && !acquisition_shadow_pending;
+	bool ready = gps_nav_solve_period_pending && pipeline_idle;
+	if(ready)
+	{
+		/* One foreground attempt per hardware NAV period.  A failed WLS is not
+		 * retried by an unrelated NAV-bit, acquisition or SysTick wake-up. */
+		gps_nav_solve_period_pending = false;
+	}
+	bsp_irq_restore(irq_state);
+	return ready;
+}
+
+void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics)
+{
+	if(diagnostics == NULL)
+	{
+		return;
+	}
+	uint64_t irq_state = bsp_irq_save();
+	*diagnostics = gps_runtime_diagnostics;
+	bsp_irq_restore(irq_state);
+}
+
 static void gps_clear_tracking_slot(unsigned char trck_ip_no)
 {
 	if(trck_ip_no >= GPS_TRCK_IP_COUNT)
@@ -277,6 +288,10 @@ static void gps_runtime_state_init(void)
 	gps_plic_repeat_count = 0;
 	gps_acq_schedule_pending = true;
 	gps_timer_maintenance_pending = true;
+	gps_nav_solve_period_pending = false;
+	memset((void*)&gps_runtime_diagnostics, 0, sizeof(gps_runtime_diagnostics));
+	gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
+	gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
 	gps_acq_round_robin_prn = 0;
 
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
@@ -441,6 +456,7 @@ void gps_irq_handler(void)
 		{
 			bsp_plic_complete(irq_id);
 			gps_plic_repeat_count++;
+			gps_runtime_diagnostics.plic_repeat_total++;
 			return;
 		}
 		seen_mask |= irq_bit;
@@ -451,6 +467,7 @@ void gps_irq_handler(void)
 	}
 
 	gps_plic_drain_limit_count++;
+	gps_runtime_diagnostics.plic_drain_limit_total++;
 }
 
 static bool gps_tracking_prn_active(unsigned int prn_no)
@@ -483,6 +500,9 @@ static void gps_mark_satellite_tracking(unsigned int prn_no, unsigned char trck_
 		satellite_tracking_ip[prn_no] = trck_ip_no;
 		if((previous_state != GPS_SAT_TRACKING) || (previous_ip != trck_ip_no))
 		{
+			gps_runtime_diagnostics.satellite_connected_total++;
+			gps_runtime_diagnostics.last_connected_prn = prn_no;
+			gps_runtime_diagnostics.last_connected_tick = gps_get_systick();
 			GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
 			        "satellite connected: prn=%u trk=%u previous=%s previous_trk=%u\r\n",
 			        prn_no, trck_ip_no, gps_satellite_state_name(previous_state), previous_ip);
@@ -500,6 +520,9 @@ static void gps_mark_satellite_lost(unsigned int prn_no)
 		satellite_tracking_ip[prn_no] = GPS_PRN_INVALID;
 		if(previous_state != GPS_SAT_LOST)
 		{
+			gps_runtime_diagnostics.satellite_lost_total++;
+			gps_runtime_diagnostics.last_lost_prn = prn_no;
+			gps_runtime_diagnostics.last_lost_tick = gps_get_systick();
 			GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
 			        "satellite lost: prn=%u previous=%s previous_trk=%u\r\n",
 			        prn_no, gps_satellite_state_name(previous_state), previous_ip);
@@ -623,7 +646,8 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
 	event.status = status_reg;
 	event.control = trck_ip->CONTROL_REG;
 	event.prn = trck_ip->PRN_NUMBER;
-	/* Hardware updates ABS_SAMPLE when the nav-solve interrupt is raised. */
+	/* Hardware latches ABS_SAMPLE and REM_CODE_PHASE together for both a valid
+	 * subframe and a nav-solve event. */
 	event.abs_sample_raw = trck_ip->ABS_SAMPLE;
 	event.rem_code_phase = trck_ip->REM_CODE_PHASE_REG;
 	event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
@@ -889,15 +913,39 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
 
 	gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample;
 	gps_frame_parsed->gps_ephems.SVID = (unsigned char)event->prn;
-	/* Enabling an interrupt is not a timing event. HOW remains the TOW owner
-	 * until this channel's first nav-solve event is actually consumed. */
-	bool update_tow = node->gps_tow_source == GPS_TOW_SOURCE_HOW;
+	unsigned int how_tow_sec = 0u;
 	unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe,
 	                                           (gps_frame_t*)gps_frame_parsed,
-	                                           update_tow);
+	                                           &how_tow_sec);
 	if((subframe_id >= 1u) && (subframe_id <= 5u))
 	{
 		node->gps_subframe_mask |= (1u << subframe_id);
+		int fallback_week = gps_frame_parsed->gps_ephems.tow.week;
+		if(subframe_id == 1u)
+		{
+			/* WN is ten bits in LNAV.  Keeping it in the same modulo domain as
+			 * the ephemeris is sufficient for satellite-time differences. */
+			fallback_week = (int)((gps_subframe[2] >> 20) & 0x3ffu);
+		}
+		/* A subframe-1 WN is authoritative in its native 10-bit domain.  On
+		 * other subframes, resolve the HOW seconds rollover from the previous
+		 * anchor.  This also upgrades a pre-SF1 temporary week immediately. */
+		int anchor_week = (subframe_id == 1u) ? fallback_week :
+			gps_time_resolve_how_week(node->gps_how_anchor_valid,
+			                          node->gps_how_tow.week,
+			                          node->gps_how_tow.sec,
+			                          fallback_week,
+			                          (double)how_tow_sec);
+		node->gps_how_tow.week = anchor_week;
+		node->gps_how_tow.sec = (double)how_tow_sec;
+		node->gps_how_abs_sample = event->abs_sample_raw;
+		node->gps_how_corrected_abs_sample = event->corrected_abs_sample;
+		node->gps_how_tick = event->tick;
+		node->gps_how_anchor_valid = true;
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
+		        trck_ip_no, event->prn, anchor_week, (double)how_tow_sec,
+		        event->abs_sample_raw, event->corrected_abs_sample);
 	}
 	else
 	{
@@ -934,6 +982,8 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 				}
 				if(nav_solve_event_count > event->nav_solve_event_count)
 				{
+					gps_runtime_diagnostics.nav_epoch_coalesced_total +=
+						nav_solve_event_count - event->nav_solve_event_count;
 					GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
 					        "recovered %u coalesced nav-solve epochs: trk=%u prn=%u\r\n",
 					        nav_solve_event_count, trck_ip_no, event->prn);
@@ -947,26 +997,39 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 			node->gps_last_pps_capture_sequence += nav_solve_event_count;
 		}
 
-		double tow_delta_sec = 0.0;
-		if(!gps_advance_tow_from_nav_timer(&gps_frame_parsed->gps_ephems,
-		                                  event->nav_solve_timer,
-		                                  nav_solve_event_count,
-		                                  &tow_delta_sec))
+		if(!node->gps_how_anchor_valid)
 		{
 			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
-			        "nav solve has invalid timer: trk=%u prn=%u timer=%u\r\n",
-			        trck_ip_no, event->prn, event->nav_solve_timer);
+			        "nav solve rejected without HOW/sample anchor: trk=%u prn=%u\r\n",
+			        trck_ip_no, event->prn);
 			return;
 		}
-		if(node->gps_tow_source == GPS_TOW_SOURCE_HOW)
+		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))
 		{
-			node->gps_tow_source = GPS_TOW_SOURCE_NAV_SOLVE;
-			GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
-			        "TOW ownership transferred to nav solve: trk=%u prn=%u tow=%.3f\r\n",
-			        trck_ip_no, event->prn, gps_frame_parsed->gps_ephems.tow.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",
+			        trck_ip_no, event->prn, node->gps_how_tow.sec,
+			        node->gps_how_abs_sample, event->abs_sample_raw);
+			return;
 		}
+		gps_frame_parsed->gps_ephems.tow.week = tow_week;
+		gps_frame_parsed->gps_ephems.tow.sec = tow_sec;
 		gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample;
 		node->gps_nav_solve_sequence += nav_solve_event_count;
+		gps_nav_solve_period_pending = true;
 		node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
 		node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
 		if(event->pps_capture_valid)

soc/software/gps_system/src/gps_ip.c

@@ -1,6 +1,8 @@
 #include <stdbool.h>
 #include <stdint.h>
 #include <stdlib.h>
+#include <math.h>
+#include <string.h>
 #include "NavEquations.h"
 #include "Navigation.h"
 #include "gps_ip_defines.h"
@@ -131,6 +133,94 @@ static bool gps_snapshot_has_svid(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COU
 	return false;
 }
 
+static uint32_t gps_nav_tow_mismatch_total;
+static uint32_t gps_nav_tow_reject_total;
+
+static double gps_snapshot_tow_delta_sec(const gpstime_t* first, const gpstime_t* second)
+{
+	double delta = ((double)(first->week - second->week) * 604800.0) +
+	               (first->sec - second->sec);
+	if(delta > 302400.0)
+	{
+		delta -= 604800.0;
+	}
+	else if(delta < -302400.0)
+	{
+		delta += 604800.0;
+	}
+	return delta;
+}
+
+static bool gps_validate_snapshot_tow(const gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
+	                                  unsigned int satellite_count)
+{
+	if((snapshot == NULL) || (satellite_count < MIN_NEEDED_SATELLITE))
+	{
+		return false;
+	}
+
+	unsigned int reference_row = 0u;
+	unsigned int reference_cluster_count = 0u;
+	for(unsigned int candidate = 0u ; candidate < satellite_count ; candidate++)
+	{
+		unsigned int cluster_count = 0u;
+		for(unsigned int row = 0u ; row < satellite_count ; row++)
+		{
+			double delta = gps_snapshot_tow_delta_sec(&snapshot[row].tow,
+			                                             &snapshot[candidate].tow);
+			if(fabs(delta) <= GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC)
+			{
+				cluster_count++;
+			}
+		}
+		if(cluster_count > reference_cluster_count)
+		{
+			reference_cluster_count = cluster_count;
+			reference_row = candidate;
+		}
+	}
+
+	if(reference_cluster_count <= (satellite_count / 2u))
+	{
+		gps_nav_tow_reject_total++;
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+		        "NAV epoch rejected: no majority TOW cluster sats=%u best_cluster=%u rejects=%u\r\n",
+		        satellite_count, reference_cluster_count, gps_nav_tow_reject_total);
+		return false;
+	}
+
+	unsigned int mismatch_count = 0u;
+	double maximum_mismatch_sec = 0.0;
+	unsigned int maximum_mismatch_row = reference_row;
+	for(unsigned int row = 0u ; row < satellite_count ; row++)
+	{
+		double delta = gps_snapshot_tow_delta_sec(&snapshot[row].tow,
+		                                             &snapshot[reference_row].tow);
+		if(fabs(delta) <= GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC)
+		{
+			continue;
+		}
+		mismatch_count++;
+		if(!isfinite(delta) || (fabs(delta) > fabs(maximum_mismatch_sec)))
+		{
+			maximum_mismatch_sec = delta;
+			maximum_mismatch_row = row;
+		}
+	}
+	if(mismatch_count != 0u)
+	{
+		gps_nav_tow_mismatch_total += mismatch_count;
+		gps_nav_tow_reject_total++;
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+		        "NAV epoch rejected: TOW mismatch count=%u SVID=%u delta=%.6lf s reference_svid=%u mismatches=%u rejects=%u\r\n",
+		        mismatch_count, snapshot[maximum_mismatch_row].SVID,
+		        maximum_mismatch_sec, snapshot[reference_row].SVID,
+		        gps_nav_tow_mismatch_total, gps_nav_tow_reject_total);
+		return false;
+	}
+	return true;
+}
+
 static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
 {
 	if((bits == 0u) || (bits >= 32u))
@@ -418,15 +508,13 @@ static void gps_stage_ephemeris_subframe(const unsigned int* subframe,
 }
 
 unsigned char parse_subframe(int unsigned* subframe, gps_frame_t* gps_frame,
-	                         bool update_tow)
+	                         unsigned int* how_tow_sec_out)
 {				
 	unsigned char subframe_id = (unsigned char)((subframe[1] >> 8) & 0x07);
 	unsigned int how_tow = (subframe[1] >> 13) * 6u;
-	if(update_tow)
+	if(how_tow_sec_out != NULL)
 	{
-		gps_frame->gps_ephems.tow.sec = (double)how_tow;
-		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "tow.sec initialized from HOW as %lf\n",
-		        gps_frame->gps_ephems.tow.sec);
+		*how_tow_sec_out = how_tow;
 	}
 	if((subframe_id >= 1u) && (subframe_id <= 3u))
 	{
@@ -708,9 +796,11 @@ static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GP
 static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 	                                   const uint64_t pps_counters[GPS_TRCK_IP_COUNT],
 	                                   const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT],
-	                                   unsigned int satellite_count)
+	                                   unsigned int satellite_count,
+	                                   gps_nav_solution_t* solution_out)
 {
-	if((ephemeris == NULL) || (satellite_count < MIN_NEEDED_SATELLITE))
+	if((ephemeris == NULL) || (solution_out == NULL) ||
+	   (satellite_count < MIN_NEEDED_SATELLITE))
 	{
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "nav solve skipped, usable satellites=%u\r\n", satellite_count);
 		return false;
@@ -731,7 +821,9 @@ 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;
-	double solution[4] = {0.0, 0.0, 0.0, 0.0};
+	gps_nav_solution_t solution;
+	memset(&solution, 0, sizeof(solution));
+	memset(solution_out, 0, sizeof(*solution_out));
 	GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "Klobuchar correction %s for this solve\r\n",
 	        (ionoutc != NULL) ? "available" : "waiting for subframe 4 page 18");
 
@@ -802,14 +894,28 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 
 	ok = leastSquaresSTD(satellitePositions, localPseudoRanges, satellite_count, az, el,
 	                    ura_indices,
-	                    ionoutc, ephemeris[0].tow.sec, solution);
-	if(ok && (pps_capture_sequences[minimum_index] != 0u))
+	                    ionoutc, ephemeris[0].tow.sec, &solution);
+	if(ok && solution.fde_applied)
+	{
+		for(unsigned int row = 0 ; row < satellite_count ; row++)
+		{
+			if((solution.excluded_row_mask & (1u << row)) != 0u)
+			{
+				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+				        "FDE excluded row=%u SVID=%u\r\n", row, ephemeris[row].SVID);
+			}
+		}
+	}
+	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))
 	{
 		/* 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[3] / SPEED_OF_LIGHT);
+		                      (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))
@@ -817,6 +923,18 @@ static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
 			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "PPS anchor skipped: mailbox busy or invalid time\r\n");
 		}
 	}
+	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);
+	}
+	else if(ok && !solution.solution_valid)
+	{
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+		        "PPS/last_solution protected from invalid best-effort NAVSOL quality=%.1lf ratio=%.3lf\r\n",
+		        solution.quality_score, solution.chi_square_ratio);
+	}
 
 cleanup:
 	if(preciseTxTimes != NULL) free_matrix(preciseTxTimes);
@@ -828,9 +946,118 @@ cleanup:
 	free(rho0);
 	free(psTVLtime);
 	free(clockCorr);
+	*solution_out = solution;
 	return ok;
 }
 
+#ifdef GPS_NAV_JUMP_DIAGNOSTICS
+static void gps_log_nav_jump_diagnostic(const gps_nav_solution_t* solution,
+	                                    const gps_ephemeris_t* ephemeris,
+	                                    unsigned int satellite_count)
+{
+	static bool previous_valid = false;
+	static double previous_ecef_m[3];
+	static double previous_height_m = 0.0;
+	static uint32_t previous_excluded_mask = 0u;
+	static uint32_t previous_satellite_mask = 0u;
+	static uint64_t last_jump_log_tick = 0u;
+	if((solution == NULL) || (ephemeris == NULL))
+	{
+		return;
+	}
+
+	double step_3d_m = 0.0;
+	double vertical_step_m = 0.0;
+	if(previous_valid)
+	{
+		double delta[3] = {
+			solution->ecef_m[0] - previous_ecef_m[0],
+			solution->ecef_m[1] - previous_ecef_m[1],
+			solution->ecef_m[2] - previous_ecef_m[2]
+		};
+		step_3d_m = normVect(delta);
+		vertical_step_m = solution->height_m - previous_height_m;
+	}
+	bool exclusion_changed = previous_valid &&
+	                         (solution->excluded_row_mask != previous_excluded_mask);
+	uint32_t satellite_mask = 0u;
+	for(unsigned int row = 0 ; row < satellite_count ; row++)
+	{
+		if(ephemeris[row].SVID < 32u)
+		{
+			satellite_mask |= 1u << ephemeris[row].SVID;
+		}
+	}
+	bool satellite_set_changed = previous_valid && (satellite_mask != previous_satellite_mask);
+	bool position_jumped = previous_valid && (step_3d_m >= 20.0);
+	uint64_t now = gps_get_systick();
+	bool rate_limit_pass = (now - last_jump_log_tick) >= GPS_SYSTICK_HZ;
+	if(exclusion_changed || satellite_set_changed || (position_jumped && rate_limit_pass))
+	{
+		double minimum_abs_sample = ephemeris[0].abs_sample;
+		for(unsigned int row = 1u ; row < satellite_count ; row++)
+		{
+			if(ephemeris[row].abs_sample < minimum_abs_sample)
+			{
+				minimum_abs_sample = ephemeris[row].abs_sample;
+			}
+		}
+		gps_runtime_diagnostics_t runtime_diagnostics;
+		gps_get_runtime_diagnostics(&runtime_diagnostics);
+		uint64_t loss_age_ticks = (runtime_diagnostics.satellite_lost_total != 0u)
+		                        ? (now - runtime_diagnostics.last_lost_tick) : 0u;
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "NAVDIAG step3d=%.2lf vertical=%.2lf valid=%u fde=%u "
+		        "excluded=0x%03x previous_excluded=0x%03x svmask=0x%08x previous_svmask=0x%08x "
+		        "full_ratio=%.3lf selected_ratio=%.3lf math_corr=%.6lf full_math_corr=%.6lf "
+		        "math_clock_corr=%.6lf full_math_clock_corr=%.6lf "
+		        "isr_overflow=%u plic_repeat=%u drain_limit=%u coalesced=%u "
+		        "lost_total=%u last_lost_prn=%u loss_age_ticks=%lu\r\n",
+		        step_3d_m, vertical_step_m,
+		        solution->solution_valid ? 1u : 0u,
+		        solution->fde_applied ? 1u : 0u,
+		        solution->excluded_row_mask, previous_excluded_mask,
+		        satellite_mask, previous_satellite_mask,
+		        solution->full_chi_square_ratio, solution->chi_square_ratio,
+		        solution->postfit_position_correction_m,
+		        solution->full_postfit_position_correction_m,
+		        solution->postfit_clock_correction_m,
+		        solution->full_postfit_clock_correction_m,
+		        runtime_diagnostics.event_overflow_total,
+		        runtime_diagnostics.plic_repeat_total,
+		        runtime_diagnostics.plic_drain_limit_total,
+		        runtime_diagnostics.nav_epoch_coalesced_total,
+		        runtime_diagnostics.satellite_lost_total,
+		        runtime_diagnostics.last_lost_prn,
+		        (unsigned long)loss_age_ticks);
+		for(unsigned int row = 0 ; row < satellite_count ; row++)
+		{
+			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+			        "NAVSAT row=%u svid=%u abs_sample=%.6lf raw_delta_m=%.2lf "
+			        "full_residual=%.2lf elevation=%.1lf sigma=%.2lf used=%u excluded=%u\r\n",
+			        row, ephemeris[row].SVID,
+			        ephemeris[row].abs_sample,
+			        ((ephemeris[row].abs_sample - minimum_abs_sample) / SAMPLES_PER_CODE) *
+			            (SPEED_OF_LIGHT / 1000.0),
+			        solution->full_postfit_residual_m[row],
+			        solution->full_elevation_deg[row],
+			        solution->full_measurement_sigma_m[row],
+			        (solution->used_row_mask & (1u << row)) ? 1u : 0u,
+			        (solution->excluded_row_mask & (1u << row)) ? 1u : 0u);
+		}
+		last_jump_log_tick = now;
+	}
+	for(int component = 0 ; component < 3 ; component++)
+	{
+		previous_ecef_m[component] = solution->ecef_m[component];
+	}
+	previous_height_m = solution->height_m;
+	previous_excluded_mask = solution->excluded_row_mask;
+	previous_satellite_mask = satellite_mask;
+	previous_valid = true;
+}
+#endif
+
 #ifdef EKF_ENABLE
 void calculate_position(Ekf* g_ekf)
 #else
@@ -857,6 +1084,8 @@ void calculate_position()
 #ifdef EKF_ENABLE
 	(void)g_ekf;
 #endif
+	bool nav_period_ready = gps_take_nav_solve_period_ready();
+	uint64_t nav_period_start_cycles = nav_period_ready ? gps_read_cycle_counter() : 0u;
 
 	uint64_t irq_state = bsp_irq_save();
 
@@ -873,16 +1102,21 @@ void calculate_position()
 		/* The nav-bit snapshot is used only to qualify ephemeris and select the
 		 * channels. Position input must come from a coherent nav-solve epoch. */
 		gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
-		satellite_count = gps_collect_nav_snapshot_locked(snapshot, snapshot_ips, true, true);
-		coherent_nav_count = satellite_count;
-		run_nav = (satellite_count >= MIN_NEEDED_SATELLITE) &&
-		          (satellite_count >= ready_count);
-		initial_run = !gps_initial_position_valid;
-		if(run_nav)
+		if(nav_period_ready)
 		{
-			gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
-			                               snapshot_pps_counters, snapshot_pps_sequences,
-			                               satellite_count);
+			satellite_count = gps_collect_nav_snapshot_locked(snapshot, snapshot_ips, true, true);
+			coherent_nav_count = satellite_count;
+			bool tow_coherent = (satellite_count >= MIN_NEEDED_SATELLITE) &&
+			                    (satellite_count >= ready_count) &&
+			                    gps_validate_snapshot_tow(snapshot, satellite_count);
+			run_nav = tow_coherent;
+			initial_run = !gps_initial_position_valid;
+			if(run_nav)
+			{
+				gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
+				                               snapshot_pps_counters, snapshot_pps_sequences,
+				                               satellite_count);
+			}
 		}
 	}
 
@@ -914,14 +1148,67 @@ void calculate_position()
 			GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "nav solve period\r\n");
 		}
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV, "navigation software running with %u satellites\r\n", satellite_count);
+		gps_nav_solution_t nav_solution;
+		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_sequences, satellite_count);
+		                                         snapshot_pps_sequences, satellite_count,
+		                                         &nav_solution);
+		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
 			uint64_t consume_irq_state = bsp_irq_save();
 			gps_consume_nav_sequences_locked(snapshot_ips, snapshot_sequences, satellite_count);
 			bsp_irq_restore(consume_irq_state);
-			if(initial_run)
+		}
+		uint64_t foreground_cycles = gps_read_cycle_counter() - nav_period_start_cycles;
+		uint64_t period_budget_cycles = (GPS_PROFILE_CPU_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u;
+		bool period_overrun = foreground_cycles >= period_budget_cycles;
+		gps_runtime_diagnostics_t runtime_diagnostics;
+		gps_get_runtime_diagnostics(&runtime_diagnostics);
+		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
+		        "NAVPERF period_ms=%u cpu_hz=%lu foreground_cycles=%lu "
+		        "nav_cycles=%lu wls_cycles=%lu full_cycles=%lu "
+		        "fde_cycles=%lu fde_trials=%u selected_iterations=%u sats=%u/%u "
+		        "fde=%u valid=%u budget_cycles=%lu overrun=%u math_corr=%.6lf "
+		        "math_clock_corr=%.6lf "
+		        "isr_overflow=%u plic_repeat=%u drain_limit=%u coalesced=%u lost=%u "
+		        "tow_mismatch=%u tow_rejected=%u\r\n",
+		        GPS_NAV_SOLVE_PERIOD_MS,
+		        (unsigned long)GPS_PROFILE_CPU_HZ,
+		        (unsigned long)foreground_cycles,
+		        (unsigned long)nav_software_cycles,
+		        (unsigned long)nav_solution.total_wls_cycles,
+		        (unsigned long)nav_solution.full_solution_cycles,
+		        (unsigned long)nav_solution.fde_cycles,
+		        nav_solution.fde_trial_count,
+		        nav_solution.iteration_count,
+		        nav_solution.used_satellite_count,
+		        nav_solution.available_satellite_count,
+		        nav_solution.fde_applied ? 1u : 0u,
+		        nav_solution.solution_valid ? 1u : 0u,
+		        (unsigned long)period_budget_cycles,
+		        period_overrun ? 1u : 0u,
+		        nav_solution.postfit_position_correction_m,
+		        nav_solution.postfit_clock_correction_m,
+		        runtime_diagnostics.event_overflow_total,
+		        runtime_diagnostics.plic_repeat_total,
+		        runtime_diagnostics.plic_drain_limit_total,
+		        runtime_diagnostics.nav_epoch_coalesced_total,
+		        runtime_diagnostics.satellite_lost_total,
+		        gps_nav_tow_mismatch_total,
+		        gps_nav_tow_reject_total);
+		(void)foreground_cycles;
+		(void)nav_software_cycles;
+		(void)period_budget_cycles;
+		(void)period_overrun;
+		(void)runtime_diagnostics;
+#ifdef GPS_NAV_JUMP_DIAGNOSTICS
+		gps_log_nav_jump_diagnostic(&nav_solution, snapshot, satellite_count);
+#endif
+		if(nav_ok)
+		{
+			if(initial_run && nav_solution.solution_valid)
 			{
 				gps_initial_position_valid = true;
 			}
@@ -929,7 +1216,8 @@ void calculate_position()
 			 * perturb collection of the input epoch. */
 			if(GPS_DEBUG_ENABLED(DEBUG_CHANNELS))
 			{
-				GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK, "tracked=%u prns=[", busy_count);
+				GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK,
+				        "tracked satellites=%u prns=[", busy_count);
 				for(unsigned int i = 0 ; i < busy_count ; i++)
 				{
 					PRINT_DEBUG(DEBUG_CHANNELS, "%s%u",
@@ -1040,10 +1328,18 @@ void gps_startup_initialization(unsigned int nsp_tick)
 {
 
 	(void)bsp_irq_save();
+	GPS_LOG(DEBUG_EVENTS, GPS_LOG_CAT_NAV,
+	        "NAV schedule timer_ticks=%u tick_ms=%u period_ms=%u profile_cpu_hz=%lu fde_enabled=%u\r\n",
+	        nsp_tick, GPS_NAV_SOLVE_TIMER_TICK_MS,
+	        nsp_tick * GPS_NAV_SOLVE_TIMER_TICK_MS,
+	        (unsigned long)GPS_PROFILE_CPU_HZ,
+	        GPS_NAV_ENABLE_FDE);
 	gps_init_acq_tables();
 	gps_pps_init(nsp_tick);
 	gps_ionoutc_valid = false;
 	gps_last_ionoutc = (ionoutc_t){0};
+	gps_nav_tow_mismatch_total = 0u;
+	gps_nav_tow_reject_total = 0u;
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
 		acq_ips[i]->CONTROL_REG = (1u << GPS_ACQ_INTPRT_CLR);

soc/software/gps_system/src/gps_time_anchor.c

@@ -0,0 +1,88 @@
+#include "gps_time_anchor.h"
+
+#include <math.h>
+#include <stddef.h>
+
+static double gps_time_normalize_week(double tow_sec, int* week)
+{
+	while(tow_sec >= GPS_TIME_WEEK_SECONDS)
+	{
+		tow_sec -= GPS_TIME_WEEK_SECONDS;
+		(*week)++;
+	}
+	while(tow_sec < 0.0)
+	{
+		tow_sec += GPS_TIME_WEEK_SECONDS;
+		(*week)--;
+	}
+	return tow_sec;
+}
+
+int gps_time_resolve_how_week(bool previous_valid,
+                              int previous_week,
+                              double previous_tow_sec,
+                              int fallback_week,
+                              double how_tow_sec)
+{
+	if(!previous_valid || !isfinite(previous_tow_sec) || !isfinite(how_tow_sec))
+	{
+		return fallback_week;
+	}
+
+	int week = previous_week;
+	double delta_sec = how_tow_sec - previous_tow_sec;
+	if(delta_sec < -GPS_TIME_HALF_WEEK_SECONDS)
+	{
+		week++;
+	}
+	else if(delta_sec > GPS_TIME_HALF_WEEK_SECONDS)
+	{
+		week--;
+	}
+	return week;
+}
+
+bool gps_time_from_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,
+                                 int* event_week_out,
+                                 double* event_tow_sec_out,
+                                 double* elapsed_sec_out)
+{
+	if((event_week_out == NULL) || (event_tow_sec_out == NULL) ||
+	   !(samples_per_second > 0.0) || !(max_age_sec > 0.0) ||
+	   !isfinite(anchor_tow_sec) || !isfinite(anchor_corrected_sample) ||
+	   !isfinite(event_corrected_sample))
+	{
+		return false;
+	}
+
+	/* The integer part is unwrapped modulo 2^32.  Preserve the code-phase
+	 * fractions from both simultaneous ABS_SAMPLE/REM_CODE_PHASE latches. */
+	int32_t raw_delta = (int32_t)(event_raw_sample - anchor_raw_sample);
+	double anchor_fraction = anchor_corrected_sample - (double)anchor_raw_sample;
+	double event_fraction = event_corrected_sample - (double)event_raw_sample;
+	double sample_delta = (double)raw_delta + event_fraction - anchor_fraction;
+	double elapsed_sec = sample_delta / samples_per_second;
+	if(!isfinite(elapsed_sec) || (elapsed_sec < -0.001) ||
+	   (elapsed_sec > max_age_sec))
+	{
+		return false;
+	}
+
+	int event_week = anchor_week;
+	double event_tow_sec = gps_time_normalize_week(anchor_tow_sec + elapsed_sec,
+	                                                &event_week);
+	*event_week_out = event_week;
+	*event_tow_sec_out = event_tow_sec;
+	if(elapsed_sec_out != NULL)
+	{
+		*elapsed_sec_out = elapsed_sec;
+	}
+	return true;
+}

soc/software/gps_system/src/linked_list.c

@@ -60,7 +60,12 @@ bool insert_element(volatile gps_ephem_ll_t** node,
 	new_node->tracking_ip_no = trck_ip_no;
 	new_node->next = NULL;
 	new_node->gps_subframe_mask = 0u;
-	new_node->gps_tow_source = GPS_TOW_SOURCE_HOW;
+	new_node->gps_how_tow.week = 0;
+	new_node->gps_how_tow.sec = 0.0;
+	new_node->gps_how_abs_sample = 0u;
+	new_node->gps_how_corrected_abs_sample = 0.0;
+	new_node->gps_how_tick = 0u;
+	new_node->gps_how_anchor_valid = false;
 	new_node->gps_nav_solve_sequence = 0u;
 	new_node->gps_nav_solve_consumed_sequence = 0u;
 	new_node->gps_last_nav_solve_abs_sample = 0u;

soc/software/gps_system/src/matrixLib.c

@@ -491,12 +491,9 @@ mat_type* calculate_matrix_inverse(mat_type* matrix)
     // Create a new matrix to store the inverse
     mat_type* inverse = create_matrix(n, n, "inverse_matrix");
 
-    // Create a temporary matrix to store intermediate results
-    mat_type* temp = create_matrix(n, n, "temp_matrix");
-    if((inverse == NULL) || (temp == NULL))
+    if(inverse == NULL)
     {
         free_matrix(inverse);
-        free_matrix(temp);
         return NULL;
     }
 
@@ -524,7 +521,6 @@ mat_type* calculate_matrix_inverse(mat_type* matrix)
                     "singular/ill-conditioned matrix, pivot=%e tolerance=%e\r\n",
                     maxVal, pivot_tolerance);
             free_matrix(inverse);
-            free_matrix(temp);
             return NULL;
         }
 
@@ -566,15 +562,11 @@ mat_type* calculate_matrix_inverse(mat_type* matrix)
                 GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
                         "inverse produced non-finite value at row=%d col=%d\r\n", row, col);
                 free_matrix(inverse);
-                free_matrix(temp);
                 return NULL;
             }
         }
     }
 
-    // Free the temporary matrix
-    free_matrix(temp);
-
     return inverse;
 }