Keep fused location streaming through GPS rejects

2026-07-23 23:20:23 +03:00
parent fc242b2883
commit 9ff895600b
14 changed files with 173 additions and 46 deletions
@@ -14,6 +14,9 @@ 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,
@@ -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;
@@ -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
@@ -17,6 +17,7 @@ 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;
 	bool gps_epoch_ready;
 	bool fusion_enabled;