fix(gps): recover when tracking loss IRQ is missed

2026-07-24 20:33:45 +03:00
parent 86dfbeafac
commit 64860d1ced
2 changed files with 137 additions and 3 deletions
@@ -48,6 +48,7 @@ typedef struct {
 } gps_tracking_shadow_t;

 static volatile bool tracking_allocated[GPS_TRCK_IP_COUNT];
+static unsigned char tracking_not_busy_polls[GPS_TRCK_IP_COUNT];
 static volatile gps_acq_engine_state_t acq_engine_state[GPS_ACQ_IP_COUNT];
 static volatile unsigned char acq_engine_prn[GPS_ACQ_IP_COUNT];
 static volatile uint64_t acq_engine_start_tick[GPS_ACQ_IP_COUNT];
@@ -81,6 +82,7 @@ static unsigned int gps_external_irq_id = 0u;
 static gps_external_irq_handler_t gps_external_irq_handler = NULL;

 #define GPS_PLIC_DRAIN_LIMIT (GPS_TRCK_IP_COUNT + GPS_ACQ_IP_COUNT + 4u)
+#define GPS_TRK_NOT_BUSY_CONFIRM_POLLS 2u
 #define GPS_NAV_CAPTURE_PERIOD_TICKS \
 	(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u)
 #define GPS_NAV_CAPTURE_WINDOW_TICKS \
@@ -385,6 +387,7 @@ static void gps_runtime_state_init(void)
 	for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
 	{
 		tracking_allocated[i] = false;
+		tracking_not_busy_polls[i] = 0u;
 		memset(&trk_shadow[i], 0, sizeof(trk_shadow[i]));
 		gps_clear_tracking_slot((unsigned char)i);
 	}
@@ -682,6 +685,7 @@ bool assign_tracking_from_result(const gps_acq_result_t* result)
 		   !trck_ip_exists(&tracking_node, i))
 		{
 			tracking_allocated[i] = true;
+			tracking_not_busy_polls[i] = 0u;
 			gps_clear_tracking_slot(i);
 			gps_ephemeris_t cached_ephemeris;
 			if(gps_ephemeris_cache_restore(
@@ -1030,6 +1034,7 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
 	}

 	tracking_allocated[trck_ip_no] = false;
+	tracking_not_busy_polls[trck_ip_no] = 0u;
 	if(trck_ip_exists(&tracking_node, trck_ip_no) &&
 	   !delete_by_ip_number(&tracking_node, trck_ip_no))
 	{
@@ -1731,6 +1736,73 @@ static void gps_mark_stale_measurements(void)
 	}
 }

+static unsigned int gps_prn_for_tracking_channel(
+	unsigned char trck_ip_no,
+	volatile gps_ephem_ll_t* node)
+{
+	if((node != NULL) && (node->data_p != NULL) &&
+	   gps_prn_valid(node->data_p->SVID))
+	{
+		return node->data_p->SVID;
+	}
+
+	unsigned int hardware_prn = trck_ips[trck_ip_no]->PRN_NUMBER;
+	if(gps_prn_valid(hardware_prn))
+	{
+		return hardware_prn;
+	}
+
+	for(unsigned int prn = 0u ; prn < GPS_PRN_COUNT ; prn++)
+	{
+		if(satellite_tracking_ip[prn] == trck_ip_no)
+		{
+			return prn;
+		}
+	}
+	return GPS_PRN_INVALID;
+}
+
+static void gps_reconcile_stopped_tracking_channels(void)
+{
+	for(unsigned char i = 0u ; i < GPS_TRCK_IP_COUNT ; i++)
+	{
+		volatile gps_ephem_ll_t* node =
+			get_element_by_ip_number(&tracking_node, i);
+		bool allocated = tracking_allocated[i] || (node != NULL);
+		uint32_t status = trck_ips[i]->STATUS_REG;
+		if(!allocated ||
+		   ((status & (1u << GPS_TRCK_BUSY_POS)) != 0u))
+		{
+			tracking_not_busy_polls[i] = 0u;
+			continue;
+		}
+
+		if(tracking_not_busy_polls[i] <
+		   GPS_TRK_NOT_BUSY_CONFIRM_POLLS)
+		{
+			tracking_not_busy_polls[i]++;
+		}
+		if(tracking_not_busy_polls[i] <
+		   GPS_TRK_NOT_BUSY_CONFIRM_POLLS)
+		{
+			continue;
+		}
+
+		gps_isr_event_t stopped_event;
+		memset(&stopped_event, 0, sizeof(stopped_event));
+		stopped_event.type = GPS_ISR_EVENT_TRK_STATUS;
+		stopped_event.index = i;
+		stopped_event.status = status;
+		stopped_event.control = trck_ips[i]->CONTROL_REG;
+		stopped_event.prn = gps_prn_for_tracking_channel(i, node);
+		stopped_event.tick = gps_get_systick();
+		GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK,
+		        "tracking stopped without loss IRQ: trk=%u prn=%u status=0x%x\r\n",
+		        i, stopped_event.prn, status);
+		gps_process_tracking_drop(&stopped_event);
+	}
+}
+
 void gps_background_task(void)
 {
 	gps_poll_acq_pending_completions();
@@ -1738,6 +1810,7 @@ void gps_background_task(void)
 	gps_process_isr_events();
 	if(gps_take_timer_maintenance_pending())
 	{
+		gps_reconcile_stopped_tracking_channels();
 		gps_mark_stale_measurements();
 		gps_check_acq_stuck_busy();
 		gps_log_acq_status();