Make GPS navigation epochs interrupt-coherent

Wait for every navigation-ready tracking channel in a GPS epoch, remove unlocked channels without a timeout, and keep late joins for the next epoch. Reset PLIC ownership during warm initialization and extend Linux synthetic, integrity, and fake-MMIO regression coverage.
2026-07-23 19:20:06 +03:00
parent 7cc3eb2a54
commit 516016efb9
16 changed files with 1780 additions and 246 deletions
@@ -1,6 +1,7 @@
 #include "gps/linked_list.h"
 #include "gps/gps_interrupt_handlers.h"
 #include "gps/Navigation.h"
+#include "gps/gps_nav_epoch.h"
 #include "gps/gps_pps.h"
 #include "gps/gps_time_anchor.h"
 #include <bsp/irq.h>
@@ -71,6 +72,7 @@ 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 gps_nav_epoch_assembly_t gps_nav_epoch_assembly;
 static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
 static unsigned int gps_acq_round_robin_prn = 0;
 static unsigned int gps_external_irq_id = 0u;
@@ -104,6 +106,24 @@ 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 uint32_t gps_nav_ready_channel_mask(void)
+{
+	uint32_t ready_mask = 0u;
+	for(unsigned int i = 0u ; i < GPS_TRCK_IP_COUNT ; i++)
+	{
+		uint32_t status = trck_ips[i]->STATUS_REG;
+		uint32_t control = trck_ips[i]->CONTROL_REG;
+		bool busy = (status & (1u << GPS_TRCK_BUSY_POS)) != 0u;
+		bool nav_irq_enabled =
+			(control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u;
+		if(tracking_allocated[i] && busy && nav_irq_enabled)
+		{
+			ready_mask |= 1u << i;
+		}
+	}
+	return ready_mask;
+}
+
 static uint64_t gps_systick_interval(void)
 {
 	uint64_t interval = BSP_CLINT_TIMEBASE_HZ / GPS_SYSTICK_HZ;
@@ -238,9 +258,10 @@ bool gps_take_nav_solve_period_ready(void)
 	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. */
+		/* The complete epoch is handed to the foreground exactly once. Events
+		 * arriving while WLS runs start the next assembly. */
 		gps_nav_solve_period_pending = false;
+		gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
 	}
 	bsp_irq_restore(irq_state);
 	return ready;
@@ -292,6 +313,7 @@ static void gps_runtime_state_init(void)
 	gps_acq_schedule_pending = true;
 	gps_timer_maintenance_pending = true;
 	gps_nav_solve_period_pending = false;
+	gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
 	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;
@@ -382,6 +404,10 @@ static bool gps_irq_to_tracking_channel(unsigned int irq_id, unsigned char* trck

 void gps_interrupt_init(void)
 {
+	/* A CPU-only GDB reset leaves the PLIC enable register untouched. Establish
+	 * ownership from a clean mask so an earlier IMU-enabled image cannot leave
+	 * PMOD IRQ 11 active in a GPS-only image. */
+	bsp_plic_disable_all();
 	gps_runtime_state_init();
 	bsp_set_mtvec(gps_irq_handler);
 	bsp_plic_set_threshold(0);
@@ -885,6 +911,12 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
 	unsigned char trck_ip_no = event->index;
 	unsigned int dropped_prn = event->prn;

+	if(gps_nav_epoch_assembly_remove_channel(
+		   &gps_nav_epoch_assembly, trck_ip_no))
+	{
+		gps_nav_solve_period_pending = true;
+	}
+
 	if(gps_prn_valid(dropped_prn))
 	{
 		(void)insert_acq_element(&acquisition_node, dropped_prn);
@@ -1059,7 +1091,8 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 		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_tow.week = tow_week;
+		node->gps_last_nav_solve_tow.sec = tow_sec;
 		node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
 		node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
 		node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
@@ -1069,6 +1102,40 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 			node->gps_last_pps_capture_sequence = event->pps_capture_sequence;
 		}
 		gps_mark_node_fresh(node, event->status, event->tick);
+
+		uint64_t epoch_id;
+		if(!gps_nav_epoch_id(tow_week, tow_sec,
+		                     GPS_NAV_SOLVE_PERIOD_MS, &epoch_id))
+		{
+			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+			        "nav solve has invalid epoch id: trk=%u week=%d tow=%.6f\r\n",
+			        trck_ip_no, tow_week, tow_sec);
+			return;
+		}
+		gps_nav_epoch_event_result_t assembly_result =
+			gps_nav_epoch_assembly_push(
+				&gps_nav_epoch_assembly, epoch_id, trck_ip_no,
+				gps_nav_ready_channel_mask());
+		if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE)
+		{
+			gps_nav_solve_period_pending = true;
+		}
+		else if(assembly_result == GPS_NAV_EPOCH_EVENT_STALE)
+		{
+			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+			        "stale nav epoch interrupt ignored: trk=%u epoch=%lu active=%lu\r\n",
+			        trck_ip_no, (unsigned long)epoch_id,
+			        (unsigned long)gps_nav_epoch_assembly.epoch_id);
+		}
+		else if(assembly_result == GPS_NAV_EPOCH_EVENT_FUTURE)
+		{
+			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+			        "future nav epoch blocked by incomplete epoch: trk=%u epoch=%lu active=%lu expected=0x%x received=0x%x\r\n",
+			        trck_ip_no, (unsigned long)epoch_id,
+			        (unsigned long)gps_nav_epoch_assembly.epoch_id,
+			        gps_nav_epoch_assembly.expected_mask,
+			        gps_nav_epoch_assembly.received_mask);
+		}
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK,
 		        "nav solve: trk=%u abs_raw=%u rem=%u abs=%.6f timer=%u count=%u dt=%.6f tow=%f seq=%u\r\n",
 		        trck_ip_no,
@@ -8,12 +8,16 @@
 #include "gps/gps_ip_defines.h"
 #include "gps/linked_list.h"
 #include "gps/gps_interrupt_handlers.h"
+#include "gps/gps_nav_epoch.h"
 #include "gps/gps_pps.h"
 #include "gps/defines.h"
 #include <bsp/irq.h>

 #define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES ((uint32_t)(SAMPLES_PER_CODE * (double)GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS))

+_Static_assert(GPS_TRCK_IP_COUNT <= GPS_NAV_EPOCH_MAX_CANDIDATES,
+               "GPS nav epoch selector capacity is smaller than the tracking channel count");
+
 volatile gps_acq_t* const  acq0_ip  = (volatile gps_acq_t*)ACQ0_IP_BASE_ADDR;
 volatile gps_acq_t* acq_ips[GPS_ACQ_IP_COUNT];
 unsigned int acq_ips_intr_lines[GPS_ACQ_IP_COUNT];
@@ -98,16 +102,6 @@ bool gps_get_ionoutc(ionoutc_t* ionoutc)
 	return true;
 }

-static bool gps_abs_sample_after(uint32_t a, uint32_t b)
-{
-	return ((int32_t)(a - b)) > 0;
-}
-
-static uint32_t gps_abs_sample_delta(uint32_t newer, uint32_t older)
-{
-	return newer - older;
-}
-
 static double gps_unwrap_corrected_abs_sample(uint32_t raw_sample,
 	                                          double corrected_sample,
 	                                          uint32_t reference_sample)
@@ -133,93 +127,8 @@ 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 uint32_t gps_nav_epoch_excluded_total;
+static uint32_t gps_nav_epoch_reject_total;

 static int32_t gps_sign_extend(uint32_t value, unsigned int bits)
 {
@@ -497,6 +406,7 @@ static void gps_stage_ephemeris_subframe(const unsigned int* subframe,
 	gps_parse_ephemeris_subframe1(sf1, &frame->gps_ephems);
 	gps_parse_ephemeris_subframe2(sf2, &frame->gps_ephems);
 	gps_parse_ephemeris_subframe3(sf3, &frame->gps_ephems);
+	frame->gps_ephems.ephemeris_valid = true;
 	frame->pending_ephemeris_mask = 0u;
 	GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV, "consistent ephemeris set committed: iode=%u\r\n", iode2);
 	if(frame->gps_ephems.svhlth != 0u)
@@ -591,10 +501,10 @@ static bool gps_tracking_ip_alive(unsigned char trck_ip_no)
 	return true;
 }

-static bool gps_node_has_initial_frames(volatile gps_ephem_ll_t* node)
+static bool gps_node_has_committed_ephemeris(volatile gps_ephem_ll_t* node)
 {
-	return (node != NULL) &&
-	       ((node->gps_subframe_mask & GPS_INITIAL_SUBFRAME_MASK) == GPS_INITIAL_SUBFRAME_MASK);
+	return (node != NULL) && (node->data_p != NULL) &&
+	       node->data_p->ephemeris_valid;
 }

 static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
@@ -604,7 +514,8 @@ static bool gps_node_ready_for_position(volatile gps_ephem_ll_t* node)
 		return false;
 	}

-	if(!gps_tracking_ip_alive(node->tracking_ip_no) || !gps_node_has_initial_frames(node))
+	if(!gps_tracking_ip_alive(node->tracking_ip_no) ||
+	   !gps_node_has_committed_ephemeris(node))
 	{
 		return false;
 	}
@@ -656,11 +567,11 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
 		volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, ip_no);
 		if((node != NULL) && gps_node_ready_for_position(node))
 		{
-				if(!gps_channel_nav_solve_enabled(ip_no))
-				{
-					gps_set_channel_nav_solve_mode(ip_no);
-					node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
-					GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
+			if(!gps_channel_nav_solve_enabled(ip_no))
+			{
+				gps_set_channel_nav_solve_mode(ip_no);
+				node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
+				GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
 				        "tracking channel %u enabled nav solve; nav bits remain enabled\r\n",
 				        ip_no);
 			}
@@ -688,38 +599,11 @@ static unsigned int gps_collect_busy_tracking_locked(

 static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
                                                    unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
+                                                    uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
                                                    bool require_nav_solve_mode,
-	                                                    bool require_nav_sequence)
+                                                    bool require_nav_sequence)
 {
 	unsigned int count = 0;
-	uint32_t newest_abs_sample = 0;
-	bool have_newest_abs_sample = false;
-
-	if(require_nav_sequence)
-	{
-		for(volatile gps_ephem_ll_t* epoch_iterator = tracking_node;
-		    epoch_iterator != NULL;
-		    epoch_iterator = epoch_iterator->next)
-		{
-			unsigned char ip_no = epoch_iterator->tracking_ip_no;
-			if(gps_node_ready_for_position(epoch_iterator) &&
-			   (!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
-			   (epoch_iterator->gps_nav_solve_sequence != epoch_iterator->gps_nav_solve_consumed_sequence))
-			{
-				if(!have_newest_abs_sample ||
-				   gps_abs_sample_after(epoch_iterator->gps_last_nav_solve_abs_sample, newest_abs_sample))
-				{
-					newest_abs_sample = epoch_iterator->gps_last_nav_solve_abs_sample;
-					have_newest_abs_sample = true;
-				}
-			}
-		}
-
-		if(!have_newest_abs_sample)
-		{
-			return 0;
-		}
-	}

 	for(volatile gps_ephem_ll_t* iterator = tracking_node;
 	    iterator != NULL;
@@ -729,10 +613,10 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
 		gps_ephemeris_t* eph = (gps_ephemeris_t*)iterator->data_p;
 		if(gps_node_ready_for_position(iterator) &&
 		   (!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
-			   (!require_nav_sequence ||
-			    ((eph != NULL) &&
-			     (iterator->gps_nav_solve_sequence != iterator->gps_nav_solve_consumed_sequence) &&
-			     (gps_abs_sample_delta(newest_abs_sample, iterator->gps_last_nav_solve_abs_sample) <= GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES))))
+		   (!require_nav_sequence ||
+		    ((eph != NULL) &&
+		     (iterator->gps_nav_solve_sequence !=
+		      iterator->gps_nav_solve_consumed_sequence))))
 		{
 			if(gps_snapshot_has_svid(snapshot, count, eph->SVID))
 			{
@@ -742,17 +626,22 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
 				continue;
 			}
 			snapshot[count] = *eph;
-				if(require_nav_sequence)
+			if(require_nav_sequence)
 			{
 				/* Nav bits may update the shared ephemeris between nav-solve
-				 * epochs. Position measurements must use nav-solve timing only. */
-				snapshot[count].abs_sample = gps_unwrap_corrected_abs_sample(
-					iterator->gps_last_nav_solve_abs_sample,
-					iterator->gps_last_nav_solve_corrected_abs_sample,
-					newest_abs_sample);
+				 * epochs. Position measurements must use the timing latched by
+				 * the nav-solve event, not the shared nav-bit fields. */
+				snapshot[count].tow = iterator->gps_last_nav_solve_tow;
+				snapshot[count].abs_sample =
+					iterator->gps_last_nav_solve_corrected_abs_sample;
+				if(snapshot_raw_samples != NULL)
+				{
+					snapshot_raw_samples[count] =
+						iterator->gps_last_nav_solve_abs_sample;
+				}
 			}
 			snapshot_ips[count] = ip_no;
-				count++;
+			count++;
 			if(count >= GPS_TRCK_IP_COUNT)
 			{
 				break;
@@ -762,6 +651,79 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
 	return count;
 }

+static unsigned int gps_select_coherent_nav_snapshot(
+	gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
+	unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
+	uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
+	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
+	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
+	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
+	uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
+	unsigned int candidate_count,
+	uint32_t* selected_mask_out)
+{
+	if((candidate_count == 0u) ||
+	   (candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES))
+	{
+		*selected_mask_out = 0u;
+		return 0u;
+	}
+
+	gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES];
+	for(unsigned int i = 0u ; i < candidate_count ; i++)
+	{
+		candidates[i].week = snapshot[i].tow.week;
+		candidates[i].tow_sec = snapshot[i].tow.sec;
+		candidates[i].raw_sample = snapshot_raw_samples[i];
+	}
+
+	gps_nav_epoch_selection_t selection;
+	if(!gps_nav_epoch_select(candidates, candidate_count,
+	                         GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_SAMPLES,
+	                         GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
+	                         &selection))
+	{
+		*selected_mask_out = 0u;
+		return 0u;
+	}
+
+	uint32_t reference_sample =
+		snapshot_raw_samples[selection.reference_index];
+	unsigned int destination = 0u;
+	for(unsigned int source = 0u ; source < candidate_count ; source++)
+	{
+		if((selection.selected_mask & (1u << source)) == 0u)
+		{
+			continue;
+		}
+
+		double corrected_sample = snapshot[source].abs_sample;
+		if(destination != source)
+		{
+			snapshot[destination] = snapshot[source];
+			snapshot_ips[destination] = snapshot_ips[source];
+			snapshot_sequences[destination] = snapshot_sequences[source];
+			snapshot_pps_counters[destination] =
+				snapshot_pps_counters[source];
+			snapshot_pps_sequences[destination] =
+				snapshot_pps_sequences[source];
+			snapshot_local_timestamps[destination] =
+				snapshot_local_timestamps[source];
+			snapshot_raw_samples[destination] =
+				snapshot_raw_samples[source];
+		}
+		snapshot[destination].abs_sample =
+			gps_unwrap_corrected_abs_sample(
+				snapshot_raw_samples[destination],
+				corrected_sample,
+				reference_sample);
+		destination++;
+	}
+
+	*selected_mask_out = selection.selected_mask;
+	return destination;
+}
+
 static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
 	                                         uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
 	                                         uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
@@ -819,6 +781,32 @@ static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GP
 	}
 }

+static void gps_consume_rejected_nav_sequences_locked(
+	const unsigned char candidate_ips[GPS_TRCK_IP_COUNT],
+	const uint32_t candidate_sequences[GPS_TRCK_IP_COUNT],
+	unsigned int candidate_count,
+	uint32_t selected_mask)
+{
+	for(unsigned int i = 0u ; i < candidate_count ; i++)
+	{
+		if((selected_mask & (1u << i)) != 0u)
+		{
+			continue;
+		}
+		volatile gps_ephem_ll_t* node =
+			get_element_by_ip_number(&tracking_node, candidate_ips[i]);
+		if(node != NULL)
+		{
+			/*
+			 * Consume exactly the rejected snapshot.  If an interrupt has
+			 * already produced a newer sequence, it remains different from
+			 * this consumed value and is preserved for the next epoch.
+			 */
+			node->gps_nav_solve_consumed_sequence = candidate_sequences[i];
+		}
+	}
+}
+
 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],
@@ -1104,7 +1092,10 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 	uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
 	uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
 	uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
+	uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT];
 	unsigned int satellite_count = 0;
+	unsigned int candidate_count = 0;
+	uint32_t selected_candidate_mask = 0u;
 	bool run_nav = false;
 	bool initial_run = false;
 	unsigned int ready_count;
@@ -1124,7 +1115,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,

 	uint64_t irq_state = bsp_irq_save();

-	ready_count = gps_collect_nav_snapshot_locked(snapshot, snapshot_ips, false, false);
+	ready_count = gps_collect_nav_snapshot_locked(
+		snapshot, snapshot_ips, NULL, false, false);
 	busy_count = gps_collect_busy_tracking_locked(
 		GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);

@@ -1139,25 +1131,71 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 		gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
 		if(nav_period_ready)
 		{
-			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;
+			candidate_count = gps_collect_nav_snapshot_locked(
+				snapshot, snapshot_ips, snapshot_raw_samples, true, true);
+			gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
+			                               snapshot_pps_counters,
+			                               snapshot_pps_sequences,
+			                               snapshot_local_timestamps,
+			                               candidate_count);
 			initial_run = !gps_initial_position_valid;
-			if(run_nav)
-			{
-				gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
-				                               snapshot_pps_counters, snapshot_pps_sequences,
-				                               snapshot_local_timestamps,
-				                               satellite_count);
-			}
 		}
 	}

 	bsp_irq_restore(irq_state);

+	if(nav_period_ready && (candidate_count != 0u))
+	{
+		/*
+		 * Preserve the pre-compaction identity of rejected candidates so their
+		 * exact sequence can be retired without touching a newer interrupt.
+		 */
+		unsigned char candidate_ips[GPS_TRCK_IP_COUNT];
+		uint32_t candidate_sequences[GPS_TRCK_IP_COUNT];
+		memcpy(candidate_ips, snapshot_ips,
+		       candidate_count * sizeof(candidate_ips[0]));
+		memcpy(candidate_sequences, snapshot_sequences,
+		       candidate_count * sizeof(candidate_sequences[0]));
+
+		satellite_count = gps_select_coherent_nav_snapshot(
+			snapshot, snapshot_ips, snapshot_sequences,
+			snapshot_pps_counters, snapshot_pps_sequences,
+			snapshot_local_timestamps, snapshot_raw_samples,
+			candidate_count, &selected_candidate_mask);
+		coherent_nav_count = satellite_count;
+		run_nav = satellite_count >= MIN_NEEDED_SATELLITE;
+
+		if(run_nav && (satellite_count < candidate_count))
+		{
+			unsigned int excluded_count = candidate_count - satellite_count;
+			gps_nav_epoch_excluded_total += excluded_count;
+			uint64_t consume_irq_state = bsp_irq_save();
+			gps_consume_rejected_nav_sequences_locked(
+				candidate_ips, candidate_sequences, candidate_count,
+				selected_candidate_mask);
+			bsp_irq_restore(consume_irq_state);
+			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+			        "NAV coherent subset selected: candidates=%u selected=%u excluded=%u excluded_total=%u\r\n",
+			        candidate_count, satellite_count, excluded_count,
+			        gps_nav_epoch_excluded_total);
+		}
+		else if(!run_nav)
+		{
+			uint64_t consume_irq_state = bsp_irq_save();
+			gps_consume_rejected_nav_sequences_locked(
+				candidate_ips, candidate_sequences, candidate_count, 0u);
+			bsp_irq_restore(consume_irq_state);
+			if(candidate_count >= MIN_NEEDED_SATELLITE)
+			{
+				gps_nav_epoch_reject_total++;
+				GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
+				        "NAV epoch has no coherent four-satellite subset: candidates=%u best=%u rejects=%u\r\n",
+				        candidate_count, satellite_count,
+				        gps_nav_epoch_reject_total);
+			}
+		}
+	}
+
 	uint64_t now = gps_get_systick();
 	if((busy_count != last_nav_status_busy_count) ||
 	   (ready_count != last_nav_status_ready_count) ||
@@ -1202,10 +1240,14 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 				*measurement_timestamp_ticks_out = measurement_timestamp;
 				result_ready = true;
 			}
-			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);
 		}
+		/* A completed hardware epoch is attempted once. Retrying the same
+		 * measurements after a WLS failure would couple solution cadence to an
+		 * unrelated foreground wake-up. */
+		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);
 		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;
@@ -1218,7 +1260,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 		        "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",
+		        "epoch_excluded=%u epoch_rejected=%u\r\n",
 		        GPS_NAV_SOLVE_PERIOD_MS,
 		        (unsigned long)GPS_PROFILE_CPU_HZ,
 		        (unsigned long)foreground_cycles,
@@ -1241,8 +1283,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 		        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);
+		        gps_nav_epoch_excluded_total,
+		        gps_nav_epoch_reject_total);
 		(void)foreground_cycles;
 		(void)nav_software_cycles;
 		(void)period_budget_cycles;
@@ -1384,8 +1426,8 @@ void gps_startup_initialization(unsigned int nsp_tick)
 	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;
+	gps_nav_epoch_excluded_total = 0u;
+	gps_nav_epoch_reject_total = 0u;
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
 		acq_ips[i]->CONTROL_REG = (1u << GPS_ACQ_INTPRT_CLR);
@@ -0,0 +1,301 @@
+#include "gps/gps_nav_epoch.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#define GPS_WEEK_SECONDS      604800.0
+#define GPS_HALF_WEEK_SECONDS 302400.0
+#define GPS_WEEK_MILLISECONDS 604800000ULL
+
+static double gps_nav_epoch_tow_delta(
+	const gps_nav_epoch_key_t* first,
+	const gps_nav_epoch_key_t* second)
+{
+	double delta = (((double)first->week - (double)second->week) *
+	                GPS_WEEK_SECONDS) +
+	               first->tow_sec - second->tow_sec;
+	if(delta > GPS_HALF_WEEK_SECONDS)
+	{
+		delta -= GPS_WEEK_SECONDS;
+	}
+	else if(delta < -GPS_HALF_WEEK_SECONDS)
+	{
+		delta += GPS_WEEK_SECONDS;
+	}
+	return delta;
+}
+
+static uint32_t gps_nav_epoch_sample_distance(uint32_t first, uint32_t second)
+{
+	uint32_t forward = first - second;
+	uint32_t reverse = second - first;
+	return (forward < reverse) ? forward : reverse;
+}
+
+static bool gps_nav_epoch_sample_after(uint32_t first, uint32_t second)
+{
+	uint32_t delta = first - second;
+	return (delta != 0u) && (delta < 0x80000000u);
+}
+
+static unsigned int gps_nav_epoch_mask_count(uint32_t mask)
+{
+	unsigned int count = 0u;
+	while(mask != 0u)
+	{
+		count += mask & 1u;
+		mask >>= 1u;
+	}
+	return count;
+}
+
+static bool gps_nav_epoch_candidate_valid(const gps_nav_epoch_key_t* candidate)
+{
+	return (candidate != NULL) &&
+	       isfinite(candidate->tow_sec) &&
+	       (candidate->tow_sec >= 0.0) &&
+	       (candidate->tow_sec < GPS_WEEK_SECONDS);
+}
+
+static bool gps_nav_epoch_subset_coherent(
+	const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
+	unsigned int candidate_count,
+	uint32_t subset_mask,
+	uint32_t sample_window,
+	double tow_tolerance_sec,
+	unsigned int* reference_index)
+{
+	unsigned int reference = GPS_NAV_EPOCH_MAX_CANDIDATES;
+	for(unsigned int first = 0u ; first < candidate_count ; first++)
+	{
+		if((subset_mask & (1u << first)) == 0u)
+		{
+			continue;
+		}
+		if(!gps_nav_epoch_candidate_valid(&candidates[first]))
+		{
+			return false;
+		}
+		if((reference == GPS_NAV_EPOCH_MAX_CANDIDATES) ||
+		   gps_nav_epoch_sample_after(candidates[first].raw_sample,
+		                              candidates[reference].raw_sample))
+		{
+			reference = first;
+		}
+
+		for(unsigned int second = first + 1u ; second < candidate_count ; second++)
+		{
+			if((subset_mask & (1u << second)) == 0u)
+			{
+				continue;
+			}
+			double tow_delta = gps_nav_epoch_tow_delta(
+				&candidates[first], &candidates[second]);
+			if(!isfinite(tow_delta) ||
+			   (fabs(tow_delta) > tow_tolerance_sec) ||
+			   (gps_nav_epoch_sample_distance(candidates[first].raw_sample,
+			                                  candidates[second].raw_sample) >
+			    sample_window))
+			{
+				return false;
+			}
+		}
+	}
+
+	if(reference == GPS_NAV_EPOCH_MAX_CANDIDATES)
+	{
+		return false;
+	}
+	*reference_index = reference;
+	return true;
+}
+
+bool gps_nav_epoch_select(
+	const gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES],
+	unsigned int candidate_count,
+	uint32_t sample_window,
+	double tow_tolerance_sec,
+	gps_nav_epoch_selection_t* selection)
+{
+	if((candidates == NULL) || (selection == NULL) ||
+	   (candidate_count == 0u) ||
+	   (candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES) ||
+	   (sample_window == 0u) ||
+	   !isfinite(tow_tolerance_sec) ||
+	   (tow_tolerance_sec < 0.0))
+	{
+		return false;
+	}
+
+	selection->selected_mask = 0u;
+	selection->selected_count = 0u;
+	selection->reference_index = 0u;
+
+	uint32_t subset_limit = 1u << candidate_count;
+	for(uint32_t subset = 1u ; subset < subset_limit ; subset++)
+	{
+		unsigned int subset_count = gps_nav_epoch_mask_count(subset);
+		if(subset_count < selection->selected_count)
+		{
+			continue;
+		}
+
+		unsigned int reference;
+		if(!gps_nav_epoch_subset_coherent(candidates, candidate_count, subset,
+		                                  sample_window, tow_tolerance_sec,
+		                                  &reference))
+		{
+			continue;
+		}
+
+		bool select_subset = subset_count > selection->selected_count;
+		if(!select_subset && (selection->selected_count != 0u))
+		{
+			/*
+			 * Equal-sized disjoint clusters represent different nav epochs.
+			 * Prefer the newer raw-sample epoch.  Overlapping equal-sized
+			 * subsets have the same/new-near reference and retain deterministic
+			 * mask order.
+			 */
+			uint32_t reference_delta =
+				candidates[reference].raw_sample -
+				candidates[selection->reference_index].raw_sample;
+			if(gps_nav_epoch_sample_after(
+				   candidates[reference].raw_sample,
+				   candidates[selection->reference_index].raw_sample) &&
+			   (reference_delta > sample_window))
+			{
+				select_subset = true;
+			}
+		}
+
+		if(select_subset)
+		{
+			selection->selected_mask = subset;
+			selection->selected_count = subset_count;
+			selection->reference_index = reference;
+		}
+	}
+
+	return selection->selected_count != 0u;
+}
+
+bool gps_nav_epoch_id(
+	int week,
+	double tow_sec,
+	unsigned int period_ms,
+	uint64_t* epoch_id)
+{
+	if((epoch_id == NULL) || (week < 0) || (period_ms == 0u) ||
+	   ((GPS_WEEK_MILLISECONDS % period_ms) != 0u) ||
+	   !isfinite(tow_sec) || (tow_sec < 0.0) ||
+	   (tow_sec >= GPS_WEEK_SECONDS))
+	{
+		return false;
+	}
+
+	uint64_t epochs_per_week = GPS_WEEK_MILLISECONDS / period_ms;
+	double epoch_in_week = (tow_sec * 1000.0) / (double)period_ms;
+	uint64_t rounded_epoch = (uint64_t)llround(epoch_in_week);
+	if(rounded_epoch == epochs_per_week)
+	{
+		week++;
+		rounded_epoch = 0u;
+	}
+	*epoch_id = ((uint64_t)(unsigned int)week * epochs_per_week) +
+	            rounded_epoch;
+	return true;
+}
+
+void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly)
+{
+	if(assembly == NULL)
+	{
+		return;
+	}
+	assembly->epoch_id = 0u;
+	assembly->expected_mask = 0u;
+	assembly->received_mask = 0u;
+	assembly->active = false;
+}
+
+bool gps_nav_epoch_assembly_complete(
+	const gps_nav_epoch_assembly_t* assembly)
+{
+	return (assembly != NULL) && assembly->active &&
+	       (assembly->expected_mask != 0u) &&
+	       ((assembly->received_mask & assembly->expected_mask) ==
+	        assembly->expected_mask);
+}
+
+gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
+	gps_nav_epoch_assembly_t* assembly,
+	uint64_t epoch_id,
+	unsigned int channel,
+	uint32_t ready_mask)
+{
+	if((assembly == NULL) ||
+	   (channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
+	{
+		return GPS_NAV_EPOCH_EVENT_INVALID;
+	}
+
+	uint32_t channel_mask = 1u << channel;
+	if(!assembly->active)
+	{
+		uint32_t valid_mask =
+			(1u << GPS_NAV_EPOCH_MAX_CANDIDATES) - 1u;
+		ready_mask &= valid_mask;
+		if((ready_mask & channel_mask) == 0u)
+		{
+			return GPS_NAV_EPOCH_EVENT_INVALID;
+		}
+		assembly->epoch_id = epoch_id;
+		assembly->expected_mask = ready_mask;
+		assembly->received_mask = 0u;
+		assembly->active = true;
+	}
+
+	if(epoch_id < assembly->epoch_id)
+	{
+		return GPS_NAV_EPOCH_EVENT_STALE;
+	}
+	if(epoch_id > assembly->epoch_id)
+	{
+		return GPS_NAV_EPOCH_EVENT_FUTURE;
+	}
+	if((assembly->expected_mask & channel_mask) == 0u)
+	{
+		return GPS_NAV_EPOCH_EVENT_INVALID;
+	}
+	if((assembly->received_mask & channel_mask) != 0u)
+	{
+		return GPS_NAV_EPOCH_EVENT_DUPLICATE;
+	}
+
+	assembly->received_mask |= channel_mask;
+	return gps_nav_epoch_assembly_complete(assembly) ?
+	       GPS_NAV_EPOCH_EVENT_COMPLETE :
+	       GPS_NAV_EPOCH_EVENT_ACCEPTED;
+}
+
+bool gps_nav_epoch_assembly_remove_channel(
+	gps_nav_epoch_assembly_t* assembly,
+	unsigned int channel)
+{
+	if((assembly == NULL) || !assembly->active ||
+	   (channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
+	{
+		return false;
+	}
+
+	uint32_t channel_mask = 1u << channel;
+	assembly->expected_mask &= ~channel_mask;
+	assembly->received_mask &= assembly->expected_mask;
+	if(assembly->expected_mask == 0u)
+	{
+		gps_nav_epoch_assembly_reset(assembly);
+		return false;
+	}
+	return gps_nav_epoch_assembly_complete(assembly);
+}
@@ -68,6 +68,8 @@ bool insert_element(volatile gps_ephem_ll_t** node,
 	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_tow.week = 0;
+	new_node->gps_last_nav_solve_tow.sec = 0.0;
 	new_node->gps_last_nav_solve_abs_sample = 0u;
 	new_node->gps_last_nav_solve_corrected_abs_sample = 0.0;
 	new_node->gps_last_nav_solve_local_ticks = 0u;