Keep fused location streaming through GPS rejects

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,6 +29,33 @@ 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";
+
+	/* At 50 Hz this compact record uses about 14% of the 460800-baud link,
+	 * leaving headroom for navigation work and lower-rate diagnostics. */
+	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,