Fix GPS nav epoch recovery and document debug levels

2026-07-23 20:48:42 +03:00
parent 516016efb9
commit fc242b2883
7 changed files with 252 additions and 38 deletions
@@ -708,7 +708,7 @@ weight   = 1 / variance
 - `computeRange()` icinde aktif parametreler `psTVLTime`, `sat_no`, `eph`, `clockCorr` ve `satellitePositions`'dir. `rho`, `ionoutc`, `xyz`, `tempPos` ve `velocities` eski genis API'den kalmistir. Ionosphere correction artik receiver azimut/elevasyonu hazir oldugunda WLS icinde uygulanir.
 - Ephemeris alanlarinin mevcut isimleri ile ICD anlamlari su sekildedir: `omgdot` gercekte `Omega0`, `omg0` gercekte argument of perigee `omega`, `omgkdot` gercekte `OmegaDot` degeridir. `satpos()` bunlari dogru matematiksel yerde kullanir; konu su an hesap hatasi degil, ileride yapilacak kontrollu isim duzeltmesidir.
 - `POSTPONE_TOW=0.160` donanim/pipeline zaman kalibrasyonunun bir parcasidir; kaynagi dogrulanmadan kaldirilmamalidir.
- Ilk nav-solve interrupt'i geldiginde o anda allocated, hardware busy ve nav-solve interrupt'i etkin olan kanallar epoch'un sabit bekleme maskesine alinir. WLS ancak bu kanallarin ayni GPS epoch'una ait interrupt'lari tamamlandiginda tetiklenir. Unlock interrupt'i gelen kanal bekleme maskesinden cikarilir; epoch ortasinda navigation-ready olan yeni kanal ise aktif maskeyi buyutmez ve sonraki epoch'tan itibaren beklenir. Timeout yoktur: calisan tracking IP nav-solve interrupt'i, lock kaybeden IP unlock interrupt'i ile ilerleme saglar.
+- Ilk nav-solve interrupt'i geldiginde o anda allocated, hardware busy ve nav-solve interrupt'i etkin olan kanallar epoch'un sabit bekleme maskesine alinir. WLS ancak bu kanallarin ayni GPS epoch'una ait interrupt'lari tamamlandiginda tetiklenir. Unlock interrupt'i gelen kanal bekleme maskesinden cikarilir; epoch ortasinda navigation-ready olan yeni kanal ise aktif maskeyi buyutmez ve sonraki epoch'tan itibaren beklenir. Timeout yoktur. Eksik kalan bir epoch'tan daha yeni bir interrupt gelirse eski maske temizlenir ve yeni epoch ilk kanaldan baslatilarak butun ready kanallar yeniden beklenir; eski ve yeni olcumler birlestirilmez.
 - Interrupt bekleme maskesi tamamlandiktan sonra en az dort uyduluk en buyuk ortak TOW/`ABS_SAMPLE` kumesi WLS girdisi olarak kullanilir; kume disi kanallar `epoch_excluded` olarak raporlanir ve sonraki donanim epoch'unda yeniden katilabilir.
 - Dort uydu 3D konum + receiver clock bias icin minimum ve yeterlidir. Bu durumda
  `dof=N-4=0` oldugu icin WLS konumu publish edilir fakat residual chi-square
@@ -733,13 +733,30 @@ GPS_LOG(level, category, ...)
  - `SCHED`
  - `MEM`
  - `HWREG`
- Seviyeler:
-  - `1 STATUS`: konum, GDOP/HDOP/VDOP/TDOP ve takip edilen uydu sayisi
-  - `2 ERROR`: hata mesajlari
-  - `3 WARN`: uyari mesajlari
-  - `4 INFO`: acquisition/tracking/navigation durum gecisleri
-  - `5 DEBUG`: kanal, scheduler ve hesaplama ayrintilari
-  - `6 TRACE`: en ayrintili parser, olcum ve matris tanilari
+  - `FUSION`
+  - `LOCATION`
+- Debug seviyeleri kumulatiftir; secilen seviye kendisini ve daha dusuk
+  seviyeleri gosterir:
+
+| `GPS_DEBUG_LEVEL` | Eklenen cikti |
+| ---: | --- |
+| `1` | Temel telemetri: ham GPS/WLS `[LOCATION] source=gps`, gecerliyse ESKF `[LOCATION] source=imu_corrected`, DOP degerleri ve `[TRK] tracked satellites=... prns=[...]`. Baslangicta fusion/IMU konfigurasyonu da bir kez yazilir. |
+| `2` | Gelecekteki ek status ayrintilari icin ayrilmistir. Su an level 2'ye ozel mesaj yoktur; pratikte level 1 ile ayni ciktiyi verir. |
+| `3` | Level 1'e ek olarak acquisition/tracking/navigation durum gecisleri, warning/error mesajlari, integrity bilgileri ve periyodik ayrintili `[FUSION] state=...` durumu. |
+| `4` | Level 3'e ek olarak performans, scheduler, nav-bit/parser, uydu/olcum ve dusuk seviye TRACE tanilari. UART yuku en yuksek seviyedir. |
+
+`Makefile` varsayilanlari `GPS_DEBUG=1` ve `GPS_DEBUG_LEVEL=4`'tur. Bu nedenle
+dogrudan parametresiz `make`, debug loglarini derler ve dort seviyenin tamamini
+acar. Yalniz temel GPS/ESKF konumu ve tracked-satellite telemetrisi icin:
+
+```sh
+make GPS_DEBUG=1 GPS_DEBUG_LEVEL=1 GPS_ESKF_ENABLE=1
+```
+
+Butun UART debug mesajlarini derleme disinda birakmak icin `GPS_DEBUG=0`
+kullanilir. Komut satirinda veya ust build'de verilen degerler `?=` ile
+tanimlanan Makefile varsayilanlarini ezer.
+
 - Core ACQ/TRCK/ISR path'inde yuksek frekansli uncategorized debug print'ler azaltildi.

 ### Memory Safety
@@ -35,7 +35,7 @@ typedef enum {
 	GPS_NAV_EPOCH_EVENT_ACCEPTED,
 	GPS_NAV_EPOCH_EVENT_DUPLICATE,
 	GPS_NAV_EPOCH_EVENT_STALE,
-	GPS_NAV_EPOCH_EVENT_FUTURE,
+	GPS_NAV_EPOCH_EVENT_RESTARTED,
 	GPS_NAV_EPOCH_EVENT_COMPLETE
 } gps_nav_epoch_event_result_t;

@@ -1112,6 +1112,9 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 			        trck_ip_no, tow_week, tow_sec);
 			return;
 		}
+		uint64_t previous_epoch = gps_nav_epoch_assembly.epoch_id;
+		uint32_t previous_expected = gps_nav_epoch_assembly.expected_mask;
+		uint32_t previous_received = gps_nav_epoch_assembly.received_mask;
 		gps_nav_epoch_event_result_t assembly_result =
 			gps_nav_epoch_assembly_push(
 				&gps_nav_epoch_assembly, epoch_id, trck_ip_no,
@@ -1127,14 +1130,16 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
 			        trck_ip_no, (unsigned long)epoch_id,
 			        (unsigned long)gps_nav_epoch_assembly.epoch_id);
 		}
-		else if(assembly_result == GPS_NAV_EPOCH_EVENT_FUTURE)
+		else if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED)
 		{
+			/* The previous snapshot is no longer the active assembly. Do not
+			 * let an already-pending foreground hand-off consume mixed nodes. */
+			gps_nav_solve_period_pending = false;
 			GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
-			        "future nav epoch blocked by incomplete epoch: trk=%u epoch=%lu active=%lu expected=0x%x received=0x%x\r\n",
-			        trck_ip_no, (unsigned long)epoch_id,
-			        (unsigned long)gps_nav_epoch_assembly.epoch_id,
-			        gps_nav_epoch_assembly.expected_mask,
-			        gps_nav_epoch_assembly.received_mask);
+			        "NAV epoch restarted by newer interrupt: old=%lu expected=0x%x received=0x%x new=%lu first_trk=%u\r\n",
+			        (unsigned long)previous_epoch,
+			        previous_expected, previous_received,
+			        (unsigned long)epoch_id, trck_ip_no);
 		}
 		GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_TRK,
 		        "nav solve: trk=%u abs_raw=%u rem=%u abs=%.6f timer=%u count=%u dt=%.6f tow=%f seq=%u\r\n",
@@ -241,29 +241,36 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
 	}

 	uint32_t channel_mask = 1u << channel;
+	uint32_t valid_mask =
+		(1u << GPS_NAV_EPOCH_MAX_CANDIDATES) - 1u;
+	ready_mask &= valid_mask;
+	if((ready_mask & channel_mask) == 0u)
+	{
+		return GPS_NAV_EPOCH_EVENT_INVALID;
+	}
+
+	bool restarted = false;
+	if(assembly->active)
+	{
+		if(epoch_id < assembly->epoch_id)
+		{
+			return GPS_NAV_EPOCH_EVENT_STALE;
+		}
+		if(epoch_id > assembly->epoch_id)
+		{
+			gps_nav_epoch_assembly_reset(assembly);
+			restarted = true;
+		}
+	}
+
 	if(!assembly->active)
 	{
-		uint32_t valid_mask =
-			(1u << GPS_NAV_EPOCH_MAX_CANDIDATES) - 1u;
-		ready_mask &= valid_mask;
-		if((ready_mask & channel_mask) == 0u)
-		{
-			return GPS_NAV_EPOCH_EVENT_INVALID;
-		}
 		assembly->epoch_id = epoch_id;
 		assembly->expected_mask = ready_mask;
 		assembly->received_mask = 0u;
 		assembly->active = true;
 	}

-	if(epoch_id < assembly->epoch_id)
-	{
-		return GPS_NAV_EPOCH_EVENT_STALE;
-	}
-	if(epoch_id > assembly->epoch_id)
-	{
-		return GPS_NAV_EPOCH_EVENT_FUTURE;
-	}
 	if((assembly->expected_mask & channel_mask) == 0u)
 	{
 		return GPS_NAV_EPOCH_EVENT_INVALID;
@@ -274,8 +281,12 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
 	}

 	assembly->received_mask |= channel_mask;
-	return gps_nav_epoch_assembly_complete(assembly) ?
-	       GPS_NAV_EPOCH_EVENT_COMPLETE :
+	if(gps_nav_epoch_assembly_complete(assembly))
+	{
+		return GPS_NAV_EPOCH_EVENT_COMPLETE;
+	}
+	return restarted ?
+	       GPS_NAV_EPOCH_EVENT_RESTARTED :
 	       GPS_NAV_EPOCH_EVENT_ACCEPTED;
 }

@@ -2,7 +2,8 @@ CC ?= gcc
 CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror

 ROOT := ..
-TARGETS := test_imu_calibration test_gps_nav_epoch test_plic_warm_reset
+TARGETS := test_imu_calibration test_gps_nav_epoch \
+	test_gps_nav_epoch_coalescing test_plic_warm_reset

 .PHONY: all run clean

@@ -14,13 +15,17 @@ test_imu_calibration: test_imu_calibration.c $(ROOT)/src/imu/imu_calibration.c
 test_gps_nav_epoch: test_gps_nav_epoch.c $(ROOT)/src/gps/gps_nav_epoch.c
 	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@

+test_gps_nav_epoch_coalescing: test_gps_nav_epoch_coalescing.c $(ROOT)/src/gps/gps_nav_epoch.c
+	$(CC) $(CFLAGS) -I$(ROOT)/header $^ -lm -o $@
+
 test_plic_warm_reset: test_plic_warm_reset.c $(ROOT)/../bsp/drivers/plic.c
 	$(CC) $(CFLAGS) -Ifake_bsp -I$(ROOT)/../bsp/include $^ -o $@

 run: $(TARGETS)
 	./test_imu_calibration
 	./test_gps_nav_epoch
+	./test_gps_nav_epoch_coalescing
 	./test_plic_warm_reset

 clean:
-	rm -f $(TARGETS)
+	rm -f $(TARGETS) test_gps_nav_epoch_coalescing_repro
@@ -97,10 +97,12 @@ static void assembly_handles_duplicates_and_epoch_order(void)
 	      GPS_NAV_EPOCH_EVENT_STALE,
 	      "stale epoch interrupt was not rejected");
 	CHECK(gps_nav_epoch_assembly_push(&assembly, 2001u, 1u, 0x0fu) ==
-	      GPS_NAV_EPOCH_EVENT_FUTURE,
-	      "future epoch was mixed into an incomplete epoch");
-	CHECK(assembly.received_mask == 0x01u,
-	      "rejected interrupts changed the received mask");
+	      GPS_NAV_EPOCH_EVENT_RESTARTED,
+	      "newer epoch did not replace the incomplete epoch");
+	CHECK((assembly.epoch_id == 2001u) &&
+	      (assembly.expected_mask == 0x0fu) &&
+	      (assembly.received_mask == 0x02u),
+	      "newer epoch restart retained measurements from the old epoch");
 }

 static void assembly_disconnect_and_reconnect(void)
@@ -0,0 +1,174 @@
+#include "gps/gps_nav_epoch.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdio.h>
+
+#define CHANNEL_COUNT 6u
+#define READY_MASK    0x3fu
+#define MISSING_CH    4u
+
+typedef struct {
+	bool pending;
+	uint32_t capture_sequence;
+	uint64_t mailbox_epoch;
+} synthetic_tracking_irq_t;
+
+static int failures;
+
+#define CHECK(condition, message) do { \
+	if(!(condition)) { \
+		fprintf(stderr, "FAIL: %s\n", message); \
+		failures++; \
+	} \
+} while(0)
+
+/*
+ * The tracking interrupt source is a sticky pending bit, while the associated
+ * measurement registers are a one-entry mailbox. A second pulse therefore
+ * advances the PPS capture sequence and overwrites the mailbox even when the
+ * first pulse has not yet been serviced.
+ */
+static void hardware_nav_pulse(
+	synthetic_tracking_irq_t channels[CHANNEL_COUNT],
+	uint64_t epoch)
+{
+	for(unsigned int channel = 0u ; channel < CHANNEL_COUNT ; channel++)
+	{
+		channels[channel].capture_sequence++;
+		channels[channel].mailbox_epoch = epoch;
+		channels[channel].pending = true;
+	}
+}
+
+static gps_nav_epoch_event_result_t service_channel(
+	synthetic_tracking_irq_t channels[CHANNEL_COUNT],
+	uint32_t serviced_sequence[CHANNEL_COUNT],
+	gps_nav_epoch_assembly_t* assembly,
+	unsigned int channel,
+	unsigned int* coalesced_pulses)
+{
+	if((channel >= CHANNEL_COUNT) || !channels[channel].pending)
+	{
+		return GPS_NAV_EPOCH_EVENT_INVALID;
+	}
+
+	uint32_t sequence_delta =
+		channels[channel].capture_sequence - serviced_sequence[channel];
+	if((serviced_sequence[channel] != 0u) && (sequence_delta > 1u))
+	{
+		*coalesced_pulses += sequence_delta - 1u;
+	}
+	serviced_sequence[channel] = channels[channel].capture_sequence;
+	channels[channel].pending = false;
+
+	return gps_nav_epoch_assembly_push(
+		assembly, channels[channel].mailbox_epoch, channel, READY_MASK);
+}
+
+static void service_complete_epoch(
+	synthetic_tracking_irq_t channels[CHANNEL_COUNT],
+	uint32_t serviced_sequence[CHANNEL_COUNT],
+	gps_nav_epoch_assembly_t* assembly,
+	unsigned int* coalesced_pulses)
+{
+	for(unsigned int channel = 0u ; channel < CHANNEL_COUNT ; channel++)
+	{
+		gps_nav_epoch_event_result_t result =
+			service_channel(channels, serviced_sequence, assembly,
+			                channel, coalesced_pulses);
+		CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
+		      (channel == CHANNEL_COUNT - 1u),
+		      "healthy epoch did not complete on its final channel");
+	}
+}
+
+int main(void)
+{
+	synthetic_tracking_irq_t channels[CHANNEL_COUNT] = {0};
+	uint32_t serviced_sequence[CHANNEL_COUNT] = {0};
+	gps_nav_epoch_assembly_t assembly;
+	unsigned int coalesced_pulses = 0u;
+	unsigned int completed_epochs = 0u;
+	const uint64_t baseline_epoch = 1151141640u;
+	const uint64_t blocked_epoch = 1151141641u;
+
+	/* Establish the same non-zero PPS sequence baseline as the running board. */
+	gps_nav_epoch_assembly_reset(&assembly);
+	hardware_nav_pulse(channels, baseline_epoch);
+	service_complete_epoch(channels, serviced_sequence, &assembly,
+	                       &coalesced_pulses);
+	completed_epochs++;
+	gps_nav_epoch_assembly_reset(&assembly);
+
+	/* All six pulse, but the CPU services every source except tracking 4. */
+	hardware_nav_pulse(channels, blocked_epoch);
+	for(unsigned int channel = 0u ; channel < CHANNEL_COUNT ; channel++)
+	{
+		if(channel == MISSING_CH)
+		{
+			continue;
+		}
+		CHECK(service_channel(channels, serviced_sequence, &assembly,
+		                      channel, &coalesced_pulses) ==
+		      GPS_NAV_EPOCH_EVENT_ACCEPTED,
+		      "partial blocked epoch was not accepted");
+	}
+	CHECK(assembly.expected_mask == 0x3fu,
+	      "blocked epoch expected mask differs from the board");
+	CHECK(assembly.received_mask == 0x2fu,
+	      "blocked epoch received mask differs from the board");
+
+	/*
+	 * Before channel 4 is serviced, the next hardware pulse overwrites its
+	 * one-entry mailbox. Its sequence delta is two, but only the newer epoch
+	 * can be delivered to the assembler.
+	 */
+	hardware_nav_pulse(channels, blocked_epoch + 1u);
+	for(unsigned int channel = 0u ; channel < CHANNEL_COUNT ; channel++)
+	{
+		gps_nav_epoch_event_result_t result =
+			service_channel(channels, serviced_sequence, &assembly,
+			                channel, &coalesced_pulses);
+		gps_nav_epoch_event_result_t expected =
+			(channel == 0u) ? GPS_NAV_EPOCH_EVENT_RESTARTED :
+			(channel == CHANNEL_COUNT - 1u) ?
+				GPS_NAV_EPOCH_EVENT_COMPLETE :
+				GPS_NAV_EPOCH_EVENT_ACCEPTED;
+		CHECK(result == expected,
+		      "newer hardware epoch did not restart and complete cleanly");
+	}
+	CHECK(coalesced_pulses == 1u,
+	      "PPS sequence did not expose the overwritten channel-4 pulse");
+	CHECK((assembly.epoch_id == blocked_epoch + 1u) &&
+	      (assembly.expected_mask == READY_MASK) &&
+	      (assembly.received_mask == READY_MASK) &&
+	      gps_nav_epoch_assembly_complete(&assembly),
+	      "assembler did not recover on the first complete newer epoch");
+	completed_epochs++;
+
+	/* Every later healthy epoch must continue to complete normally. */
+	for(uint64_t offset = 2u ; offset < 102u ; offset++)
+	{
+		gps_nav_epoch_assembly_reset(&assembly);
+		hardware_nav_pulse(channels, blocked_epoch + offset);
+		service_complete_epoch(channels, serviced_sequence, &assembly,
+		                       &coalesced_pulses);
+		completed_epochs++;
+	}
+
+	CHECK(completed_epochs == 102u,
+	      "recovery stream lost or duplicated a completed epoch");
+
+	if(failures == 0)
+	{
+		printf("RECOVERED: active=%llu expected=0x%x received=0x%x "
+		       "coalesced=%u completed_epochs=%u\n",
+		       (unsigned long long)assembly.epoch_id,
+		       assembly.expected_mask,
+		       assembly.received_mask,
+		       coalesced_pulses,
+		       completed_epochs);
+	}
+	return failures == 0 ? 0 : 1;
+}