fix(gps): make acquisition and reacquisition deterministic

soc/software/gps_system/src/adapters/gps_system_uart.c

@@ -9,6 +9,10 @@
 
 #define GPS_AIDED_MAX_AGE_MS 1000u
 
+#ifndef GPS_BUILD_ID
+#define GPS_BUILD_ID "unknown"
+#endif
+
 void gps_system_uart_publish_location(
 	const char* source,
 	double latitude_deg,
@@ -66,9 +70,10 @@ void gps_system_uart_publish_startup(
 	imu_sensor_diagnostics_t imu;
 	imu_sensor_get_diagnostics(&imu);
 	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_FUSION,
-	        "init eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
+	        "init build=%s eskf_enabled=%u eskf_ready=%u imu=%u sensor=%s who=0x%02x "
 	        "running=%u target_hz=%lu actual_hz=%lu publish_every=%u "
 	        "nav_ms=%u timebase_hz=%lu\r\n",
+	        GPS_BUILD_ID,
 	        fusion_enabled ? 1u : 0u,
 	        fusion_initialized ? 1u : 0u,
 	        imu_initialized ? 1u : 0u,

soc/software/gps_system/src/gps/gps_acq_manager.c

@@ -0,0 +1,289 @@
+#include "gps/gps_acq_manager.h"
+
+#include <stddef.h>
+#include <string.h>
+
+static bool gps_acq_manager_prn_valid(unsigned int prn)
+{
+	return prn < GPS_ACQ_MANAGER_PRN_COUNT;
+}
+
+static bool gps_acq_manager_retry_due(
+	const gps_acq_manager_t* manager,
+	const gps_acq_prn_entry_t* entry,
+	uint64_t now,
+	bool hot)
+{
+	uint64_t retry_delay = hot
+		? manager->config.hot_retry_delay_ticks
+		: manager->config.retry_delay_ticks;
+	return !entry->has_attempted ||
+	       ((now - entry->last_attempt_tick) >=
+	        retry_delay);
+}
+
+static bool gps_acq_manager_hot_active(
+	const gps_acq_prn_entry_t* entry,
+	uint64_t now)
+{
+	return (entry->state == GPS_ACQ_PRN_HOT) &&
+	       (now <= entry->hot_until_tick);
+}
+
+static bool gps_acq_manager_find(
+	const gps_acq_manager_t* manager,
+	uint64_t now,
+	bool hot,
+	unsigned int cursor,
+	unsigned int* prn_out)
+{
+	for(unsigned int offset = 0u;
+	    offset < GPS_ACQ_MANAGER_PRN_COUNT;
+	    offset++)
+	{
+		unsigned int prn =
+			(cursor + offset) % GPS_ACQ_MANAGER_PRN_COUNT;
+		const gps_acq_prn_entry_t* entry = &manager->prn[prn];
+		bool candidate = hot
+			? gps_acq_manager_hot_active(entry, now)
+			: ((entry->state == GPS_ACQ_PRN_COLD) ||
+			   ((entry->state == GPS_ACQ_PRN_HOT) &&
+			    (now > entry->hot_until_tick)));
+		if(candidate &&
+		   gps_acq_manager_retry_due(
+			   manager, entry, now, hot))
+		{
+			*prn_out = prn;
+			return true;
+		}
+	}
+	return false;
+}
+
+void gps_acq_manager_init(
+	gps_acq_manager_t* manager,
+	const gps_acq_manager_config_t* config)
+{
+	if((manager == NULL) || (config == NULL))
+	{
+		return;
+	}
+	memset(manager, 0, sizeof(*manager));
+	manager->config = *config;
+	if(manager->config.max_hot_streak == 0u)
+	{
+		manager->config.max_hot_streak = 1u;
+	}
+}
+
+bool gps_acq_manager_enable_cold(
+	gps_acq_manager_t* manager,
+	unsigned int prn)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return false;
+	}
+	gps_acq_prn_entry_t* entry = &manager->prn[prn];
+	if(entry->state == GPS_ACQ_PRN_DISABLED)
+	{
+		entry->state = GPS_ACQ_PRN_COLD;
+		return true;
+	}
+	return entry->state == GPS_ACQ_PRN_COLD;
+}
+
+gps_acq_prn_state_t gps_acq_manager_state(
+	const gps_acq_manager_t* manager,
+	unsigned int prn)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return GPS_ACQ_PRN_DISABLED;
+	}
+	return manager->prn[prn].state;
+}
+
+const char* gps_acq_manager_state_name(gps_acq_prn_state_t state)
+{
+	switch(state)
+	{
+		case GPS_ACQ_PRN_DISABLED:
+			return "disabled";
+		case GPS_ACQ_PRN_COLD:
+			return "cold";
+		case GPS_ACQ_PRN_HOT:
+			return "hot";
+		case GPS_ACQ_PRN_ACQUIRING:
+			return "acquiring";
+		case GPS_ACQ_PRN_ACQUIRED:
+			return "acquired";
+		case GPS_ACQ_PRN_TRACKING:
+			return "tracking";
+		case GPS_ACQ_PRN_STALE:
+			return "stale";
+		default:
+			return "unknown";
+	}
+}
+
+void gps_acq_manager_mark_acquired(
+	gps_acq_manager_t* manager,
+	unsigned int prn)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return;
+	}
+	manager->prn[prn].state = GPS_ACQ_PRN_ACQUIRED;
+	manager->prn[prn].attempt_was_hot = false;
+}
+
+void gps_acq_manager_mark_tracking(
+	gps_acq_manager_t* manager,
+	unsigned int prn)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return;
+	}
+	manager->prn[prn].state = GPS_ACQ_PRN_TRACKING;
+	manager->prn[prn].attempt_was_hot = false;
+	manager->prn[prn].hot_until_tick = 0u;
+}
+
+void gps_acq_manager_mark_stale(
+	gps_acq_manager_t* manager,
+	unsigned int prn)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return;
+	}
+	if(manager->prn[prn].state == GPS_ACQ_PRN_TRACKING)
+	{
+		manager->prn[prn].state = GPS_ACQ_PRN_STALE;
+	}
+}
+
+void gps_acq_manager_mark_lost(
+	gps_acq_manager_t* manager,
+	unsigned int prn,
+	uint64_t now)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return;
+	}
+	gps_acq_prn_entry_t* entry = &manager->prn[prn];
+	bool already_hot = entry->state == GPS_ACQ_PRN_HOT;
+	entry->state = GPS_ACQ_PRN_HOT;
+	entry->attempt_was_hot = false;
+	entry->hot_until_tick = now + manager->config.hot_window_ticks;
+	if(!already_hot)
+	{
+		/* A real tracking loss gets one immediate reacquisition attempt.
+		 * Later failures observe the hot retry backoff. */
+		entry->has_attempted = false;
+	}
+}
+
+void gps_acq_manager_mark_failed(
+	gps_acq_manager_t* manager,
+	unsigned int prn,
+	uint64_t now)
+{
+	if((manager == NULL) || !gps_acq_manager_prn_valid(prn))
+	{
+		return;
+	}
+	gps_acq_prn_entry_t* entry = &manager->prn[prn];
+	entry->last_attempt_tick = now;
+	entry->has_attempted = true;
+	entry->state =
+		entry->attempt_was_hot && (now <= entry->hot_until_tick)
+		? GPS_ACQ_PRN_HOT : GPS_ACQ_PRN_COLD;
+	entry->attempt_was_hot = false;
+}
+
+bool gps_acq_manager_start_next(
+	gps_acq_manager_t* manager,
+	uint64_t now,
+	unsigned int* prn_out,
+	bool* hot_out)
+{
+	if((manager == NULL) || (prn_out == NULL))
+	{
+		return false;
+	}
+
+	unsigned int hot_prn = GPS_ACQ_MANAGER_INVALID_PRN;
+	unsigned int cold_prn = GPS_ACQ_MANAGER_INVALID_PRN;
+	bool have_hot = gps_acq_manager_find(
+		manager, now, true, manager->hot_cursor, &hot_prn);
+	bool have_cold = gps_acq_manager_find(
+		manager, now, false, manager->cold_cursor, &cold_prn);
+	bool choose_hot = have_hot &&
+		(!have_cold ||
+		 (manager->hot_streak < manager->config.max_hot_streak));
+
+	unsigned int prn;
+	if(choose_hot)
+	{
+		prn = hot_prn;
+		manager->hot_cursor =
+			(prn + 1u) % GPS_ACQ_MANAGER_PRN_COUNT;
+		manager->hot_streak++;
+	}
+	else if(have_cold)
+	{
+		prn = cold_prn;
+		manager->cold_cursor =
+			(prn + 1u) % GPS_ACQ_MANAGER_PRN_COUNT;
+		manager->hot_streak = 0u;
+	}
+	else
+	{
+		return false;
+	}
+
+	gps_acq_prn_entry_t* entry = &manager->prn[prn];
+	entry->attempt_was_hot = choose_hot;
+	entry->last_attempt_tick = now;
+	entry->has_attempted = true;
+	entry->state = GPS_ACQ_PRN_ACQUIRING;
+	*prn_out = prn;
+	if(hot_out != NULL)
+	{
+		*hot_out = choose_hot;
+	}
+	return true;
+}
+
+unsigned int gps_acq_manager_count_state(
+	const gps_acq_manager_t* manager,
+	gps_acq_prn_state_t state)
+{
+	if(manager == NULL)
+	{
+		return 0u;
+	}
+	unsigned int count = 0u;
+	for(unsigned int prn = 0u;
+	    prn < GPS_ACQ_MANAGER_PRN_COUNT;
+	    prn++)
+	{
+		if(manager->prn[prn].state == state)
+		{
+			count++;
+		}
+	}
+	return count;
+}
+
+unsigned int gps_acq_manager_count_searchable(
+	const gps_acq_manager_t* manager)
+{
+	return gps_acq_manager_count_state(manager, GPS_ACQ_PRN_COLD) +
+	       gps_acq_manager_count_state(manager, GPS_ACQ_PRN_HOT);
+}

soc/software/gps_system/src/gps/gps_ephemeris_cache.c

@@ -0,0 +1,68 @@
+#include "gps/gps_ephemeris_cache.h"
+
+#include <stddef.h>
+#include <string.h>
+
+void gps_ephemeris_cache_init(
+	gps_ephemeris_cache_t* cache,
+	uint64_t max_age_ticks)
+{
+	if(cache == NULL)
+	{
+		return;
+	}
+	memset(cache, 0, sizeof(*cache));
+	cache->max_age_ticks = max_age_ticks;
+}
+
+bool gps_ephemeris_cache_store(
+	gps_ephemeris_cache_t* cache,
+	unsigned int prn,
+	const gps_ephemeris_t* ephemeris,
+	uint64_t now)
+{
+	if((cache == NULL) || (ephemeris == NULL) ||
+	   (prn >= GPS_PRN_COUNT) ||
+	   !ephemeris->ephemeris_valid ||
+	   (ephemeris->SVID != prn))
+	{
+		return false;
+	}
+	cache->entry[prn].ephemeris = *ephemeris;
+	cache->entry[prn].committed_tick = now;
+	cache->entry[prn].valid = true;
+	return true;
+}
+
+bool gps_ephemeris_cache_restore(
+	gps_ephemeris_cache_t* cache,
+	unsigned int prn,
+	uint64_t now,
+	gps_ephemeris_t* ephemeris_out)
+{
+	if((cache == NULL) || (ephemeris_out == NULL) ||
+	   (prn >= GPS_PRN_COUNT))
+	{
+		return false;
+	}
+	gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
+	if(!entry->valid ||
+	   ((now - entry->committed_tick) > cache->max_age_ticks))
+	{
+		entry->valid = false;
+		return false;
+	}
+	*ephemeris_out = entry->ephemeris;
+	return true;
+}
+
+void gps_ephemeris_cache_invalidate(
+	gps_ephemeris_cache_t* cache,
+	unsigned int prn)
+{
+	if((cache == NULL) || (prn >= GPS_PRN_COUNT))
+	{
+		return;
+	}
+	memset(&cache->entry[prn], 0, sizeof(cache->entry[prn]));
+}

soc/software/gps_system/src/gps/gps_interrupt_handlers.c

@@ -1,4 +1,6 @@
 #include "gps/linked_list.h"
+#include "gps/gps_acq_manager.h"
+#include "gps/gps_ephemeris_cache.h"
 #include "gps/gps_interrupt_handlers.h"
 #include "gps/Navigation.h"
 #include "gps/gps_nav_epoch.h"
@@ -56,10 +58,10 @@ static volatile bool acq_shadow_pending[GPS_ACQ_IP_COUNT];
 static gps_acq_result_t acq_shadow_result[GPS_ACQ_IP_COUNT];
 static gps_tracking_shadow_t trk_shadow[GPS_TRCK_IP_COUNT];
 
-static volatile gps_satellite_state_t satellite_state[GPS_PRN_COUNT];
 static volatile unsigned char satellite_tracking_ip[GPS_PRN_COUNT];
-static volatile uint64_t satellite_last_acq_tick[GPS_PRN_COUNT];
 static gps_acq_result_t satellite_acq_result[GPS_PRN_COUNT];
+static gps_acq_manager_t gps_acq_manager;
+static gps_ephemeris_cache_t gps_ephemeris_cache;
 
 static gps_isr_event_t gps_event_queue[GPS_EVENT_QUEUE_SIZE];
 static volatile unsigned int gps_event_head = 0;
@@ -74,7 +76,7 @@ static gps_nav_epoch_snapshot_t gps_nav_active_snapshot;
 static gps_nav_epoch_snapshot_t gps_nav_completed_snapshot;
 static bool gps_nav_completed_snapshot_pending;
 static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
-static unsigned int gps_acq_round_robin_prn = 0;
+static uint64_t gps_last_acq_status_log_tick = 0u;
 static unsigned int gps_external_irq_id = 0u;
 static gps_external_irq_handler_t gps_external_irq_handler = NULL;
 
@@ -104,27 +106,6 @@ static unsigned int gps_mask_count(uint32_t mask)
 	return count;
 }
 
-static const char* gps_satellite_state_name(gps_satellite_state_t state)
-{
-	switch(state)
-	{
-		case GPS_SAT_NOT_SEEN:
-			return "not_seen";
-		case GPS_SAT_ACQUIRING:
-			return "acquiring";
-		case GPS_SAT_ACQUIRED:
-			return "acquired";
-		case GPS_SAT_TRACKING:
-			return "tracking";
-		case GPS_SAT_LOST:
-			return "lost";
-		case GPS_SAT_STALE:
-			return "stale";
-		default:
-			return "unknown";
-	}
-}
-
 static double gps_correct_abs_sample(uint32_t abs_sample, uint32_t rem_code_phase)
 {
 	return (double)abs_sample - ((double)rem_code_phase / CODE_FREQ_BASIS);
@@ -353,6 +334,14 @@ static void gps_set_tracking_nav_bits_mode(unsigned char trck_ip_no)
 
 static void gps_runtime_state_init(void)
 {
+	const gps_acq_manager_config_t acq_config = {
+		.retry_delay_ticks = GPS_ACQ_RETRY_DELAY_TICKS,
+		.hot_retry_delay_ticks =
+			GPS_ACQ_HOT_RETRY_DELAY_TICKS,
+		.hot_window_ticks = GPS_ACQ_REACQ_PRIORITY_TICKS,
+		.max_hot_streak = GPS_ACQ_MAX_HOT_STREAK
+	};
+
 	gps_event_head = 0;
 	gps_event_tail = 0;
 	gps_event_overflow_count = 0;
@@ -367,9 +356,13 @@ static void gps_runtime_state_init(void)
 	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;
+	gps_last_acq_status_log_tick = 0u;
 	gps_external_irq_id = 0u;
 	gps_external_irq_handler = NULL;
+	gps_acq_manager_init(&gps_acq_manager, &acq_config);
+	gps_ephemeris_cache_init(
+		&gps_ephemeris_cache,
+		GPS_EPHEMERIS_CACHE_MAX_AGE_TICKS);
 
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
@@ -385,9 +378,7 @@ static void gps_runtime_state_init(void)
 
 	for(unsigned int i = 0 ; i < GPS_PRN_COUNT ; i++)
 	{
-		satellite_state[i] = GPS_SAT_NOT_SEEN;
 		satellite_tracking_ip[i] = GPS_PRN_INVALID;
-		satellite_last_acq_tick[i] = 0;
 		memset(&satellite_acq_result[i], 0, sizeof(satellite_acq_result[i]));
 	}
 
@@ -400,6 +391,25 @@ static void gps_runtime_state_init(void)
 
 }
 
+void gps_acq_configure_prns(
+	const unsigned char* prns,
+	unsigned int count)
+{
+	if(prns == NULL)
+	{
+		return;
+	}
+	for(unsigned int i = 0u ; i < count ; i++)
+	{
+		if(gps_prn_valid(prns[i]))
+		{
+			(void)gps_acq_manager_enable_cold(
+				&gps_acq_manager, prns[i]);
+		}
+	}
+	gps_acq_schedule_pending = true;
+}
+
 bool gps_interrupt_attach_external_source(
 	unsigned int irq_id,
 	gps_external_irq_handler_t handler)
@@ -598,18 +608,22 @@ static void gps_mark_satellite_tracking(unsigned int prn_no, unsigned char trck_
 {
 	if(gps_prn_valid(prn_no))
 	{
-		gps_satellite_state_t previous_state = satellite_state[prn_no];
+		gps_acq_prn_state_t previous_state =
+			gps_acq_manager_state(&gps_acq_manager, prn_no);
 		unsigned char previous_ip = satellite_tracking_ip[prn_no];
-		satellite_state[prn_no] = GPS_SAT_TRACKING;
+		gps_acq_manager_mark_tracking(&gps_acq_manager, prn_no);
 		satellite_tracking_ip[prn_no] = trck_ip_no;
-		if((previous_state != GPS_SAT_TRACKING) || (previous_ip != trck_ip_no))
+		if((previous_state != GPS_ACQ_PRN_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);
+			        prn_no, trck_ip_no,
+			        gps_acq_manager_state_name(previous_state),
+			        previous_ip);
 		}
 	}
 }
@@ -618,18 +632,22 @@ static void gps_mark_satellite_lost(unsigned int prn_no)
 {
 	if(gps_prn_valid(prn_no))
 	{
-		gps_satellite_state_t previous_state = satellite_state[prn_no];
+		gps_acq_prn_state_t previous_state =
+			gps_acq_manager_state(&gps_acq_manager, prn_no);
 		unsigned char previous_ip = satellite_tracking_ip[prn_no];
-		satellite_state[prn_no] = GPS_SAT_LOST;
+		uint64_t now = gps_get_systick();
+		gps_acq_manager_mark_lost(&gps_acq_manager, prn_no, now);
 		satellite_tracking_ip[prn_no] = GPS_PRN_INVALID;
-		if(previous_state != GPS_SAT_LOST)
+		if(previous_state != GPS_ACQ_PRN_HOT)
 		{
 			gps_runtime_diagnostics.satellite_lost_total++;
 			gps_runtime_diagnostics.last_lost_prn = prn_no;
-			gps_runtime_diagnostics.last_lost_tick = gps_get_systick();
+			gps_runtime_diagnostics.last_lost_tick = now;
 			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);
+			        prn_no,
+			        gps_acq_manager_state_name(previous_state),
+			        previous_ip);
 		}
 	}
 }
@@ -652,7 +670,6 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
 		if(active && (trk->PRN_NUMBER == prn_no))
 		{
 			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "acquired prn %u is already being tracked\r\n", prn_no);
-			(void)delete_acq_element_by_prn_number(&acquisition_node, prn_no);
 			gps_mark_satellite_tracking(prn_no, i);
 			return false;
 		}
@@ -666,13 +683,25 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
 		{
 			tracking_allocated[i] = true;
 			gps_clear_tracking_slot(i);
+			gps_ephemeris_t cached_ephemeris;
+			if(gps_ephemeris_cache_restore(
+			       &gps_ephemeris_cache, prn_no,
+			       gps_get_systick(), &cached_ephemeris))
+			{
+				gps_frames_parsed[i].gps_ephems =
+					cached_ephemeris;
+				gps_runtime_diagnostics
+					.ephemeris_cache_restore_total++;
+				GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
+				        "restored ephemeris cache: prn=%u trk=%u iode=%u\r\n",
+				        prn_no, i, cached_ephemeris.iode);
+			}
 			gps_set_tracking_nav_bits_mode(i);
 			trck_ips[i]->PRN_NUMBER = result->prn;
 			trck_ips[i]->CODE_PHASE = result->code_phase;
 			trck_ips[i]->DOPPLER_REG = result->doppler;
 			trck_ips[i]->CONTROL_REG |= (1u << GPS_TRCK_NAV_BITS_INTR_EN_POS);
 			trck_ips[i]->CONTROL_REG |= (1u << GPS_TRCK_START_POS);
-			(void)delete_acq_element_by_prn_number(&acquisition_node, prn_no);
 			gps_mark_satellite_tracking(prn_no, i);
 			GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK, "tracking started: prn=%u trk=%u code=%u doppler=%u\r\n",
 			        prn_no, i, result->code_phase, result->doppler);
@@ -850,19 +879,16 @@ static void gps_process_acq_result(const gps_acq_result_t* result)
 		return;
 	}
 
-	satellite_last_acq_tick[prn] = result->tick;
 	if(!result->acquired)
 	{
-		if(satellite_state[prn] == GPS_SAT_ACQUIRING)
-		{
-			satellite_state[prn] = GPS_SAT_NOT_SEEN;
-		}
+		gps_acq_manager_mark_failed(
+			&gps_acq_manager, prn, result->tick);
 		GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_ACQ, "satellite search ended: engine=%u prn=%u status=0x%x result=not_acquired\r\n",
 		        result->engine_id, prn, result->status);
 		return;
 	}
 
-	satellite_state[prn] = GPS_SAT_ACQUIRED;
+	gps_acq_manager_mark_acquired(&gps_acq_manager, prn);
 	satellite_acq_result[prn] = *result;
 	GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_ACQ,
 	        "acquired: engine=%u prn=%u code=%u doppler=%u peak=%u\r\n",
@@ -870,8 +896,7 @@ static void gps_process_acq_result(const gps_acq_result_t* result)
 
 	if(gps_tracking_prn_active(prn))
 	{
-		(void)delete_acq_element_by_prn_number(&acquisition_node, prn);
-		satellite_state[prn] = GPS_SAT_TRACKING;
+		gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
 		return;
 	}
 
@@ -962,8 +987,16 @@ static void gps_mark_node_fresh(volatile gps_ephem_ll_t* node, unsigned int stat
 	                              ((status & (1u << GPS_TRCK_STOP_UNLOCK_POS)) == 0u);
 	if((node->data_p != NULL) && gps_prn_valid(node->data_p->SVID))
 	{
-		satellite_state[node->data_p->SVID] = node->gps_measurement_valid ? GPS_SAT_TRACKING : GPS_SAT_STALE;
-		satellite_tracking_ip[node->data_p->SVID] = node->tracking_ip_no;
+		unsigned int prn = node->data_p->SVID;
+		if(node->gps_measurement_valid)
+		{
+			gps_mark_satellite_tracking(prn, node->tracking_ip_no);
+		}
+		else
+		{
+			gps_acq_manager_mark_stale(&gps_acq_manager, prn);
+			satellite_tracking_ip[prn] = node->tracking_ip_no;
+		}
 	}
 }
 
@@ -987,7 +1020,6 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
 
 	if(gps_prn_valid(dropped_prn))
 	{
-		(void)insert_acq_element(&acquisition_node, dropped_prn);
 		gps_mark_satellite_lost(dropped_prn);
 	}
 
@@ -1083,6 +1115,19 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
 	unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe,
 	                                           (gps_frame_t*)gps_frame_parsed,
 	                                           &how_tow_sec);
+	if((subframe_id >= 1u) && (subframe_id <= 3u) &&
+	   (gps_frame_parsed->pending_ephemeris_mask == 0u) &&
+	   gps_ephemeris_cache_store(
+		   &gps_ephemeris_cache, event->prn,
+		   (const gps_ephemeris_t*)&gps_frame_parsed->gps_ephems,
+		   event->tick))
+	{
+		gps_runtime_diagnostics.ephemeris_cache_store_total++;
+		GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_NAV,
+		        "stored ephemeris cache: prn=%u trk=%u iode=%u\r\n",
+		        event->prn, trck_ip_no,
+		        gps_frame_parsed->gps_ephems.iode);
+	}
 	if((subframe_id >= 1u) && (subframe_id <= 5u) &&
 	   (how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS))
 	{
@@ -1169,10 +1214,6 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
 	}
 
 	gps_mark_node_fresh(node, event->status, event->tick);
-	if(gps_prn_valid(event->prn))
-	{
-		gps_mark_satellite_tracking(event->prn, trck_ip_no);
-	}
 
 	GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK, "nav bits: trk=%u prn=%u mask=0x%x status=0x%x\r\n",
 	        trck_ip_no, event->prn, node->gps_subframe_mask, event->status);
@@ -1412,7 +1453,7 @@ static void gps_process_tracking_event(const gps_isr_event_t* event)
 		}
 		else if(gps_prn_valid(event->prn))
 		{
-			(void)insert_acq_element(&acquisition_node, event->prn);
+			gps_mark_satellite_lost(event->prn);
 			tracking_allocated[event->index] = false;
 			if(trck_ip_exists(&tracking_node, event->index))
 			{
@@ -1502,80 +1543,28 @@ static void gps_promote_acquired_satellites(void)
 {
 	for(unsigned int prn = 0 ; prn < GPS_PRN_COUNT ; prn++)
 	{
-		if((satellite_state[prn] == GPS_SAT_ACQUIRED) && !gps_tracking_prn_active(prn))
+		if((gps_acq_manager_state(&gps_acq_manager, prn) ==
+		    GPS_ACQ_PRN_ACQUIRED) &&
+		   !gps_tracking_prn_active(prn))
 		{
 			(void)assign_tracking_from_result(&satellite_acq_result[prn]);
 		}
 	}
 }
 
-static void gps_remove_tracked_prns_from_acq_list(void)
+static void gps_reconcile_tracked_prns(void)
 {
 	for(unsigned int prn = 0 ; prn < GPS_PRN_COUNT ; prn++)
 	{
-		if(gps_tracking_prn_active(prn))
+		if(gps_tracking_prn_active(prn) &&
+		   (gps_acq_manager_state(&gps_acq_manager, prn) !=
+		    GPS_ACQ_PRN_STALE))
 		{
-			if(acq_prn_exists(&acquisition_node, prn))
-			{
-				(void)delete_acq_element_by_prn_number(&acquisition_node, prn);
-			}
-			satellite_state[prn] = GPS_SAT_TRACKING;
+			gps_acq_manager_mark_tracking(&gps_acq_manager, prn);
 		}
 	}
 }
 
-static bool gps_prn_schedulable(unsigned int prn, uint64_t now)
-{
-	if(!gps_prn_valid(prn))
-	{
-		return false;
-	}
-	if((satellite_state[prn] == GPS_SAT_ACQUIRING) ||
-	   (satellite_state[prn] == GPS_SAT_ACQUIRED) ||
-	   gps_tracking_prn_active(prn))
-	{
-		return false;
-	}
-	if((satellite_last_acq_tick[prn] != 0u) &&
-	   ((now - satellite_last_acq_tick[prn]) < GPS_ACQ_RETRY_DELAY_TICKS))
-	{
-		return false;
-	}
-	return true;
-}
-
-static bool gps_find_next_acq_prn(uint64_t now, unsigned int* prn_out)
-{
-	gps_acq_ll_t* iterator = (gps_acq_ll_t*)acquisition_node;
-	gps_acq_ll_t* wrapped_candidate = NULL;
-	while(iterator != NULL)
-	{
-		unsigned int prn = iterator->prn_no;
-		if(gps_prn_schedulable(prn, now))
-		{
-			if(prn >= gps_acq_round_robin_prn)
-			{
-				*prn_out = prn;
-				gps_acq_round_robin_prn = (prn + 1u) % GPS_PRN_COUNT;
-				return true;
-			}
-			if(wrapped_candidate == NULL)
-			{
-				wrapped_candidate = iterator;
-			}
-		}
-		iterator = iterator->next;
-	}
-
-	if(wrapped_candidate != NULL)
-	{
-		*prn_out = wrapped_candidate->prn_no;
-		gps_acq_round_robin_prn = (*prn_out + 1u) % GPS_PRN_COUNT;
-		return true;
-	}
-	return false;
-}
-
 static bool gps_acq_engine_available(unsigned int engine_id, volatile gps_acq_t* acq_ip)
 {
 	if((engine_id >= GPS_ACQ_IP_COUNT) || (acq_ip == NULL))
@@ -1604,7 +1593,7 @@ static bool gps_acq_engine_available(unsigned int engine_id, volatile gps_acq_t*
 void gps_acq_scheduler_run(void)
 {
 	uint64_t now = gps_get_systick();
-	gps_remove_tracked_prns_from_acq_list();
+	gps_reconcile_tracked_prns();
 	gps_promote_acquired_satellites();
 
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
@@ -1620,7 +1609,9 @@ void gps_acq_scheduler_run(void)
 			continue;
 		}
 		unsigned int prn;
-		if(!gps_find_next_acq_prn(now, &prn))
+		bool hot = false;
+		if(!gps_acq_manager_start_next(
+		       &gps_acq_manager, now, &prn, &hot))
 		{
 			continue;
 		}
@@ -1629,10 +1620,10 @@ void gps_acq_scheduler_run(void)
 		acq_engine_prn[i] = (unsigned char)prn;
 		acq_engine_start_tick[i] = now;
 		acq_engine_stuck_warned[i] = false;
-		satellite_state[prn] = GPS_SAT_ACQUIRING;
-		satellite_last_acq_tick[prn] = now;
 		acq_start(acq_ip, (unsigned char)prn);
-		GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_SCHED, "satellite search started: engine=%u prn=%u\r\n", i, prn);
+		GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_SCHED,
+		        "satellite search started: engine=%u prn=%u priority=%s\r\n",
+		        i, prn, hot ? "reacquire" : "cold");
 	}
 }
 
@@ -1641,17 +1632,12 @@ static void gps_check_acq_stuck_busy(void)
 	uint64_t now = gps_get_systick();
 	for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
 	{
-		if((acq_engine_state[i] != GPS_ACQ_ENGINE_BUSY) || acq_engine_stuck_warned[i])
+		if(acq_engine_state[i] != GPS_ACQ_ENGINE_BUSY)
 		{
 			continue;
 		}
 
 		uint64_t age = now - acq_engine_start_tick[i];
-		if(age <= GPS_ACQ_STUCK_WARN_TICKS)
-		{
-			continue;
-		}
-
 		volatile gps_acq_t* acq_ip = gps_get_acq_ip(i);
 		if(acq_ip == NULL)
 		{
@@ -1660,15 +1646,68 @@ static void gps_check_acq_stuck_busy(void)
 
 		uint32_t control = acq_ip->CONTROL_REG;
 		uint32_t status = acq_ip->STATUS_REG;
-		acq_engine_stuck_warned[i] = true;
-		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_ACQ,
-		        "ACQ engine %u busy for %lu ticks prn=%u status=0x%x control=0x%x; check rf_clk/sample_align/acq_end\r\n",
-		        i, (unsigned long)age, acq_engine_prn[i], status, control);
+		if((age >= GPS_ACQ_STUCK_WARN_TICKS) &&
+		   !acq_engine_stuck_warned[i])
+		{
+			acq_engine_stuck_warned[i] = true;
+			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_ACQ,
+			        "ACQ engine %u busy for %lu ticks prn=%u status=0x%x control=0x%x; check rf_clk/sample_align/acq_end\r\n",
+			        i, (unsigned long)age, acq_engine_prn[i],
+			        status, control);
+		}
 		(void)status;
 		(void)control;
 	}
 }
 
+static void gps_log_acq_status(void)
+{
+	uint64_t now = gps_get_systick();
+	if((gps_last_acq_status_log_tick != 0u) &&
+	   ((now - gps_last_acq_status_log_tick) <
+	    GPS_ACQ_STATUS_LOG_PERIOD_TICKS))
+	{
+		return;
+	}
+	gps_last_acq_status_log_tick = now;
+
+	unsigned int engine = GPS_PRN_INVALID;
+	unsigned int engine_prn = GPS_PRN_INVALID;
+	uint64_t engine_age_ticks = 0u;
+	for(unsigned int i = 0u ; i < GPS_ACQ_IP_COUNT ; i++)
+	{
+		if(acq_engine_state[i] == GPS_ACQ_ENGINE_BUSY)
+		{
+			engine = i;
+			engine_prn = acq_engine_prn[i];
+			engine_age_ticks = now - acq_engine_start_tick[i];
+			break;
+		}
+	}
+
+	unsigned int tracking = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_TRACKING);
+	unsigned int stale = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_STALE);
+	unsigned int hot = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_HOT);
+	unsigned int cold = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_COLD);
+	unsigned int acquiring = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_ACQUIRING);
+	unsigned int acquired = gps_acq_manager_count_state(
+		&gps_acq_manager, GPS_ACQ_PRN_ACQUIRED);
+
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_ACQ,
+	        "state=%s tracked=%u stale=%u hot=%u cold=%u acquiring=%u acquired=%u engine=%u prn=%u age_ms=%lu\r\n",
+	        (tracking >= GPS_NAV_MIN_WLS_SATELLITES)
+			? "tracking" : "searching",
+	        tracking, stale, hot, cold, acquiring, acquired,
+	        engine, engine_prn,
+	        (unsigned long)((engine_age_ticks * 1000u) /
+	                        GPS_SYSTICK_HZ));
+}
+
 static void gps_mark_stale_measurements(void)
 {
 	uint64_t now = gps_get_systick();
@@ -1681,7 +1720,9 @@ static void gps_mark_stale_measurements(void)
 			iterator->gps_measurement_valid = false;
 			if(iterator->data_p != NULL && gps_prn_valid(iterator->data_p->SVID))
 			{
-				satellite_state[iterator->data_p->SVID] = GPS_SAT_STALE;
+				gps_acq_manager_mark_stale(
+					&gps_acq_manager,
+					iterator->data_p->SVID);
 			}
 			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "stale measurement: trk=%u age_ticks=%lu\r\n",
 			        iterator->tracking_ip_no, (unsigned long)(now - iterator->gps_last_measurement_tick));
@@ -1699,6 +1740,7 @@ void gps_background_task(void)
 	{
 		gps_mark_stale_measurements();
 		gps_check_acq_stuck_busy();
+		gps_log_acq_status();
 	}
 	if(gps_take_acq_schedule_pending())
 	{

soc/software/gps_system/src/gps/gps_ip.c

@@ -57,7 +57,6 @@ int unsigned gps_subframes[GPS_TRCK_IP_COUNT][10];
 volatile gps_frame_t gps_frames_parsed[GPS_TRCK_IP_COUNT];
 
 volatile gps_ephem_ll_t* tracking_node = NULL;
-volatile gps_acq_ll_t* acquisition_node = NULL;
 
 static volatile bool gps_initial_position_valid = false;
 static volatile bool gps_ionoutc_valid = false;
@@ -1405,12 +1404,6 @@ void gps_startup_initialization(unsigned int nsp_tick)
 
 	unsigned char gps_active_satellites[] = GPS_ACTIVE_PRNS;
 
-	for(unsigned char i = 0; i < GPS_ACTIVE_SAT_NUM ; i++)
-	{
-		unsigned int prn = (unsigned int) gps_active_satellites[i];
-		(void)insert_acq_element(&acquisition_node, prn);
-	}
-
 	for(unsigned i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
 	{
 		// stop all working tracking ips & disable interrupts.
@@ -1436,6 +1429,9 @@ void gps_startup_initialization(unsigned int nsp_tick)
 				}
 
 	gps_interrupt_init();
+	gps_acq_configure_prns(
+		gps_active_satellites,
+		GPS_ACTIVE_SAT_NUM);
 	/* The AFE and its sample timebase are shared by all acquisition/tracking
 	 * channels. Start them exactly once; per-PRN acquisition must not rephase it. */
 	acq_afe_start(acq_ips[0]);

soc/software/gps_system/src/gps/linked_list.c

@@ -127,82 +127,3 @@ bool delete_by_ip_number(volatile gps_ephem_ll_t** node, unsigned char trck_ip_n
 	}
 	return false;
 }
-
-bool acq_prn_exists(volatile gps_acq_ll_t** node, unsigned int prn_no)
-{
-	if(node == NULL)
-	{
-		return false;
-	}
-
-	volatile gps_acq_ll_t* iterator = *node;
-	while(iterator != NULL)
-	{
-		if(iterator->prn_no == prn_no)
-		{
-			return true;
-		}
-		iterator = iterator->next;
-	}
-	return false;
-}
-
-bool insert_acq_element(volatile gps_acq_ll_t** node, unsigned int prn_no)
-{
-	if((node == NULL) || acq_prn_exists(node, prn_no))
-	{
-		return false;
-	}
-
-	gps_acq_ll_t* new_node = (gps_acq_ll_t*)malloc(sizeof(*new_node));
-	if(new_node == NULL)
-	{
-		return false;
-	}
-	new_node->prn_no = prn_no;
-	new_node->next = NULL;
-
-	if(*node == NULL)
-	{
-		*node = new_node;
-		return true;
-	}
-
-	volatile gps_acq_ll_t* tail = *node;
-	while(tail->next != NULL)
-	{
-		tail = tail->next;
-	}
-	tail->next = new_node;
-	return true;
-}
-
-bool delete_acq_element_by_prn_number(volatile gps_acq_ll_t** node, unsigned int prn_no)
-{
-	if(node == NULL)
-	{
-		return false;
-	}
-
-	volatile gps_acq_ll_t* iterator = *node;
-	volatile gps_acq_ll_t* previous = NULL;
-	while(iterator != NULL)
-	{
-		if(iterator->prn_no == prn_no)
-		{
-			if(previous == NULL)
-			{
-				*node = iterator->next;
-			}
-			else
-			{
-				previous->next = iterator->next;
-			}
-			free((void*)iterator);
-			return true;
-		}
-		previous = iterator;
-		iterator = iterator->next;
-	}
-	return false;
-}