Rate limit corrected UART telemetry

soc/software/Makefile

@@ -13,7 +13,10 @@ GPS_DEBUG ?= 1
 GPS_DEBUG_LEVEL ?= 4
 BSP_UART_CLOCK_HZ ?= 100000000
 BSP_UART_BAUDRATE ?= 115200
+GPS_SYSTEM_UART_BAUDRATE ?= 460800
 BSP_CLINT_TIMEBASE_HZ ?= 1000000
+GPS_SYSTEM_CLINT_TIMEBASE_HZ ?= 500000
+GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES ?= 5
 # BSP_DTIM_* mirrors the FPGA DTIM_* knobs and is used by C code plus linker scripts.
 BSP_DTIM_BASE ?= 0x80000000
 BSP_DTIM_SIZE ?= 0x00100000
@@ -38,7 +41,10 @@ help:
 	@echo "  APP_FLASH_OFFSET=$(APP_FLASH_OFFSET)"
 	@echo "  BSP_UART_CLOCK_HZ=$(BSP_UART_CLOCK_HZ)"
 	@echo "  BSP_UART_BAUDRATE=$(BSP_UART_BAUDRATE)"
+	@echo "  GPS_SYSTEM_UART_BAUDRATE=$(GPS_SYSTEM_UART_BAUDRATE)"
 	@echo "  BSP_CLINT_TIMEBASE_HZ=$(BSP_CLINT_TIMEBASE_HZ)"
+	@echo "  GPS_SYSTEM_CLINT_TIMEBASE_HZ=$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)"
+	@echo "  GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)"
 	@echo "  BSP_DTIM_BASE=$(BSP_DTIM_BASE)"
 	@echo "  BSP_DTIM_SIZE=$(BSP_DTIM_SIZE)"
 
@@ -57,7 +63,7 @@ steval-mki217v1-test:
 	@make -C steval_mki217v1_test TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/steval_mki217v1_test" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
 
 gps-system:
-	@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(BSP_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(BSP_CLINT_TIMEBASE_HZ)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
+	@make -C gps_system TOOLCHAIN="$(TOOLCHAIN)" BUILD_DIR="$(OUT_DIR)/gps_system" GPS_DEBUG="$(GPS_DEBUG)" GPS_DEBUG_LEVEL="$(GPS_DEBUG_LEVEL)" BSP_UART_CLOCK_HZ="$(BSP_UART_CLOCK_HZ)" BSP_UART_BAUDRATE="$(GPS_SYSTEM_UART_BAUDRATE)" BSP_CLINT_TIMEBASE_HZ="$(GPS_SYSTEM_CLINT_TIMEBASE_HZ)" IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES="$(GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES)" BSP_DTIM_BASE="$(BSP_DTIM_BASE)" BSP_DTIM_SIZE="$(BSP_DTIM_SIZE)" all
 
 app:
 	@test -d "$(APP)"

soc/software/fusion/tests/platform_scenario_matrix_test.c

@@ -474,14 +474,14 @@ static void print_metrics(scenario_kind_t scenario, unsigned int gps_ms,
 static int run_matrix(void)
 {
 	static const unsigned int gps_periods_ms[] = {200u, 500u, 1000u};
-	static const unsigned int imu_periods_ms[] = {19u, 20u, 200u, 300u};
+	static const unsigned int imu_periods_ms[] = {19u, 20u, 100u, 200u, 300u};
 	bool passed = true;
 	printf("Four-satellite asynchronous platform matrix; 3 s outage and 2 s coherent GNSS fault\n");
 	for(unsigned int scenario = 0u; scenario < SCENARIO_COUNT; scenario++)
 	{
 		for(unsigned int gps_index = 0u; gps_index < 3u; gps_index++)
 		{
-			for(unsigned int imu_index = 0u; imu_index < 4u; imu_index++)
+			for(unsigned int imu_index = 0u; imu_index < 5u; imu_index++)
 			{
 				metrics_t metrics = run_case((scenario_kind_t)scenario,
 					gps_periods_ms[gps_index], imu_periods_ms[imu_index], NULL);
@@ -504,7 +504,7 @@ static int run_matrix(void)
 		fprintf(stderr, "FAIL: platform scenario matrix contract.\n");
 		return 1;
 	}
-	printf("PASS: 19/20 ms production rates stayed bounded; 200/300 ms coarse IMU "
+	printf("PASS: 19/20 ms production rates stayed bounded; 100/200/300 ms coarse IMU "
 	       "rates remained finite and their dynamic accuracy loss is reported, not hidden.\n");
 	return 0;
 }
@@ -512,6 +512,33 @@ static int run_matrix(void)
 int main(int argc, char** argv)
 {
 	if(argc == 1) return run_matrix();
+	if((argc == 6) && (strcmp(argv[1], "--benchmark") == 0))
+	{
+		scenario_kind_t scenario;
+		if(!parse_scenario(argv[2], &scenario))
+		{
+			fprintf(stderr, "unknown scenario: %s\n", argv[2]);
+			return 2;
+		}
+		unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
+		unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
+		unsigned int iterations = (unsigned int)strtoul(argv[5], NULL, 10);
+		if((iterations == 0u) ||
+		   !((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
+		   !((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
+		      (imu_ms == 200u) || (imu_ms == 300u)))
+		{
+			fprintf(stderr, "supported GPS: 200/500/1000 ms; "
+			                "IMU: 19/20/100/200/300 ms; iterations > 0\n");
+			return 2;
+		}
+		metrics_t metrics;
+		for(unsigned int iteration = 0u; iteration < iterations; iteration++)
+			metrics = run_case(scenario, gps_ms, imu_ms, NULL);
+		print_metrics(scenario, gps_ms, imu_ms, &metrics);
+		printf("completed %u benchmark iterations\n", iterations);
+		return metrics.finite ? 0 : 1;
+	}
 	if((argc == 6) && (strcmp(argv[1], "--log") == 0))
 	{
 		scenario_kind_t scenario;
@@ -523,10 +550,10 @@ int main(int argc, char** argv)
 		unsigned int gps_ms = (unsigned int)strtoul(argv[3], NULL, 10);
 		unsigned int imu_ms = (unsigned int)strtoul(argv[4], NULL, 10);
 		if(!((gps_ms == 200u) || (gps_ms == 500u) || (gps_ms == 1000u)) ||
-		   !((imu_ms == 19u) || (imu_ms == 20u) ||
+		   !((imu_ms == 19u) || (imu_ms == 20u) || (imu_ms == 100u) ||
 		      (imu_ms == 200u) || (imu_ms == 300u)))
 		{
-			fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/200/300 ms\n");
+			fprintf(stderr, "supported GPS: 200/500/1000 ms; IMU: 19/20/100/200/300 ms\n");
 			return 2;
 		}
 		FILE* log_file = fopen(argv[5], "w");
@@ -546,6 +573,9 @@ int main(int argc, char** argv)
 		return metrics.finite ? 0 : 1;
 	}
 	fprintf(stderr, "usage: %s [--log stationary|pedestrian|car|aircraft "
-	                "200|500|1000 19|20|200|300 output.log]\n", argv[0]);
+	                "200|500|1000 19|20|100|200|300 output.log]\n"
+	                "       %s [--benchmark stationary|pedestrian|car|aircraft "
+	                "200|500|1000 19|20|100|200|300 iterations]\n",
+	        argv[0], argv[0]);
 	return 2;
 }

soc/software/gps_system/Makefile

@@ -8,9 +8,11 @@ GPS_DEBUG ?= 1
 GPS_DEBUG_LEVEL ?= 4
 
 # 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.
+# The 50 Hz fused-location stream plus GPS diagnostics does not fit safely in
+# the polling UART budget at 115200 baud.
 BSP_UART_CLOCK_HZ ?= 50000000
+BSP_UART_BAUDRATE ?= 460800
+BSP_CLINT_TIMEBASE_HZ ?= 500000
 
 # Select one sensor backend, or keep the default read-only WHO_AM_I probe.
 # Accepted values: auto, tdk, st (stm is an alias for st).
@@ -42,6 +44,9 @@ endif
 IMU_ACCEL_FS_G ?= 0
 IMU_GYRO_FS_DPS ?= 0
 GPS_ESKF_ENABLE ?= 1
+# UART telemetry cadence only; ESKF prediction still runs for every IMU sample.
+# At the default 50 Hz ODR, 5 samples produce one corrected record every 100 ms.
+IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES ?= 5
 
 APP_SRCS := \
 	gps_system.c \
@@ -79,6 +84,7 @@ CFLAGS += $(IMU_BACKEND_CFLAG) \
 	-DIMU_ACCEL_FS_G=$(IMU_ACCEL_FS_G) \
 	-DIMU_GYRO_FS_DPS=$(IMU_GYRO_FS_DPS) \
 	-DGPS_ESKF_ENABLE=$(GPS_ESKF_ENABLE) \
+	-DIMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=$(IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES) \
 	-DGPS_NAV_SOLVE_TIMER_TICKS=$(GPS_NAV_SOLVE_TIMER_TICKS)
 LDLIBS += -lm
 

soc/software/gps_system/README.md

@@ -198,9 +198,9 @@ bagimsiz yuksek-ODR zamanlama gerekirse sensor FIFO timestamp'i kullanilmalidir.
 
 ### Level-1 konum UART protokolu
 
-Her gecerli GPS navigation epoch'unda level 1'de iki konum satiri art arda
-yayinlanir. Ilk satir WLS GPS konumu, ikinci satir o anda mevcut IMU+ESKF
-duzeltilmis konumdur:
+Her gecerli GPS navigation epoch'unda ham WLS GPS konumu yayinlanir. ESKF
+duzeltilmis konumu ise GPS epoch'undan bagimsiz olarak IMU ornek sayaci
+uzerinden yayinlanir:
 
 ```text
 [LOCATION] source=gps lat=... lon=... height=... gdop=... pdop=... hdop=... vdop=... tdop=... tracked=... used=...
@@ -208,10 +208,28 @@ duzeltilmis konumdur:
 ```
 
 `tracked`, donanimda calisan tracking kanal sayisini; `used`, ilgili WLS
-cozumunde kullanilan uydu sayisini belirtir. DOP alanlari ayni GPS epoch'unun
-geometrisidir ve iki satirda ayni tasinir. ESKF henuz kalibrasyon/origin
-asamasindaysa yalniz GPS satiri yayinlanir; ESKF valid olduktan sonra her GPS
-konumu geldiginde iki satir da yayinlanir. Eski `[NAV] position` ve periyodik
+cozumunde kullanilan uydu sayisini belirtir. ESKF henuz kalibrasyon/origin
+asamasindaysa yalniz GPS satiri yayinlanir. Origin kurulduktan sonra GPS
+reddedilse veya gecici olarak kesilse de corrected konum IMU ile ilerlemeye
+devam eder.
+
+Varsayilan corrected UART periyodu bes IMU ornegidir. 50 Hz sensor hedefinde
+bu yaklasik 100 ms/10 Hz demektir. Bu bolucu yalniz UART logunu seyreltir;
+sensor okuma ve ESKF prediction her IMU orneginde calismaya devam eder:
+
+```sh
+# Varsayilan: 20 ms x 5 ~= 100 ms
+make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=5
+
+# 20 ms x 2 ~= 40 ms
+make -C soc/software/gps_system IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES=2
+
+# Parent software hedefi uzerinden ayni ayar
+make -C soc/software gps-system GPS_SYSTEM_IMU_PUBLISH_EVERY_N_SAMPLES=5
+```
+
+Deger en az `1` olmalidir. Secilen bolucu startup satirinda
+`publish_every=` alaniyla gorulur. Eski `[NAV] position` ve periyodik
 `[FUSION] lat/lon` ayrintilari daha yuksek debug seviyesindedir; level 1
 parser'inin ayni konumu birden fazla kez gormesi engellenir.
 

soc/software/gps_system/gps_system.c

@@ -18,6 +18,17 @@
 #error "GPS_ESKF_ENABLE must be 0 or 1"
 #endif
 
+#ifndef IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES
+#define IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES 5u
+#endif
+
+#if (IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES < 1)
+#error "IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES must be at least one"
+#endif
+
+#define IMU_SAMPLES_BEFORE_GPS_SERVICE 1u
+#define IMU_SAMPLES_AFTER_GPS_SERVICE  3u
+
 static void initialize_sensor_fusion(gps_system_state_t* system)
 {
 	system->fusion_enabled = (GPS_ESKF_ENABLE != 0);
@@ -43,18 +54,42 @@ static void initialize_sensor_fusion(gps_system_state_t* system)
 			true); /* Current prototype is the PX4-equivalent unarmed bench mode. */
 }
 
-static unsigned int feed_pending_imu_to_eskf(gps_system_state_t* system)
+static unsigned int feed_pending_imu_to_eskf(
+	gps_system_state_t* system,
+	unsigned int max_samples)
 {
-	if(!system->fusion_initialized)
+	if(!system->fusion_initialized || (max_samples == 0u))
 	{
 		return 0u;
 	}
 
 	unsigned int processed = 0u;
 	imu_sensor_service();
-	while(imu_sensor_take_measurement(&system->imu_sample))
+	while((processed < max_samples) &&
+	      imu_sensor_take_measurement(&system->imu_sample))
 	{
-		(void)navigation_fusion_push_imu(&system->imu_sample);
+		bool accepted = navigation_fusion_push_imu(&system->imu_sample);
+		if(accepted &&
+		   navigation_fusion_get_output(&system->fused_solution) &&
+		   system->fused_solution.global_valid &&
+		   (system->fused_solution.timestamp_ticks !=
+		    system->last_fused_location_tick))
+		{
+			system->fused_publish_sample_count++;
+			bool first_location =
+				(system->last_fused_location_tick == 0u);
+			bool publish_due =
+				(system->fused_publish_sample_count >=
+				 IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);
+			if(first_location || publish_due)
+			{
+				gps_system_uart_publish_fused_location(
+					&system->fused_solution);
+				system->last_fused_location_tick =
+					system->fused_solution.timestamp_ticks;
+				system->fused_publish_sample_count = 0u;
+			}
+		}
 		processed++;
 	}
 	return processed;
@@ -76,23 +111,36 @@ int main(void)
 	gps_receiver_start_acquisition();
 	gps_system_uart_publish_startup(
 		system.fusion_enabled, system.fusion_initialized,
-		system.imu_initialized, system.imu_running);
+		system.imu_initialized, system.imu_running,
+		IMU_CORRECTED_PUBLISH_EVERY_N_SAMPLES);
 
+	bool imu_backlog_may_remain = false;
 	for(;;)
+	{
+		/* Skip WFI only while draining a bounded IMU backlog.  Every batch
+		 * still services GPS before more prediction work, so a continuously
+		 * non-empty IMU queue cannot starve navigation epochs. */
+		if(!imu_backlog_may_remain)
 		{
 			/* GPS, IMU data-ready and the maintenance timer wake this loop. */
 			asm volatile("wfi");
+		}
 
-		/* IMU prediction runs at the sensor's own timestamped rate. */
-		(void)feed_pending_imu_to_eskf(&system);
+		/* Limit pre-GPS work to one sample for low event latency. */
+		(void)feed_pending_imu_to_eskf(
+			&system, IMU_SAMPLES_BEFORE_GPS_SERVICE);
 
 		/* GPS remains an independent receiver: events first, then coherent WLS. */
 		gps_receiver_process_pending_events();
 		system.gps_epoch_ready = gps_receiver_solve_ready_epoch(
 			&system.gps_solution, &system.gps_measurement_ticks);
 
-		/* WLS may take long enough for more IMU samples to arrive. */
-		(void)feed_pending_imu_to_eskf(&system);
+		/* WLS may take long enough for more IMU samples to arrive.  Drain a
+		 * small batch, then return to GPS service before continuing. */
+		unsigned int imu_after_gps = feed_pending_imu_to_eskf(
+			&system, IMU_SAMPLES_AFTER_GPS_SERVICE);
+		imu_backlog_may_remain =
+			(imu_after_gps == IMU_SAMPLES_AFTER_GPS_SERVICE);
 
 		if(system.gps_epoch_ready && system.gps_solution.solution_valid)
 		{
@@ -114,15 +162,6 @@ int main(void)
 				/* Delayed GPS update is applied at its capture timestamp; newer
 				 * IMU history is replayed to produce the current fused state. */
 				(void)navigation_fusion_push_gps(&system.gps_for_fusion);
-				if(navigation_fusion_get_output(&system.fused_solution) &&
-				   system.fused_solution.global_valid)
-				{
-					gps_system_uart_publish_location("imu_corrected",
-					                 system.fused_solution.latitude_deg,
-					                 system.fused_solution.longitude_deg,
-					                 system.fused_solution.height_m,
-					                 &system.gps_solution);
-				}
 			}
 		}
 

soc/software/gps_system/header/adapters/gps_system_uart.h

@@ -14,11 +14,15 @@ void gps_system_uart_publish_location(
 	double height_m,
 	const gps_nav_solution_t* gps);
 
+void gps_system_uart_publish_fused_location(
+	const navigation_fusion_output_t* output);
+
 void gps_system_uart_publish_startup(
 	bool fusion_enabled,
 	bool fusion_initialized,
 	bool imu_initialized,
-	bool imu_running);
+	bool imu_running,
+	unsigned int fused_publish_every_samples);
 
 void gps_system_uart_publish_periodic_fusion_status(
 	navigation_fusion_output_t* output,

soc/software/gps_system/header/fusion/navigation_fusion.h

@@ -11,6 +11,7 @@
 
 typedef struct {
 	uint64_t timestamp_ticks;
+	uint64_t last_accepted_gps_ticks;
 	double position_enu_m[3];
 	double velocity_enu_mps[3];
 	double latitude_deg;

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

@@ -43,7 +43,9 @@
 		#define GPS_ACQ_IP_BASE_ADDR(index) (BSP_GPS_ACQ_BASE + ((uintptr_t)(index) * BSP_GPS_ACQ_STRIDE))
 		#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
+			/* Genesys2 CLINT is 500 kHz.  Keep foreground maintenance at
+			 * 10 Hz; the tracking IP's 20 ms NAV timer is independent. */
+			#define GPS_SYSTICK_HZ              10u
 			#define GPS_NAV_SOLVE_TIMER_TICK_MS 20u
 			#ifndef GPS_NAV_SOLVE_TIMER_TICKS
 			#define GPS_NAV_SOLVE_TIMER_TICKS   50u

soc/software/gps_system/header/gps_system.h

@@ -17,7 +17,9 @@ typedef struct {
 	imu_sample_t imu_sample;
 	navigation_fusion_output_t fused_solution;
 	uint64_t gps_measurement_ticks;
+	uint64_t last_fused_location_tick;
 	uint64_t last_status_tick;
+	uint32_t fused_publish_sample_count;
 	bool gps_epoch_ready;
 	bool fusion_enabled;
 	bool imu_initialized;

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

@@ -7,6 +7,8 @@
 #include "gps/gps_receiver.h"
 #include "imu/imu_sensor.h"
 
+#define GPS_AIDED_MAX_AGE_MS 1000u
+
 void gps_system_uart_publish_location(
 	const char* source,
 	double latitude_deg,
@@ -27,17 +29,46 @@ void gps_system_uart_publish_location(
 	        gps->tracked_satellite_count, gps->used_satellite_count);
 }
 
+void gps_system_uart_publish_fused_location(
+	const navigation_fusion_output_t* output)
+{
+	if((output == NULL) || !output->global_valid)
+	{
+		return;
+	}
+
+	uint64_t age_ticks = output->timestamp_ticks >= output->last_accepted_gps_ticks
+		? output->timestamp_ticks - output->last_accepted_gps_ticks : 0u;
+	uint64_t age_ms_wide =
+		age_ticks / (BSP_CLINT_TIMEBASE_HZ / 1000u);
+	uint32_t age_ms = age_ms_wide > UINT32_MAX
+		? UINT32_MAX : (uint32_t)age_ms_wide;
+	const char* mode =
+		(output->gps_accepted > 0u) && (age_ms <= GPS_AIDED_MAX_AGE_MS)
+		? "gps_aided" : "dead_reckoning";
+
+	/* The application rate-limits this compact record by IMU sample count,
+	 * leaving UART headroom without reducing the ESKF prediction rate. */
+	GPS_LOG(DEBUG_STATUS, GPS_LOG_CAT_LOCATION,
+	        "source=imu_corrected mode=%s lat=%.9lf lon=%.9lf "
+	        "height=%.2lf gps_age_ms=%lu\r\n",
+	        mode, output->latitude_deg, output->longitude_deg,
+	        output->height_m, (unsigned long)age_ms);
+}
+
 void gps_system_uart_publish_startup(
 	bool fusion_enabled,
 	bool fusion_initialized,
 	bool imu_initialized,
-	bool imu_running)
+	bool imu_running,
+	unsigned int fused_publish_every_samples)
 {
 	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 "
-	        "running=%u target_hz=%lu actual_hz=%lu nav_ms=%u timebase_hz=%lu\r\n",
+	        "running=%u target_hz=%lu actual_hz=%lu publish_every=%u "
+	        "nav_ms=%u timebase_hz=%lu\r\n",
 	        fusion_enabled ? 1u : 0u,
 	        fusion_initialized ? 1u : 0u,
 	        imu_initialized ? 1u : 0u,
@@ -46,6 +77,7 @@ void gps_system_uart_publish_startup(
 	        imu_running ? 1u : 0u,
 	        (unsigned long)imu.requested_odr_hz,
 	        (unsigned long)imu.configured_odr_hz,
+	        fused_publish_every_samples,
 	        GPS_NAV_SOLVE_PERIOD_MS,
 	        (unsigned long)BSP_CLINT_TIMEBASE_HZ);
 }

soc/software/gps_system/src/fusion/navigation_fusion.c

@@ -123,6 +123,8 @@ bool navigation_fusion_get_output(navigation_fusion_output_t* output)
 	}
 	const fusion_runtime_t* runtime = &fusion_engine.current;
 	output->timestamp_ticks = runtime->filter_timestamp_ticks;
+	output->last_accepted_gps_ticks =
+		runtime->last_accepted_position_ticks;
 	memcpy(output->position_enu_m, runtime->filter.position_m,
 	       sizeof(output->position_enu_m));
 	memcpy(output->velocity_enu_mps, runtime->filter.velocity_mps,

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

@@ -114,9 +114,10 @@ static uint32_t gps_nav_ready_channel_mask(void)
 		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 locked = (status & (1u << GPS_TRCK_LOCK_POS)) != 0u;
 		bool nav_irq_enabled =
 			(control & (1u << GPS_TRCK_NAV_SOLVE_INTR_EN_POS)) != 0u;
-		if(tracking_allocated[i] && busy && nav_irq_enabled)
+		if(tracking_allocated[i] && busy && locked && nav_irq_enabled)
 		{
 			ready_mask |= 1u << i;
 		}

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

@@ -579,13 +579,13 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
 	}
 }
 
-static unsigned int gps_collect_busy_tracking_locked(
+static unsigned int gps_collect_tracked_channels_locked(
 	unsigned int tracked_prns[GPS_TRCK_IP_COUNT])
 {
 	unsigned int count = 0;
 	for(unsigned char i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
 	{
-		if(trck_ips[i]->STATUS_REG & (1u << GPS_TRCK_BUSY_POS))
+		if(gps_tracking_ip_alive(i))
 		{
 			if(tracked_prns != NULL)
 			{
@@ -1099,11 +1099,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 	bool run_nav = false;
 	bool initial_run = false;
 	unsigned int ready_count;
-	unsigned int busy_count;
+	unsigned int tracked_count;
 	unsigned int tracked_prns[GPS_TRCK_IP_COUNT];
 	unsigned int coherent_nav_count = 0;
 	static uint64_t last_nav_status_log_tick = 0;
-	static unsigned int last_nav_status_busy_count = GPS_TRCK_IP_COUNT + 1u;
+	static unsigned int last_nav_status_tracked_count = GPS_TRCK_IP_COUNT + 1u;
 	static unsigned int last_nav_status_ready_count = GPS_TRCK_IP_COUNT + 1u;
 	static unsigned int last_nav_status_coherent_count = GPS_TRCK_IP_COUNT + 1u;
 
@@ -1117,7 +1117,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 
 	ready_count = gps_collect_nav_snapshot_locked(
 		snapshot, snapshot_ips, NULL, false, false);
-	busy_count = gps_collect_busy_tracking_locked(
+	tracked_count = gps_collect_tracked_channels_locked(
 		GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
 
 	if(ready_count < MIN_NEEDED_SATELLITE)
@@ -1197,19 +1197,19 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 	}
 
 	uint64_t now = gps_get_systick();
-	if((busy_count != last_nav_status_busy_count) ||
+	if((tracked_count != last_nav_status_tracked_count) ||
 	   (ready_count != last_nav_status_ready_count) ||
 	   (coherent_nav_count != last_nav_status_coherent_count) ||
 	   ((now - last_nav_status_log_tick) >= GPS_NAV_STATUS_LOG_PERIOD_TICKS))
 	{
-		GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "busy channels=%u ready channels=%u coherent_nav=%u\r\n",
-		        busy_count, ready_count, coherent_nav_count);
-		last_nav_status_busy_count = busy_count;
+		GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV, "tracked channels=%u ready channels=%u coherent_nav=%u\r\n",
+		        tracked_count, ready_count, coherent_nav_count);
+		last_nav_status_tracked_count = tracked_count;
 		last_nav_status_ready_count = ready_count;
 		last_nav_status_coherent_count = coherent_nav_count;
 		last_nav_status_log_tick = now;
 	}
-	(void)busy_count;
+	(void)tracked_count;
 
 	if(run_nav)
 	{
@@ -1231,7 +1231,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 		uint64_t nav_software_cycles = gps_read_cycle_counter() - nav_software_start_cycles;
 		if(nav_ok)
 		{
-			nav_solution.tracked_satellite_count = busy_count;
+			nav_solution.tracked_satellite_count = tracked_count;
 			uint64_t measurement_timestamp = gps_nav_median_local_timestamp(
 				snapshot_local_timestamps, satellite_count);
 			if(measurement_timestamp != 0u)
@@ -1304,8 +1304,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
 			if(GPS_DEBUG_ENABLED(DEBUG_CHANNELS))
 			{
 				GPS_LOG(DEBUG_CHANNELS, GPS_LOG_CAT_TRK,
-				        "tracked satellites=%u prns=[", busy_count);
-				for(unsigned int i = 0 ; i < busy_count ; i++)
+				        "tracked satellites=%u prns=[", tracked_count);
+				for(unsigned int i = 0 ; i < tracked_count ; i++)
 				{
 					PRINT_DEBUG(DEBUG_CHANNELS, "%s%u",
 					            (i == 0u) ? "" : ",", tracked_prns[i]);

soc/software/gps_system/src/imu/imu_sensor.c

@@ -216,12 +216,13 @@ bool imu_sensor_init(const imu_sensor_config_t* config)
 #elif defined(IMU_SENSOR_BUILD_ST)
 	return try_st(config);
 #else
-	/* WHO_AM_I probing is read-only until a matching backend is found. */
-	if(try_tdk(config))
+	/* WHO_AM_I probing is read-only until a matching backend is found.
+	 * Prefer the Genesys2 STEVAL/LSM6DSOX path when both devices respond. */
+	if(try_st(config))
 	{
 		return true;
 	}
-	return try_st(config);
+	return try_tdk(config);
 #endif
 }
 

soc/software/gps_system/tests/test_gps_nav_epoch.c

@@ -83,6 +83,36 @@ static void assembly_waits_for_every_ready_channel(void)
 	      "assembly lost a ready-channel interrupt");
 }
 
+static void busy_unlocked_channel_does_not_block_epoch(void)
+{
+	gps_nav_epoch_assembly_t assembly;
+	gps_nav_epoch_assembly_reset(&assembly);
+
+	/* Model the board failure: channels 0..8 are allocated and busy, but
+	 * channel 4 has lost lock and therefore cannot produce this NAV epoch. */
+	const uint32_t allocated_busy_mask = 0x1ffu;
+	const uint32_t locked_mask = allocated_busy_mask & ~(1u << 4);
+	const uint32_t ready_mask = allocated_busy_mask & locked_mask;
+
+	for(unsigned int channel = 0u ; channel < 9u ; channel++)
+	{
+		if((ready_mask & (1u << channel)) == 0u)
+		{
+			continue;
+		}
+		gps_nav_epoch_event_result_t result =
+			gps_nav_epoch_assembly_push(
+				&assembly, 1500u, channel, ready_mask);
+		bool last = channel == 8u;
+		CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
+		      "busy unlocked channel blocked a complete locked-channel epoch");
+	}
+	CHECK(assembly.expected_mask == 0x1efu,
+	      "busy unlocked channel entered the ready mask");
+	CHECK(assembly.received_mask == assembly.expected_mask,
+	      "locked channels did not complete the epoch");
+}
+
 static void assembly_handles_duplicates_and_epoch_order(void)
 {
 	gps_nav_epoch_assembly_t assembly;
@@ -458,6 +488,7 @@ int main(void)
 {
 	epoch_id_rounding_and_week_wrap();
 	assembly_waits_for_every_ready_channel();
+	busy_unlocked_channel_does_not_block_epoch();
 	assembly_handles_duplicates_and_epoch_order();
 	assembly_disconnect_and_reconnect();
 	channel_join_does_not_expand_active_epoch();