onur/gps
0
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(gps): harden navigation epoch recovery

Browse Source
This commit is contained in:
onur
2026-07-24 14:01:58 +03:00
parent ea055a36d2
commit fe0eb19dc5
15 changed files with 1007 additions and 773 deletions
Download Patch File Download Diff File Expand all files Collapse all files
soc/software/gps_system/README.md
+6 -5
View File
@@ -627,7 +627,7 @@ GPS_EVENT_QUEUE_SIZE
gps_last_measurement_tick
gps_measurement_valid
gps_nav_solve_sequence
gps_nav_solve_consumed_sequence
gps_last_nav_solve_pps_counter
```
- Navigation input'u hazir kabul edilmeden once su kontroller yapilir:
@@ -668,7 +668,7 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
- Nav-bit ve nav-solve handler'lari ephemeris deposunu ortak kullansa da corrected `ABS_SAMPLE` nav-solve icin node icinde ayri saklanir. Pozisyon snapshot'i bu ayri alani kullanir; araya giren nav-bit interrupt'i pozisyon olcum epoch'unu degistiremez.
- Coherent snapshot olusturulurken 32-bit raw `ABS_SAMPLE` degerleri en yeni sample etrafinda modulo fark ile unwrap edilir. Sayac wrap sinirinin iki tarafindaki uydular ayni pseudorange hesabinda kullanilabilir.
- Nav bits akisi ilk tutarli subframe 1..3 seti commit edildikten sonra kapatilmaz. Boylece yeni subframe 1..3 verisi ephemerisi, subframe 4 page 18 verisi iyonosfer/UTC parametrelerini guncel tutar.
- Nav-solve interrupt'lari acilmadan once TOW, nav-bit HOW alanindan kurulur. Nav-solve acildiktan sonra normal nav-bit HOW gecmisi tutulmaya devam eder fakat canli TOW'a yazmaz. Her nav-solve interrupt'i tam olarak `NAV_SOLVE_TIMER * 20 ms` ilerlemeyi temsil eder ve TOW bu sabit miktarda artirilir.
- Nav-solve interrupt'lari acilmadan once TOW, parity kontrolunden gecen nav-bit HOW alanindan kurulur. Her yeni HOW'un GPS zaman farki receiver sample-counter farkiyla karsilastirilir; tek bir sureksiz HOW saglikli anchor'i degistiremez. Iki ardışık aday kendi aralarinda tutarliysa kanal reset gerektirmeden yeni zaman cizgisine gecerek toparlanir. Subframe 1 gelmeden onceki gecici hafta, ilk gecerli WN ile atomik olarak guncellenir.
- Subframe 1..3 ham kelimeleri once staging alaninda biriktirilir. IODC'nin alt 8 biti ile iki IODE alani eslesen tam set olusmadan canli ephemeris degistirilmez; yayin guncellemesi sirasinda eski/yeni ephemeris alanlari karismaz.
- Subframe 1 `toc` alani parity bitleri disarida birakilarak 16 bit okunur ve `2^4` ile olceklenir.
- Subframe 4 page 18 henuz alinmadiysa iyonosfer duzeltmesi uygulanmaz. Page 18 hazir oldugunda her navigation solve tek bir `ionoutc` kopyasi kullanir.
@@ -678,8 +678,9 @@ GPS_TRK_MEASUREMENT_STALE_TICKS
- Tracking kanallari farkli zamanlarda lock olabilir ve ilk gecerli subframe'lerini 6 saniyenin farkli katlarinda gorebilir. Timer bu nedenle ilk subframe'in HOW alanindaki 17-bit Z-count degerini kullanarak `Z_COUNT * 300 mod NAV_SOLVE_TIMER` baslangic fazini hesaplar. Hesap ALIGN durumunda 26 PCLK cevriminde ard arda modulo ile yapilir; 20 ms'lik ilk navigation bitinden cok once tamamlanir. RTL ayni GPS haftasi icinde keyfi pozitif `N` degerlerini hizalayabilir; firmware ise hafta rollover'i oncesinden aktif kalan kanallarla rollover sonrasinda katilan kanallarin da ayni fazda kalmasi icin `30240000 mod N == 0` kosulunu compile time'da zorunlu tutar. Ornegin `N=40`, `N=100`, `N=125`, `N=150`, `N=200` ve `N=300` gecerlidir; kosulu saglamayan build acik bir `#error` ile durur.
- Initial position sonrasinda subframe'lerini tamamlayan yeni tracking kanallarinda da nav bits acik tutularak nav-solve otomatik etkinlestirilir.
- Coherence kontrolu sadece software systick ile yapilmaz; periodic snapshot icin tracking node'da saklanan nav-solve interrupt sonrasi raw `ABS_SAMPLE` kullanilir. `GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS=25` oldugu icin 4 satellite measurement'i 25 ms'den fazla ayriksa navigation solve calistirilmaz. Bu pencere interrupt skew'den cok uydu path delay farkini kapsamak icindir: 20 ms yaklasik 6000 km, 25 ms yaklasik 7500 km pseudorange farkina denk gelir. 25 ms, dusuk-elevation uydu geometrisi ve tracking jitter icin pay birakir ama 50 ms kadar gevsek degildir.
- Her nav periyodunda yeni sequence'i bulunan adaylar sabit boyutlu, heap kullanmayan epoch selector'a verilir. Selector TOW ve modulo-32-bit `ABS_SAMPLE` kosullarini ikili olarak saglayan en buyuk coherent alt kumeyi secer; tek bir gecikmis veya bozuk kanal diger en az 4 uyduyu bloke etmez. Esit buyuklukte ayrik kumelerde daha yeni sample epoch'u tercih edilir.
- En az 4 uydu secildiginde kume disindaki snapshot sequence'leri emekliye ayrilir. Tuketim snapshot aninda kaydedilen sequence degerine kadar yapildigi icin arada gelen daha yeni interrupt korunur. Tamamlanan donanim epoch'u WLS sonucu basarili veya basarisiz olsa da yalniz bir kez denenir; ayni olcumlerin ilgisiz bir foreground wake-up ile tekrar cozulmesi engellenir.
- Interrupt'lar epoch'a GPS TOW ile degil, butun kanallarda ortak olan 64-bit PPS capture counter ile atanir. Ilk olcum sabit expected maskeyi ve 30 ms bounded holdoff'u baslatir. 25 ms pencereye giren kanallar ayni fiziksel epoch'a katilir; yarim nav periyodundan uzaktaki eski/yeni capture'lar stale veya yeni epoch olarak siniflanir. Boylece bozuk bir gelecek HOW degeri epoch assembler'i kilitleyemez.
- Tam veya en az dort uydulu timeout snapshot'i sabit boyutlu iki buffer arasinda immutable olarak foreground'a devredilir. Hardware mailbox birden fazla pulse'u kacirdiysa sequence farki yalniz diagnostic sayacidir; elde olmayan ara olcumler sentetik olarak uretilmez. Tamamlanan snapshot WLS sonucu basarili veya basarisiz olsa da yalniz bir kez denenir.
- Immutable snapshot icindeki adaylar heap kullanmayan selector'a verilir. Selector TOW ve modulo-32-bit `ABS_SAMPLE` kosullarini ikili olarak saglayan en buyuk coherent alt kumeyi secer; tek bir gecikmis veya bozuk kanal diger en az 4 uyduyu bloke etmez. Esit buyuklukte ayrik kumelerde daha yeni sample epoch'u tercih edilir.
- Navigation snapshot ayni SVID/PRN'i ikinci kez kabul etmez. Ayni uydu iki tracking IP uzerinden ready gorunurse ikinci kopya sessizce snapshot disinda birakilir; ayrinti sadece `DEBUG_TRACE` seviyesinde gorulur.
- UART debug satirindaki `coherent_nav=` degeri, o anda ayni nav-solve penceresinde kullanilabilir kanal sayisini gosterir. 600 ms pozisyon gelmiyorsa `busy channels`, `ready channels` ve `coherent_nav` birlikte kontrol edilmelidir.
- Tracking ISR event queue dolarsa, her tracking IP icin son event shadow buffer'a alinir. Shadow overwrite sayaci WARN olarak loglanir; bu log gorulurse CPU nav hesaplamasi veya UART debug cikisi tracking interrupt isleme hizini geciktiriyor olabilir.
@@ -727,7 +728,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. 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.
- 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. 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. Butun beklenen kanallar gelirse snapshot hemen, 30 ms holdoff doldugunda ise elde en az dort kanal varsa eksik kanalla yayinlanir. Dortten az kanal discard edilir. Daha yeni bir fiziksel epoch basladiginda kullanilabilir eski snapshot once yayinlanir; 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
soc/software/gps_system/header/gps/gps_interrupt_handlers.h
+16 -1
View File
@@ -8,6 +8,10 @@
uint32_t plic_drain_limit_total;
uint32_t plic_repeat_total;
uint32_t nav_epoch_coalesced_total;
uint32_t pps_sequence_discontinuity_total;
uint32_t nav_epoch_incomplete_total;
uint32_t nav_epoch_snapshot_overwrite_total;
uint32_t how_anchor_rejected_total;
uint32_t satellite_connected_total;
uint32_t satellite_lost_total;
uint32_t last_connected_prn;
@@ -16,6 +20,17 @@
uint64_t last_lost_tick;
} gps_runtime_diagnostics_t;
typedef struct {
uint32_t channel_mask;
uint32_t prn[GPS_TRCK_IP_COUNT];
gpstime_t tow[GPS_TRCK_IP_COUNT];
uint32_t raw_sample[GPS_TRCK_IP_COUNT];
double corrected_abs_sample[GPS_TRCK_IP_COUNT];
uint64_t pps_counter[GPS_TRCK_IP_COUNT];
uint32_t pps_capture_sequence[GPS_TRCK_IP_COUNT];
uint64_t local_timestamp_ticks[GPS_TRCK_IP_COUNT];
} gps_nav_epoch_snapshot_t;
typedef void (*gps_external_irq_handler_t)(void);
bool assign_tracking_from_result(const gps_acq_result_t* result);
@@ -28,7 +43,7 @@
void gps_process_isr_events(void);
void gps_acq_scheduler_run(void);
void gps_background_task(void);
bool gps_take_nav_solve_period_ready(void);
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot);
void gps_get_runtime_diagnostics(gps_runtime_diagnostics_t* diagnostics);
void trck_intr_handler(volatile gps_trck_t* const trck_ip, volatile gps_frame_t *gps_frame_parsed, volatile int unsigned gps_subframe[10], unsigned char trck_ip_no);
soc/software/gps_system/header/gps/gps_ip_defines.h
+3
View File
@@ -63,8 +63,11 @@
#define GPS_NAV_STATUS_LOG_PERIOD_TICKS GPS_SYSTICK_HZ
#define GPS_TRK_MEASUREMENT_STALE_TICKS (GPS_SYSTICK_HZ * 3u)
#define GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS 25u
#define GPS_NAV_EPOCH_HOLDOFF_MS 30u
#define GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC 0.001
#define GPS_HOW_ANCHOR_MAX_AGE_SEC 12.0
#define GPS_HOW_CONTINUITY_MAX_GAP_SEC 120.0
#define GPS_HOW_CONTINUITY_TOLERANCE_SEC 0.050
#if (GPS_NAV_SOLVE_TIMER_TICKS == 0u)
#error "GPS_NAV_SOLVE_TIMER_TICKS must be non-zero"
soc/software/gps_system/header/gps/gps_nav_epoch.h
+19 -9
View File
@@ -24,7 +24,10 @@ typedef struct {
} gps_nav_epoch_selection_t;
typedef struct {
uint64_t epoch_id;
uint64_t first_capture_counter;
uint64_t minimum_capture_counter;
uint64_t maximum_capture_counter;
uint64_t deadline_local_ticks;
uint32_t expected_mask;
uint32_t received_mask;
bool active;
@@ -35,6 +38,7 @@ typedef enum {
GPS_NAV_EPOCH_EVENT_ACCEPTED,
GPS_NAV_EPOCH_EVENT_DUPLICATE,
GPS_NAV_EPOCH_EVENT_STALE,
GPS_NAV_EPOCH_EVENT_OUTLIER,
GPS_NAV_EPOCH_EVENT_RESTARTED,
GPS_NAV_EPOCH_EVENT_COMPLETE
} gps_nav_epoch_event_result_t;
@@ -46,19 +50,17 @@ bool gps_nav_epoch_select(
double tow_tolerance_sec,
gps_nav_epoch_selection_t* selection);
bool gps_nav_epoch_id(
int week,
double tow_sec,
unsigned int period_ms,
uint64_t* epoch_id);
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly);
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
gps_nav_epoch_assembly_t* assembly,
uint64_t epoch_id,
uint64_t capture_counter,
uint64_t local_timestamp_ticks,
unsigned int channel,
uint32_t ready_mask);
uint32_t ready_mask,
uint64_t capture_period_ticks,
uint64_t sync_window_ticks,
uint64_t holdoff_local_ticks);
bool gps_nav_epoch_assembly_remove_channel(
gps_nav_epoch_assembly_t* assembly,
@@ -67,4 +69,12 @@ bool gps_nav_epoch_assembly_remove_channel(
bool gps_nav_epoch_assembly_complete(
const gps_nav_epoch_assembly_t* assembly);
bool gps_nav_epoch_assembly_usable(
const gps_nav_epoch_assembly_t* assembly,
unsigned int minimum_channels);
bool gps_nav_epoch_assembly_expired(
const gps_nav_epoch_assembly_t* assembly,
uint64_t now_local_ticks);
#endif
soc/software/gps_system/header/gps/gps_time_anchor.h
+20
View File
@@ -16,6 +16,11 @@ int gps_time_resolve_how_week(bool previous_valid,
int fallback_week,
double how_tow_sec);
/* Extend a modulo-1024 LNAV week number around the previous monotonic week. */
int gps_time_extend_lnav_week(bool previous_valid,
int previous_week,
unsigned int lnav_week);
/* Convert a receiver-sample timestamp to GPS time from a HOW/sample anchor.
* raw counters are modulo 2^32. max_age_sec must keep the signed difference
* unambiguous (and also rejects a stale HOW anchor). */
@@ -31,4 +36,19 @@ bool gps_time_from_sample_anchor(int anchor_week,
double* event_tow_sec_out,
double* elapsed_sec_out);
/* Validate that two decoded HOW/sample anchors describe the same monotonic
* receiver timeline. This keeps a single transient HOW from replacing a
* healthy anchor while still permitting normal week rollover. */
bool gps_time_anchors_consistent(int first_week,
double first_tow_sec,
uint32_t first_raw_sample,
double first_corrected_sample,
int second_week,
double second_tow_sec,
uint32_t second_raw_sample,
double second_corrected_sample,
double samples_per_second,
double max_gap_sec,
double tolerance_sec);
#endif
soc/software/gps_system/header/gps/linked_list.h
+5 -1
View File
@@ -13,8 +13,12 @@ typedef struct gps_ephem_ll {
volatile double gps_how_corrected_abs_sample;
volatile uint64_t gps_how_tick;
volatile bool gps_how_anchor_valid;
volatile bool gps_how_week_valid;
volatile gpstime_t gps_pending_how_tow;
volatile uint32_t gps_pending_how_abs_sample;
volatile double gps_pending_how_corrected_abs_sample;
volatile bool gps_pending_how_valid;
volatile uint32_t gps_nav_solve_sequence;
volatile uint32_t gps_nav_solve_consumed_sequence;
volatile gpstime_t gps_last_nav_solve_tow;
volatile uint32_t gps_last_nav_solve_abs_sample;
volatile double gps_last_nav_solve_corrected_abs_sample;
soc/software/gps_system/src/gps/gps_interrupt_handlers.c
+315 -93
View File
@@ -25,7 +25,6 @@ typedef struct {
uint32_t rem_code_phase;
double corrected_abs_sample;
uint32_t nav_solve_timer;
uint32_t nav_solve_event_count;
uint64_t local_timestamp_ticks;
uint64_t pps_counter;
uint32_t pps_capture_sequence;
@@ -40,7 +39,6 @@ typedef struct {
bool nav_solve_pending;
bool misc_pending;
unsigned int nav_bits_overwrite_count;
uint32_t nav_solve_event_count;
gps_isr_event_t drop_event;
gps_isr_event_t nav_bits_event;
gps_isr_event_t nav_solve_event;
@@ -71,14 +69,40 @@ static volatile unsigned int gps_plic_drain_limit_count = 0;
static volatile unsigned int gps_plic_repeat_count = 0;
static volatile bool gps_acq_schedule_pending = true;
static volatile bool gps_timer_maintenance_pending = true;
static volatile bool gps_nav_solve_period_pending = false;
static gps_nav_epoch_assembly_t gps_nav_epoch_assembly;
static gps_nav_epoch_snapshot_t gps_nav_active_snapshot;
static gps_nav_epoch_snapshot_t gps_nav_completed_snapshot;
static bool gps_nav_completed_snapshot_pending;
static volatile gps_runtime_diagnostics_t gps_runtime_diagnostics;
static unsigned int gps_acq_round_robin_prn = 0;
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_NAV_CAPTURE_PERIOD_TICKS \
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u)
#define GPS_NAV_CAPTURE_WINDOW_TICKS \
(((uint64_t)GPS_PPS_CLOCK_HZ * GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS) / 1000u)
#define GPS_NAV_HOLDOFF_LOCAL_TICKS \
(((uint64_t)BSP_CLINT_TIMEBASE_HZ * GPS_NAV_EPOCH_HOLDOFF_MS) / 1000u)
_Static_assert(GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS <
(GPS_NAV_SOLVE_PERIOD_MS / 2u),
"NAV capture window must be below half the solve period");
_Static_assert(GPS_NAV_EPOCH_HOLDOFF_MS >=
GPS_NAV_ABS_SAMPLE_SYNC_WINDOW_MS,
"NAV holdoff must cover the capture window");
static unsigned int gps_mask_count(uint32_t mask)
{
unsigned int count = 0u;
while(mask != 0u)
{
count += mask & 1u;
mask >>= 1u;
}
return count;
}
static const char* gps_satellite_state_name(gps_satellite_state_t state)
{
@@ -232,37 +256,66 @@ static bool gps_take_timer_maintenance_pending(void)
return pending;
}
bool gps_take_nav_solve_period_ready(void)
static void gps_nav_discard_active_snapshot(bool reset_assembly)
{
bool tracking_shadow_pending = false;
bool acquisition_shadow_pending = false;
memset(&gps_nav_active_snapshot, 0, sizeof(gps_nav_active_snapshot));
if(reset_assembly)
{
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
}
}
static void gps_nav_publish_active_snapshot(uint32_t channel_mask,
bool reset_assembly)
{
channel_mask &= gps_nav_active_snapshot.channel_mask;
if(gps_mask_count(channel_mask) < MIN_NEEDED_SATELLITE)
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
gps_nav_discard_active_snapshot(reset_assembly);
return;
}
if(gps_nav_completed_snapshot_pending)
{
gps_runtime_diagnostics.nav_epoch_snapshot_overwrite_total++;
}
gps_nav_active_snapshot.channel_mask = channel_mask;
gps_nav_completed_snapshot = gps_nav_active_snapshot;
gps_nav_completed_snapshot_pending = true;
gps_nav_discard_active_snapshot(reset_assembly);
}
bool gps_take_nav_epoch_snapshot(gps_nav_epoch_snapshot_t* snapshot)
{
if(snapshot == NULL)
{
return false;
}
uint64_t irq_state = bsp_irq_save();
for(unsigned int i = 0 ; i < GPS_TRCK_IP_COUNT ; i++)
uint64_t now_local_ticks = bsp_read64(BSP_CLINT_MTIME);
if(gps_nav_epoch_assembly_expired(&gps_nav_epoch_assembly,
now_local_ticks))
{
if(trk_shadow[i].drop_pending || trk_shadow[i].nav_bits_pending ||
trk_shadow[i].nav_solve_pending || trk_shadow[i].misc_pending)
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
if(gps_nav_epoch_assembly_usable(&gps_nav_epoch_assembly,
MIN_NEEDED_SATELLITE))
{
tracking_shadow_pending = true;
break;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else
{
gps_nav_discard_active_snapshot(true);
}
}
for(unsigned int i = 0 ; i < GPS_ACQ_IP_COUNT ; i++)
{
if(acq_shadow_pending[i])
{
acquisition_shadow_pending = true;
break;
}
}
bool pipeline_idle = (gps_event_head == gps_event_tail) &&
!tracking_shadow_pending && !acquisition_shadow_pending;
bool ready = gps_nav_solve_period_pending && pipeline_idle;
bool ready = gps_nav_completed_snapshot_pending;
if(ready)
{
/* The complete epoch is handed to the foreground exactly once. Events
* arriving while WLS runs start the next assembly. */
gps_nav_solve_period_pending = false;
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
*snapshot = gps_nav_completed_snapshot;
gps_nav_completed_snapshot_pending = false;
}
bsp_irq_restore(irq_state);
return ready;
@@ -313,8 +366,10 @@ static void gps_runtime_state_init(void)
gps_plic_repeat_count = 0;
gps_acq_schedule_pending = true;
gps_timer_maintenance_pending = true;
gps_nav_solve_period_pending = false;
gps_nav_epoch_assembly_reset(&gps_nav_epoch_assembly);
memset(&gps_nav_active_snapshot, 0, sizeof(gps_nav_active_snapshot));
memset(&gps_nav_completed_snapshot, 0, sizeof(gps_nav_completed_snapshot));
gps_nav_completed_snapshot_pending = false;
memset((void*)&gps_runtime_diagnostics, 0, sizeof(gps_runtime_diagnostics));
gps_runtime_diagnostics.last_connected_prn = GPS_PRN_INVALID;
gps_runtime_diagnostics.last_lost_prn = GPS_PRN_INVALID;
@@ -708,9 +763,8 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
event.rem_code_phase = trck_ip->REM_CODE_PHASE_REG;
event.corrected_abs_sample = gps_correct_abs_sample(event.abs_sample_raw, event.rem_code_phase);
event.nav_solve_timer = trck_ip->NAV_SOLVE_TIMER;
event.nav_solve_event_count = (status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) ? 1u : 0u;
event.local_timestamp_ticks = local_timestamp_ticks;
if(event.nav_solve_event_count != 0u)
if((status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS)) != 0u)
{
event.pps_capture_valid = gps_pps_read_capture(trck_ip_no,
&event.pps_counter,
@@ -741,6 +795,9 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
if(status_reg & (1u << GPS_TRCK_STOP_UNLOCK_POS))
{
shadow->drop_event = event;
shadow->drop_event.status &=
~((1u << GPS_TRCK_NAV_BITS_RCV_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->drop_pending = true;
}
if(status_reg & (1u << GPS_TRCK_NAV_BITS_RCV_POS))
@@ -750,12 +807,17 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
shadow->nav_bits_overwrite_count++;
}
shadow->nav_bits_event = event;
shadow->nav_bits_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->nav_bits_pending = true;
}
if(status_reg & (1u << GPS_TRCK_NAV_SOLVE_FLAG_POS))
{
shadow->nav_solve_event = event;
shadow->nav_solve_event_count++;
shadow->nav_solve_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_BITS_RCV_POS));
shadow->nav_solve_pending = true;
}
if((status_reg & ((1u << GPS_TRCK_STOP_UNLOCK_POS) |
@@ -763,6 +825,10 @@ void trck_intr_handler(volatile gps_trck_t* const trck_ip,
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS))) == 0u)
{
shadow->misc_event = event;
shadow->misc_event.status &=
~((1u << GPS_TRCK_STOP_UNLOCK_POS) |
(1u << GPS_TRCK_NAV_BITS_RCV_POS) |
(1u << GPS_TRCK_NAV_SOLVE_FLAG_POS));
shadow->misc_pending = true;
}
}
@@ -912,10 +978,17 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
unsigned char trck_ip_no = event->index;
unsigned int dropped_prn = event->prn;
if(gps_nav_epoch_assembly_remove_channel(
&gps_nav_epoch_assembly, trck_ip_no))
bool epoch_complete = gps_nav_epoch_assembly_remove_channel(
&gps_nav_epoch_assembly, trck_ip_no);
gps_nav_active_snapshot.channel_mask &= ~(1u << trck_ip_no);
if(epoch_complete)
{
gps_nav_solve_period_pending = true;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else if(!gps_nav_epoch_assembly.active)
{
gps_nav_discard_active_snapshot(false);
}
if(gps_prn_valid(dropped_prn))
@@ -944,6 +1017,42 @@ static void gps_process_tracking_drop(const gps_isr_event_t* event)
trck_ip_no, dropped_prn, event->status, event->control);
}
static bool gps_how_candidate_consistent(
int first_week,
double first_tow_sec,
uint32_t first_raw_sample,
double first_corrected_sample,
int second_week,
double second_tow_sec,
uint32_t second_raw_sample,
double second_corrected_sample)
{
return gps_time_anchors_consistent(
first_week, first_tow_sec,
first_raw_sample, first_corrected_sample,
second_week, second_tow_sec,
second_raw_sample, second_corrected_sample,
SAMPLES_PER_CODE * 1000.0,
GPS_HOW_CONTINUITY_MAX_GAP_SEC,
GPS_HOW_CONTINUITY_TOLERANCE_SEC);
}
static void gps_commit_how_anchor(volatile gps_ephem_ll_t* node,
int week,
double tow_sec,
const gps_isr_event_t* event,
bool week_valid)
{
node->gps_how_tow.week = week;
node->gps_how_tow.sec = tow_sec;
node->gps_how_abs_sample = event->abs_sample_raw;
node->gps_how_corrected_abs_sample = event->corrected_abs_sample;
node->gps_how_tick = event->tick;
node->gps_how_anchor_valid = true;
node->gps_how_week_valid = week_valid;
node->gps_pending_how_valid = false;
}
static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
{
unsigned char trck_ip_no = event->index;
@@ -980,39 +1089,89 @@ static void gps_process_tracking_nav_bits(const gps_isr_event_t* event)
unsigned char subframe_id = parse_subframe((unsigned int*)gps_subframe,
(gps_frame_t*)gps_frame_parsed,
&how_tow_sec);
if((subframe_id >= 1u) && (subframe_id <= 5u))
if((subframe_id >= 1u) && (subframe_id <= 5u) &&
(how_tow_sec < (unsigned int)GPS_TIME_WEEK_SECONDS))
{
node->gps_subframe_mask |= (1u << subframe_id);
int fallback_week = gps_frame_parsed->gps_ephems.tow.week;
if(subframe_id == 1u)
{
/* WN is ten bits in LNAV. Keeping it in the same modulo domain as
* the ephemeris is sufficient for satellite-time differences. */
fallback_week = (int)((gps_subframe[2] >> 20) & 0x3ffu);
fallback_week = gps_time_extend_lnav_week(
node->gps_how_anchor_valid,
node->gps_how_tow.week,
(gps_subframe[2] >> 20) & 0x3ffu);
}
/* A subframe-1 WN is authoritative in its native 10-bit domain. On
* other subframes, resolve the HOW seconds rollover from the previous
* anchor. This also upgrades a pre-SF1 temporary week immediately. */
/* Subframe 1 supplies the modulo-1024 week. Other subframes resolve
* HOW rollover from the previous monotonic anchor. */
int anchor_week = (subframe_id == 1u) ? fallback_week :
gps_time_resolve_how_week(node->gps_how_anchor_valid,
node->gps_how_tow.week,
node->gps_how_tow.sec,
fallback_week,
(double)how_tow_sec);
node->gps_how_tow.week = anchor_week;
node->gps_how_tow.sec = (double)how_tow_sec;
node->gps_how_abs_sample = event->abs_sample_raw;
node->gps_how_corrected_abs_sample = event->corrected_abs_sample;
node->gps_how_tick = event->tick;
node->gps_how_anchor_valid = true;
GPS_LOG(DEBUG_TRACE, GPS_LOG_CAT_NAV,
"HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
trck_ip_no, event->prn, anchor_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
/* Before the first SF1, fallback_week is only a placeholder. Validate
* TOW/sample continuity in that temporary domain, then atomically
* upgrade to the parity-checked LNAV week carried by SF1. */
int continuity_week =
((subframe_id == 1u) && !node->gps_how_week_valid &&
node->gps_how_anchor_valid) ?
node->gps_how_tow.week : anchor_week;
bool accept_anchor = !node->gps_how_anchor_valid ||
gps_how_candidate_consistent(
node->gps_how_tow.week, node->gps_how_tow.sec,
node->gps_how_abs_sample,
node->gps_how_corrected_abs_sample,
continuity_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
bool recovered_anchor = false;
if(!accept_anchor && node->gps_pending_how_valid)
{
accept_anchor = gps_how_candidate_consistent(
node->gps_pending_how_tow.week,
node->gps_pending_how_tow.sec,
node->gps_pending_how_abs_sample,
node->gps_pending_how_corrected_abs_sample,
anchor_week, (double)how_tow_sec,
event->abs_sample_raw, event->corrected_abs_sample);
recovered_anchor = accept_anchor;
}
if(accept_anchor)
{
gps_commit_how_anchor(node, anchor_week,
(double)how_tow_sec, event,
node->gps_how_week_valid ||
(subframe_id == 1u));
GPS_LOG(recovered_anchor ? DEBUG_WARNING : DEBUG_TRACE,
GPS_LOG_CAT_NAV,
"%s HOW anchor: trk=%u prn=%u week=%d tow=%.3f abs=%u corrected=%.6f\r\n",
recovered_anchor ? "recovered" : "accepted",
trck_ip_no, event->prn, anchor_week,
(double)how_tow_sec, event->abs_sample_raw,
event->corrected_abs_sample);
}
else
{
node->gps_pending_how_tow.week = anchor_week;
node->gps_pending_how_tow.sec = (double)how_tow_sec;
node->gps_pending_how_abs_sample = event->abs_sample_raw;
node->gps_pending_how_corrected_abs_sample =
event->corrected_abs_sample;
node->gps_pending_how_valid = true;
gps_runtime_diagnostics.how_anchor_rejected_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"HOW discontinuity rejected: trk=%u prn=%u current=%d/%.3f candidate=%d/%.3f rejects=%u\r\n",
trck_ip_no, event->prn,
node->gps_how_tow.week, node->gps_how_tow.sec,
anchor_week, (double)how_tow_sec,
gps_runtime_diagnostics.how_anchor_rejected_total);
}
}
else
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK, "invalid subframe id: trk=%u subframe=%u\r\n", trck_ip_no, subframe_id);
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_TRK,
"invalid subframe/HOW: trk=%u subframe=%u tow=%u\r\n",
trck_ip_no, subframe_id, how_tow_sec);
}
gps_mark_node_fresh(node, event->status, event->tick);
@@ -1032,32 +1191,58 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, trck_ip_no);
if(node != NULL)
{
unsigned int nav_solve_event_count = event->nav_solve_event_count;
unsigned int captured_epoch_count = 1u;
if(event->pps_capture_valid)
{
uint32_t previous_capture_sequence = node->gps_last_pps_capture_sequence;
if(previous_capture_sequence != 0u)
{
nav_solve_event_count = event->pps_capture_sequence - previous_capture_sequence;
if(nav_solve_event_count == 0u)
uint32_t sequence_delta =
event->pps_capture_sequence - previous_capture_sequence;
if(sequence_delta == 0u)
{
return;
}
if(nav_solve_event_count > event->nav_solve_event_count)
bool sequence_consistent =
event->pps_counter > node->gps_last_nav_solve_pps_counter;
if(sequence_consistent)
{
gps_runtime_diagnostics.nav_epoch_coalesced_total +=
nav_solve_event_count - event->nav_solve_event_count;
uint64_t counter_delta =
event->pps_counter -
node->gps_last_nav_solve_pps_counter;
uint64_t expected_delta =
(counter_delta + (GPS_NAV_CAPTURE_PERIOD_TICKS / 2u)) /
GPS_NAV_CAPTURE_PERIOD_TICKS;
sequence_consistent =
(expected_delta != 0u) &&
(expected_delta <= UINT32_MAX) &&
(sequence_delta == (uint32_t)expected_delta);
}
if(sequence_consistent)
{
captured_epoch_count = sequence_delta;
if(sequence_delta > 1u)
{
gps_runtime_diagnostics.nav_epoch_coalesced_total +=
sequence_delta - 1u;
}
}
else
{
gps_runtime_diagnostics.pps_sequence_discontinuity_total++;
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"recovered %u coalesced nav-solve epochs: trk=%u prn=%u\r\n",
nav_solve_event_count, trck_ip_no, event->prn);
"PPS capture sequence discontinuity: trk=%u prn=%u previous=%u current=%u total=%u\r\n",
trck_ip_no, event->prn,
previous_capture_sequence,
event->pps_capture_sequence,
gps_runtime_diagnostics.pps_sequence_discontinuity_total);
}
}
}
else if(node->gps_last_pps_capture_sequence != 0u)
{
/* Keep the sequence baseline moving on the rare APB capture race. A
* later valid capture still recovers any epochs coalesced meanwhile. */
node->gps_last_pps_capture_sequence += nav_solve_event_count;
/* Capture sequencing is transport metadata. Rebase it even if the
* decoded HOW below is unavailable or rejected. */
node->gps_last_nav_solve_pps_counter = event->pps_counter;
node->gps_last_pps_capture_sequence =
event->pps_capture_sequence;
}
if(!node->gps_how_anchor_valid)
@@ -1091,56 +1276,94 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
gps_frame_parsed->gps_ephems.tow.week = tow_week;
gps_frame_parsed->gps_ephems.tow.sec = tow_sec;
gps_frame_parsed->gps_ephems.abs_sample = event->corrected_abs_sample;
node->gps_nav_solve_sequence += nav_solve_event_count;
/* A one-entry hardware mailbox carries only the newest measurement.
* Missed capture pulses are diagnostic counts, not synthetic samples. */
node->gps_nav_solve_sequence++;
node->gps_last_nav_solve_tow.week = tow_week;
node->gps_last_nav_solve_tow.sec = tow_sec;
node->gps_last_nav_solve_abs_sample = event->abs_sample_raw;
node->gps_last_nav_solve_corrected_abs_sample = event->corrected_abs_sample;
node->gps_last_nav_solve_local_ticks = event->local_timestamp_ticks;
if(event->pps_capture_valid)
{
node->gps_last_nav_solve_pps_counter = event->pps_counter;
node->gps_last_pps_capture_sequence = event->pps_capture_sequence;
}
gps_mark_node_fresh(node, event->status, event->tick);
uint64_t epoch_id;
if(!gps_nav_epoch_id(tow_week, tow_sec,
GPS_NAV_SOLVE_PERIOD_MS, &epoch_id))
if(!event->pps_capture_valid)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"nav solve has invalid epoch id: trk=%u week=%d tow=%.6f\r\n",
trck_ip_no, tow_week, tow_sec);
"nav solve omitted after incoherent PPS capture read: trk=%u prn=%u\r\n",
trck_ip_no, event->prn);
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,
gps_nav_ready_channel_mask());
&gps_nav_epoch_assembly,
event->pps_counter,
event->local_timestamp_ticks,
trck_ip_no,
gps_nav_ready_channel_mask(),
GPS_NAV_CAPTURE_PERIOD_TICKS,
GPS_NAV_CAPTURE_WINDOW_TICKS,
GPS_NAV_HOLDOFF_LOCAL_TICKS);
if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED)
{
gps_runtime_diagnostics.nav_epoch_incomplete_total++;
if(gps_mask_count(previous_received) >= MIN_NEEDED_SATELLITE)
{
gps_nav_publish_active_snapshot(previous_received, false);
}
else
{
gps_nav_discard_active_snapshot(false);
}
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV capture epoch advanced: old_expected=0x%x old_received=0x%x new_counter=%lu first_trk=%u\r\n",
previous_expected, previous_received,
(unsigned long)event->pps_counter, trck_ip_no);
}
if((assembly_result == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED) ||
(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE))
{
uint32_t channel_mask = 1u << trck_ip_no;
gps_nav_active_snapshot.channel_mask |= channel_mask;
gps_nav_active_snapshot.prn[trck_ip_no] = event->prn;
gps_nav_active_snapshot.tow[trck_ip_no].week = tow_week;
gps_nav_active_snapshot.tow[trck_ip_no].sec = tow_sec;
gps_nav_active_snapshot.raw_sample[trck_ip_no] =
event->abs_sample_raw;
gps_nav_active_snapshot.corrected_abs_sample[trck_ip_no] =
event->corrected_abs_sample;
gps_nav_active_snapshot.pps_counter[trck_ip_no] =
event->pps_counter;
gps_nav_active_snapshot.pps_capture_sequence[trck_ip_no] =
event->pps_capture_sequence;
gps_nav_active_snapshot.local_timestamp_ticks[trck_ip_no] =
event->local_timestamp_ticks;
}
if(assembly_result == GPS_NAV_EPOCH_EVENT_COMPLETE)
{
gps_nav_solve_period_pending = true;
gps_nav_publish_active_snapshot(
gps_nav_epoch_assembly.received_mask, true);
}
else if(assembly_result == GPS_NAV_EPOCH_EVENT_STALE)
{
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"stale nav epoch interrupt ignored: trk=%u epoch=%lu active=%lu\r\n",
trck_ip_no, (unsigned long)epoch_id,
(unsigned long)gps_nav_epoch_assembly.epoch_id);
"stale NAV capture ignored: trk=%u counter=%lu active_min=%lu\r\n",
trck_ip_no, (unsigned long)event->pps_counter,
(unsigned long)gps_nav_epoch_assembly.minimum_capture_counter);
}
else if(assembly_result == GPS_NAV_EPOCH_EVENT_RESTARTED)
else if(assembly_result == GPS_NAV_EPOCH_EVENT_OUTLIER)
{
/* 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,
"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);
"NAV capture outlier ignored: trk=%u counter=%lu active=[%lu,%lu]\r\n",
trck_ip_no, (unsigned long)event->pps_counter,
(unsigned long)gps_nav_epoch_assembly.minimum_capture_counter,
(unsigned long)gps_nav_epoch_assembly.maximum_capture_counter);
}
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",
@@ -1149,7 +1372,7 @@ static void gps_process_tracking_nav_solve(const gps_isr_event_t* event)
event->rem_code_phase,
gps_frame_parsed->gps_ephems.abs_sample,
event->nav_solve_timer,
nav_solve_event_count,
captured_epoch_count,
tow_delta_sec,
gps_frame_parsed->gps_ephems.tow.sec,
node->gps_nav_solve_sequence);
@@ -1243,7 +1466,6 @@ static void gps_process_shadow_tracking_events(void)
}
if(shadow.nav_solve_pending)
{
shadow.nav_solve_event.nav_solve_event_count = shadow.nav_solve_event_count;
gps_process_tracking_event(&shadow.nav_solve_event);
}
if(shadow.misc_pending)
soc/software/gps_system/src/gps/gps_ip.c
+86 -132
View File
@@ -570,7 +570,6 @@ static void gps_enable_nav_solve_for_snapshot_locked(const unsigned char snapsho
if(!gps_channel_nav_solve_enabled(ip_no))
{
gps_set_channel_nav_solve_mode(ip_no);
node->gps_nav_solve_consumed_sequence = node->gps_nav_solve_sequence;
GPS_LOG(DEBUG_INFO, GPS_LOG_CAT_TRK,
"tracking channel %u enabled nav solve; nav bits remain enabled\r\n",
ip_no);
@@ -599,9 +598,7 @@ static unsigned int gps_collect_tracked_channels_locked(
static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
bool require_nav_solve_mode,
bool require_nav_sequence)
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT])
{
unsigned int count = 0;
@@ -611,12 +608,7 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
{
unsigned char ip_no = iterator->tracking_ip_no;
gps_ephemeris_t* eph = (gps_ephemeris_t*)iterator->data_p;
if(gps_node_ready_for_position(iterator) &&
(!require_nav_solve_mode || gps_channel_nav_solve_enabled(ip_no)) &&
(!require_nav_sequence ||
((eph != NULL) &&
(iterator->gps_nav_solve_sequence !=
iterator->gps_nav_solve_consumed_sequence))))
if(gps_node_ready_for_position(iterator) && (eph != NULL))
{
if(gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
@@ -626,19 +618,10 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
continue;
}
snapshot[count] = *eph;
if(require_nav_sequence)
if(snapshot_raw_samples != NULL)
{
/* Nav bits may update the shared ephemeris between nav-solve
* epochs. Position measurements must use the timing latched by
* the nav-solve event, not the shared nav-bit fields. */
snapshot[count].tow = iterator->gps_last_nav_solve_tow;
snapshot[count].abs_sample =
iterator->gps_last_nav_solve_corrected_abs_sample;
if(snapshot_raw_samples != NULL)
{
snapshot_raw_samples[count] =
iterator->gps_last_nav_solve_abs_sample;
}
snapshot_raw_samples[count] =
iterator->gps_last_nav_solve_abs_sample;
}
snapshot_ips[count] = ip_no;
count++;
@@ -651,21 +634,70 @@ static unsigned int gps_collect_nav_snapshot_locked(gps_ephemeris_t snapshot[GPS
return count;
}
static unsigned int gps_collect_completed_epoch_locked(
const gps_nav_epoch_snapshot_t* epoch,
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT])
{
if(epoch == NULL)
{
return 0u;
}
unsigned int count = 0u;
for(unsigned char ip_no = 0u ; ip_no < GPS_TRCK_IP_COUNT ; ip_no++)
{
if((epoch->channel_mask & (1u << ip_no)) == 0u)
{
continue;
}
volatile gps_ephem_ll_t* node =
get_element_by_ip_number(&tracking_node, ip_no);
gps_ephemeris_t* eph =
(node != NULL) ? (gps_ephemeris_t*)node->data_p : NULL;
if((node == NULL) || (eph == NULL) ||
!gps_node_ready_for_position(node) ||
!gps_channel_nav_solve_enabled(ip_no) ||
(eph->SVID != epoch->prn[ip_no]) ||
gps_snapshot_has_svid(snapshot, count, eph->SVID))
{
continue;
}
snapshot[count] = *eph;
snapshot[count].tow = epoch->tow[ip_no];
snapshot[count].abs_sample =
epoch->corrected_abs_sample[ip_no];
snapshot_ips[count] = ip_no;
snapshot_pps_counters[count] =
epoch->pps_counter[ip_no];
snapshot_pps_sequences[count] =
epoch->pps_capture_sequence[ip_no];
snapshot_local_timestamps[count] =
epoch->local_timestamp_ticks[ip_no];
snapshot_raw_samples[count] =
epoch->raw_sample[ip_no];
count++;
}
return count;
}
static unsigned int gps_select_coherent_nav_snapshot(
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT],
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT],
unsigned int candidate_count,
uint32_t* selected_mask_out)
unsigned int candidate_count)
{
if((candidate_count == 0u) ||
(candidate_count > GPS_NAV_EPOCH_MAX_CANDIDATES))
{
*selected_mask_out = 0u;
return 0u;
}
@@ -683,7 +715,6 @@ static unsigned int gps_select_coherent_nav_snapshot(
GPS_NAV_TOW_CLUSTER_TOLERANCE_SEC,
&selection))
{
*selected_mask_out = 0u;
return 0u;
}
@@ -702,7 +733,6 @@ static unsigned int gps_select_coherent_nav_snapshot(
{
snapshot[destination] = snapshot[source];
snapshot_ips[destination] = snapshot_ips[source];
snapshot_sequences[destination] = snapshot_sequences[source];
snapshot_pps_counters[destination] =
snapshot_pps_counters[source];
snapshot_pps_sequences[destination] =
@@ -720,28 +750,9 @@ static unsigned int gps_select_coherent_nav_snapshot(
destination++;
}
*selected_mask_out = selection.selected_mask;
return destination;
}
static void gps_capture_nav_metadata_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT],
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT],
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT],
unsigned int satellite_count)
{
for(unsigned int i = 0 ; i < satellite_count ; i++)
{
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, snapshot_ips[i]);
snapshot_sequences[i] = (node != NULL) ? node->gps_nav_solve_sequence : 0u;
snapshot_pps_counters[i] = (node != NULL) ? node->gps_last_nav_solve_pps_counter : 0u;
snapshot_pps_sequences[i] = (node != NULL) ? node->gps_last_pps_capture_sequence : 0u;
snapshot_local_timestamps[i] =
(node != NULL) ? node->gps_last_nav_solve_local_ticks : 0u;
}
}
static uint64_t gps_nav_median_local_timestamp(
const uint64_t timestamps[GPS_TRCK_IP_COUNT], unsigned int count)
{
@@ -765,48 +776,6 @@ static uint64_t gps_nav_median_local_timestamp(
return (valid_count != 0u) ? sorted[valid_count / 2u] : 0u;
}
static void gps_consume_nav_sequences_locked(const unsigned char snapshot_ips[GPS_TRCK_IP_COUNT],
const uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT],
unsigned int satellite_count)
{
for(unsigned int i = 0 ; i < satellite_count ; i++)
{
volatile gps_ephem_ll_t* node = get_element_by_ip_number(&tracking_node, snapshot_ips[i]);
if(node != NULL)
{
/* Consume only the epoch copied into this solution. A newer sequence,
* if present, remains different from consumed and is solved next. */
node->gps_nav_solve_consumed_sequence = snapshot_sequences[i];
}
}
}
static void gps_consume_rejected_nav_sequences_locked(
const unsigned char candidate_ips[GPS_TRCK_IP_COUNT],
const uint32_t candidate_sequences[GPS_TRCK_IP_COUNT],
unsigned int candidate_count,
uint32_t selected_mask)
{
for(unsigned int i = 0u ; i < candidate_count ; i++)
{
if((selected_mask & (1u << i)) != 0u)
{
continue;
}
volatile gps_ephem_ll_t* node =
get_element_by_ip_number(&tracking_node, candidate_ips[i]);
if(node != NULL)
{
/*
* Consume exactly the rejected snapshot. If an interrupt has
* already produced a newer sequence, it remains different from
* this consumed value and is preserved for the next epoch.
*/
node->gps_nav_solve_consumed_sequence = candidate_sequences[i];
}
}
}
static bool nav_software_from_snapshot(const gps_ephemeris_t* ephemeris,
const uint64_t pps_counters[GPS_TRCK_IP_COUNT],
const uint32_t pps_capture_sequences[GPS_TRCK_IP_COUNT],
@@ -1088,14 +1057,13 @@ bool calculate_position(gps_nav_solution_t* solution_out,
bool result_ready = false;
gps_ephemeris_t snapshot[GPS_TRCK_IP_COUNT];
unsigned char snapshot_ips[GPS_TRCK_IP_COUNT];
uint32_t snapshot_sequences[GPS_TRCK_IP_COUNT];
uint64_t snapshot_pps_counters[GPS_TRCK_IP_COUNT];
uint32_t snapshot_pps_sequences[GPS_TRCK_IP_COUNT];
uint64_t snapshot_local_timestamps[GPS_TRCK_IP_COUNT];
uint32_t snapshot_raw_samples[GPS_TRCK_IP_COUNT];
gps_nav_epoch_snapshot_t completed_epoch;
unsigned int satellite_count = 0;
unsigned int candidate_count = 0;
uint32_t selected_candidate_mask = 0u;
bool run_nav = false;
bool initial_run = false;
unsigned int ready_count;
@@ -1110,13 +1078,14 @@ bool calculate_position(gps_nav_solution_t* solution_out,
#ifdef EKF_ENABLE
(void)g_ekf;
#endif
bool nav_period_ready = gps_take_nav_solve_period_ready();
bool nav_period_ready =
gps_take_nav_epoch_snapshot(&completed_epoch);
uint64_t nav_period_start_cycles = nav_period_ready ? gps_read_cycle_counter() : 0u;
uint64_t irq_state = bsp_irq_save();
ready_count = gps_collect_nav_snapshot_locked(
snapshot, snapshot_ips, NULL, false, false);
snapshot, snapshot_ips, NULL);
tracked_count = gps_collect_tracked_channels_locked(
GPS_DEBUG_ENABLED(DEBUG_CHANNELS) ? tracked_prns : NULL);
@@ -1131,13 +1100,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
gps_enable_nav_solve_for_snapshot_locked(snapshot_ips, ready_count);
if(nav_period_ready)
{
candidate_count = gps_collect_nav_snapshot_locked(
snapshot, snapshot_ips, snapshot_raw_samples, true, true);
gps_capture_nav_metadata_locked(snapshot_ips, snapshot_sequences,
snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps,
candidate_count);
candidate_count = gps_collect_completed_epoch_locked(
&completed_epoch,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples);
initial_run = !gps_initial_position_valid;
}
}
@@ -1146,22 +1113,11 @@ bool calculate_position(gps_nav_solution_t* solution_out,
if(nav_period_ready && (candidate_count != 0u))
{
/*
* Preserve the pre-compaction identity of rejected candidates so their
* exact sequence can be retired without touching a newer interrupt.
*/
unsigned char candidate_ips[GPS_TRCK_IP_COUNT];
uint32_t candidate_sequences[GPS_TRCK_IP_COUNT];
memcpy(candidate_ips, snapshot_ips,
candidate_count * sizeof(candidate_ips[0]));
memcpy(candidate_sequences, snapshot_sequences,
candidate_count * sizeof(candidate_sequences[0]));
satellite_count = gps_select_coherent_nav_snapshot(
snapshot, snapshot_ips, snapshot_sequences,
snapshot_pps_counters, snapshot_pps_sequences,
snapshot, snapshot_ips, snapshot_pps_counters,
snapshot_pps_sequences,
snapshot_local_timestamps, snapshot_raw_samples,
candidate_count, &selected_candidate_mask);
candidate_count);
coherent_nav_count = satellite_count;
run_nav = satellite_count >= MIN_NEEDED_SATELLITE;
@@ -1169,11 +1125,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
{
unsigned int excluded_count = candidate_count - satellite_count;
gps_nav_epoch_excluded_total += excluded_count;
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_rejected_nav_sequences_locked(
candidate_ips, candidate_sequences, candidate_count,
selected_candidate_mask);
bsp_irq_restore(consume_irq_state);
GPS_LOG(DEBUG_WARNING, GPS_LOG_CAT_NAV,
"NAV coherent subset selected: candidates=%u selected=%u excluded=%u excluded_total=%u\r\n",
candidate_count, satellite_count, excluded_count,
@@ -1181,10 +1132,6 @@ bool calculate_position(gps_nav_solution_t* solution_out,
}
else if(!run_nav)
{
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_rejected_nav_sequences_locked(
candidate_ips, candidate_sequences, candidate_count, 0u);
bsp_irq_restore(consume_irq_state);
if(candidate_count >= MIN_NEEDED_SATELLITE)
{
gps_nav_epoch_reject_total++;
@@ -1202,8 +1149,15 @@ bool calculate_position(gps_nav_solution_t* solution_out,
(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, "tracked channels=%u ready channels=%u coherent_nav=%u\r\n",
tracked_count, ready_count, coherent_nav_count);
gps_runtime_diagnostics_t status_diagnostics;
gps_get_runtime_diagnostics(&status_diagnostics);
GPS_LOG(DEBUG_DEBUG, GPS_LOG_CAT_NAV,
"tracked channels=%u ready channels=%u coherent_nav=%u incomplete=%u snapshot_overwrite=%u pps_discontinuity=%u how_reject=%u\r\n",
tracked_count, ready_count, coherent_nav_count,
status_diagnostics.nav_epoch_incomplete_total,
status_diagnostics.nav_epoch_snapshot_overwrite_total,
status_diagnostics.pps_sequence_discontinuity_total,
status_diagnostics.how_anchor_rejected_total);
last_nav_status_tracked_count = tracked_count;
last_nav_status_ready_count = ready_count;
last_nav_status_coherent_count = coherent_nav_count;
@@ -1241,13 +1195,8 @@ bool calculate_position(gps_nav_solution_t* solution_out,
result_ready = true;
}
}
/* A completed hardware epoch is attempted once. Retrying the same
* measurements after a WLS failure would couple solution cadence to an
* unrelated foreground wake-up. */
uint64_t consume_irq_state = bsp_irq_save();
gps_consume_nav_sequences_locked(
snapshot_ips, snapshot_sequences, satellite_count);
bsp_irq_restore(consume_irq_state);
/* The completed snapshot is handed out exactly once. Retrying after a
* WLS failure would couple solution cadence to a foreground wake-up. */
uint64_t foreground_cycles = gps_read_cycle_counter() - nav_period_start_cycles;
uint64_t period_budget_cycles = (GPS_PROFILE_CPU_HZ * GPS_NAV_SOLVE_PERIOD_MS) / 1000u;
bool period_overrun = foreground_cycles >= period_budget_cycles;
@@ -1260,6 +1209,7 @@ bool calculate_position(gps_nav_solution_t* solution_out,
"fde=%u valid=%u budget_cycles=%lu overrun=%u math_corr=%.6lf "
"math_clock_corr=%.6lf "
"isr_overflow=%u plic_repeat=%u drain_limit=%u coalesced=%u lost=%u "
"pps_discontinuity=%u epoch_incomplete=%u snapshot_overwrite=%u how_reject=%u "
"epoch_excluded=%u epoch_rejected=%u\r\n",
GPS_NAV_SOLVE_PERIOD_MS,
(unsigned long)GPS_PROFILE_CPU_HZ,
@@ -1283,6 +1233,10 @@ bool calculate_position(gps_nav_solution_t* solution_out,
runtime_diagnostics.plic_drain_limit_total,
runtime_diagnostics.nav_epoch_coalesced_total,
runtime_diagnostics.satellite_lost_total,
runtime_diagnostics.pps_sequence_discontinuity_total,
runtime_diagnostics.nav_epoch_incomplete_total,
runtime_diagnostics.nav_epoch_snapshot_overwrite_total,
runtime_diagnostics.how_anchor_rejected_total,
gps_nav_epoch_excluded_total,
gps_nav_epoch_reject_total);
(void)foreground_cycles;
soc/software/gps_system/src/gps/gps_nav_epoch.c
+73 -40
View File
@@ -5,7 +5,6 @@
#define GPS_WEEK_SECONDS 604800.0
#define GPS_HALF_WEEK_SECONDS 302400.0
#define GPS_WEEK_MILLISECONDS 604800000ULL
static double gps_nav_epoch_tow_delta(
const gps_nav_epoch_key_t* first,
@@ -180,40 +179,16 @@ bool gps_nav_epoch_select(
return selection->selected_count != 0u;
}
bool gps_nav_epoch_id(
int week,
double tow_sec,
unsigned int period_ms,
uint64_t* epoch_id)
{
if((epoch_id == NULL) || (week < 0) || (period_ms == 0u) ||
((GPS_WEEK_MILLISECONDS % period_ms) != 0u) ||
!isfinite(tow_sec) || (tow_sec < 0.0) ||
(tow_sec >= GPS_WEEK_SECONDS))
{
return false;
}
uint64_t epochs_per_week = GPS_WEEK_MILLISECONDS / period_ms;
double epoch_in_week = (tow_sec * 1000.0) / (double)period_ms;
uint64_t rounded_epoch = (uint64_t)llround(epoch_in_week);
if(rounded_epoch == epochs_per_week)
{
week++;
rounded_epoch = 0u;
}
*epoch_id = ((uint64_t)(unsigned int)week * epochs_per_week) +
rounded_epoch;
return true;
}
void gps_nav_epoch_assembly_reset(gps_nav_epoch_assembly_t* assembly)
{
if(assembly == NULL)
{
return;
}
assembly->epoch_id = 0u;
assembly->first_capture_counter = 0u;
assembly->minimum_capture_counter = 0u;
assembly->maximum_capture_counter = 0u;
assembly->deadline_local_ticks = 0u;
assembly->expected_mask = 0u;
assembly->received_mask = 0u;
assembly->active = false;
@@ -230,12 +205,21 @@ bool gps_nav_epoch_assembly_complete(
gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
gps_nav_epoch_assembly_t* assembly,
uint64_t epoch_id,
uint64_t capture_counter,
uint64_t local_timestamp_ticks,
unsigned int channel,
uint32_t ready_mask)
uint32_t ready_mask,
uint64_t capture_period_ticks,
uint64_t sync_window_ticks,
uint64_t holdoff_local_ticks)
{
if((assembly == NULL) ||
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES))
(channel >= GPS_NAV_EPOCH_MAX_CANDIDATES) ||
(capture_counter == 0u) ||
(capture_period_ticks == 0u) ||
(sync_window_ticks == 0u) ||
(sync_window_ticks >= (capture_period_ticks / 2u)) ||
(holdoff_local_ticks == 0u))
{
return GPS_NAV_EPOCH_EVENT_INVALID;
}
@@ -252,20 +236,40 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
bool restarted = false;
if(assembly->active)
{
if(epoch_id < assembly->epoch_id)
uint64_t minimum = assembly->minimum_capture_counter;
uint64_t maximum = assembly->maximum_capture_counter;
uint64_t next_minimum =
(capture_counter < minimum) ? capture_counter : minimum;
uint64_t next_maximum =
(capture_counter > maximum) ? capture_counter : maximum;
if((next_maximum - next_minimum) > sync_window_ticks)
{
return GPS_NAV_EPOCH_EVENT_STALE;
}
if(epoch_id > assembly->epoch_id)
{
gps_nav_epoch_assembly_reset(assembly);
restarted = true;
uint64_t epoch_separation = capture_period_ticks / 2u;
if((capture_counter > maximum) &&
((capture_counter - maximum) >= epoch_separation))
{
gps_nav_epoch_assembly_reset(assembly);
restarted = true;
}
else if((capture_counter < minimum) &&
((minimum - capture_counter) >= epoch_separation))
{
return GPS_NAV_EPOCH_EVENT_STALE;
}
else
{
return GPS_NAV_EPOCH_EVENT_OUTLIER;
}
}
}
if(!assembly->active)
{
assembly->epoch_id = epoch_id;
assembly->first_capture_counter = capture_counter;
assembly->minimum_capture_counter = capture_counter;
assembly->maximum_capture_counter = capture_counter;
assembly->deadline_local_ticks =
local_timestamp_ticks + holdoff_local_ticks;
assembly->expected_mask = ready_mask;
assembly->received_mask = 0u;
assembly->active = true;
@@ -280,6 +284,14 @@ gps_nav_epoch_event_result_t gps_nav_epoch_assembly_push(
return GPS_NAV_EPOCH_EVENT_DUPLICATE;
}
if(capture_counter < assembly->minimum_capture_counter)
{
assembly->minimum_capture_counter = capture_counter;
}
if(capture_counter > assembly->maximum_capture_counter)
{
assembly->maximum_capture_counter = capture_counter;
}
assembly->received_mask |= channel_mask;
if(gps_nav_epoch_assembly_complete(assembly))
{
@@ -310,3 +322,24 @@ bool gps_nav_epoch_assembly_remove_channel(
}
return gps_nav_epoch_assembly_complete(assembly);
}
bool gps_nav_epoch_assembly_usable(
const gps_nav_epoch_assembly_t* assembly,
unsigned int minimum_channels)
{
if((assembly == NULL) || !assembly->active ||
(minimum_channels == 0u))
{
return false;
}
return gps_nav_epoch_mask_count(assembly->received_mask) >=
minimum_channels;
}
bool gps_nav_epoch_assembly_expired(
const gps_nav_epoch_assembly_t* assembly,
uint64_t now_local_ticks)
{
return (assembly != NULL) && assembly->active &&
(now_local_ticks >= assembly->deadline_local_ticks);
}
soc/software/gps_system/src/gps/gps_time_anchor.c
+69
View File
@@ -42,6 +42,28 @@ int gps_time_resolve_how_week(bool previous_valid,
return week;
}
int gps_time_extend_lnav_week(bool previous_valid,
int previous_week,
unsigned int lnav_week)
{
int week = (int)(lnav_week & 0x3ffu);
if(!previous_valid || (previous_week < 0))
{
return week;
}
int candidate = (previous_week & ~0x3ff) | week;
if((candidate - previous_week) > 512)
{
candidate -= 1024;
}
else if((previous_week - candidate) > 512)
{
candidate += 1024;
}
return candidate;
}
bool gps_time_from_sample_anchor(int anchor_week,
double anchor_tow_sec,
uint32_t anchor_raw_sample,
@@ -86,3 +108,50 @@ bool gps_time_from_sample_anchor(int anchor_week,
}
return true;
}
bool gps_time_anchors_consistent(int first_week,
double first_tow_sec,
uint32_t first_raw_sample,
double first_corrected_sample,
int second_week,
double second_tow_sec,
uint32_t second_raw_sample,
double second_corrected_sample,
double samples_per_second,
double max_gap_sec,
double tolerance_sec)
{
if((first_week < 0) || (second_week < 0) ||
!isfinite(first_tow_sec) || !isfinite(second_tow_sec) ||
!isfinite(first_corrected_sample) ||
!isfinite(second_corrected_sample) ||
(first_tow_sec < 0.0) ||
(first_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
(second_tow_sec < 0.0) ||
(second_tow_sec >= GPS_TIME_WEEK_SECONDS) ||
!(samples_per_second > 0.0) || !(max_gap_sec > 0.0) ||
!(tolerance_sec >= 0.0))
{
return false;
}
int32_t raw_delta = (int32_t)(second_raw_sample - first_raw_sample);
double first_fraction =
first_corrected_sample - (double)first_raw_sample;
double second_fraction =
second_corrected_sample - (double)second_raw_sample;
double sample_elapsed =
((double)raw_delta + second_fraction - first_fraction) /
samples_per_second;
if(!isfinite(sample_elapsed) || (sample_elapsed < -0.001) ||
(sample_elapsed > max_gap_sec))
{
return false;
}
double gps_elapsed =
((double)(second_week - first_week) * GPS_TIME_WEEK_SECONDS) +
second_tow_sec - first_tow_sec;
return isfinite(gps_elapsed) &&
(fabs(gps_elapsed - sample_elapsed) <= tolerance_sec);
}
soc/software/gps_system/src/gps/linked_list.c
+6 -1
View File
@@ -66,8 +66,13 @@ bool insert_element(volatile gps_ephem_ll_t** node,
new_node->gps_how_corrected_abs_sample = 0.0;
new_node->gps_how_tick = 0u;
new_node->gps_how_anchor_valid = false;
new_node->gps_how_week_valid = false;
new_node->gps_pending_how_tow.week = 0;
new_node->gps_pending_how_tow.sec = 0.0;
new_node->gps_pending_how_abs_sample = 0u;
new_node->gps_pending_how_corrected_abs_sample = 0.0;
new_node->gps_pending_how_valid = false;
new_node->gps_nav_solve_sequence = 0u;
new_node->gps_nav_solve_consumed_sequence = 0u;
new_node->gps_last_nav_solve_tow.week = 0;
new_node->gps_last_nav_solve_tow.sec = 0.0;
new_node->gps_last_nav_solve_abs_sample = 0u;
soc/software/gps_system/tests/Makefile
+6 -1
View File
@@ -3,7 +3,8 @@ CFLAGS ?= -O2 -std=c11 -Wall -Wextra -Werror
ROOT := ..
TARGETS := test_imu_calibration test_gps_nav_epoch \
test_gps_nav_epoch_coalescing test_plic_warm_reset
test_gps_nav_epoch_coalescing test_gps_time_anchor \
test_plic_warm_reset
.PHONY: all run clean
@@ -18,6 +19,9 @@ test_gps_nav_epoch: test_gps_nav_epoch.c $(ROOT)/src/gps/gps_nav_epoch.c
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_gps_time_anchor: test_gps_time_anchor.c $(ROOT)/src/gps/gps_time_anchor.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 $@
@@ -25,6 +29,7 @@ run: $(TARGETS)
./test_imu_calibration
./test_gps_nav_epoch
./test_gps_nav_epoch_coalescing
./test_gps_time_anchor
./test_plic_warm_reset
clean:
soc/software/gps_system/tests/test_gps_nav_epoch.c
+174 -352
View File
@@ -5,8 +5,11 @@
#include <stdio.h>
#include <string.h>
#define SAMPLE_WINDOW 409200u
#define TOW_TOLERANCE 0.001
#define SAMPLE_WINDOW 409200u
#define TOW_TOLERANCE 0.001
#define CAPTURE_PERIOD_TICKS 25000000ULL
#define CAPTURE_WINDOW_TICKS 3125000ULL
#define HOLDOFF_LOCAL_TICKS 15000ULL
static int failures;
@@ -29,167 +32,17 @@ static gps_nav_epoch_selection_t select_epoch(
return selection;
}
static unsigned int mask_count(uint32_t mask)
static gps_nav_epoch_event_result_t push_capture(
gps_nav_epoch_assembly_t* assembly,
uint64_t capture_counter,
uint64_t local_ticks,
unsigned int channel,
uint32_t ready_mask)
{
unsigned int count = 0u;
while(mask != 0u)
{
count += mask & 1u;
mask >>= 1u;
}
return count;
}
static void epoch_id_rounding_and_week_wrap(void)
{
uint64_t before_wrap;
uint64_t after_wrap;
uint64_t same_epoch_a;
uint64_t same_epoch_b;
CHECK(gps_nav_epoch_id(2300, 604799.9999, 200u, &before_wrap),
"epoch id rejected a valid pre-wrap TOW");
CHECK(gps_nav_epoch_id(2301, 0.0001, 200u, &after_wrap),
"epoch id rejected a valid post-wrap TOW");
CHECK(before_wrap == after_wrap,
"week rollover split one rounded navigation epoch");
CHECK(gps_nav_epoch_id(2301, 123.3997, 200u, &same_epoch_a) &&
gps_nav_epoch_id(2301, 123.4003, 200u, &same_epoch_b) &&
(same_epoch_a == same_epoch_b),
"sub-millisecond TOW skew split one navigation epoch");
CHECK(!gps_nav_epoch_id(2301, NAN, 200u, &same_epoch_a),
"non-finite TOW produced an epoch id");
CHECK(!gps_nav_epoch_id(2301, 1.0, 333u, &same_epoch_a),
"non-integral epochs per GPS week were accepted");
}
static void assembly_waits_for_every_ready_channel(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0xafu;
const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 1000u, order[i], ready_mask);
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"assembly completed before every ready channel arrived");
}
CHECK(assembly.expected_mask == ready_mask,
"assembly changed its frozen ready mask");
CHECK(assembly.received_mask == ready_mask,
"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;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"first channel did not start assembly");
CHECK(gps_nav_epoch_assembly_push(&assembly, 2000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"duplicate channel interrupt was not idempotent");
CHECK(gps_nav_epoch_assembly_push(&assembly, 1999u, 1u, 0x0fu) ==
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_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)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"disconnect scenario did not start");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 1u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"second interrupt unexpectedly completed");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
"pending-channel disconnect completed too early");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 2u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"post-disconnect interrupt was rejected");
CHECK(gps_nav_epoch_assembly_push(&assembly, 3000u, 3u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_COMPLETE,
"unlock did not release the removed channel from the epoch");
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 3001u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"reconnect epoch did not start");
for(unsigned int channel = 1u ; channel < 5u ; channel++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 3001u, channel, 0x1fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 4u),
"reconnected channel was not required by the next epoch");
}
}
static void channel_join_does_not_expand_active_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 0u, 0x0fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"join scenario did not start");
CHECK(gps_nav_epoch_assembly_push(&assembly, 4000u, 4u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"joining channel changed the active epoch");
CHECK(assembly.expected_mask == 0x0fu,
"active expected mask expanded after a channel joined");
for(unsigned int channel = 1u ; channel < 4u ; channel++)
{
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 4000u, channel, 0x1fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 3u),
"original ready set did not complete after a join");
}
return gps_nav_epoch_assembly_push(
assembly, capture_counter, local_ticks, channel, ready_mask,
CAPTURE_PERIOD_TICKS, CAPTURE_WINDOW_TICKS,
HOLDOFF_LOCAL_TICKS);
}
static uint32_t deterministic_random(uint32_t* state)
@@ -202,122 +55,144 @@ static uint32_t deterministic_random(uint32_t* state)
return value;
}
static void randomized_interrupt_and_unlock_model(void)
static void local_capture_assembly_completes(void)
{
uint32_t random_state = 0x6a09e667u;
for(unsigned int trial = 0u ; trial < 10000u ; trial++)
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0xafu;
const unsigned int order[] = {7u, 0u, 5u, 3u, 2u, 1u};
const uint64_t base = 810102900000ULL;
for(unsigned int i = 0u ; i < sizeof(order) / sizeof(order[0]) ; i++)
{
unsigned int channel_count =
4u + (deterministic_random(&random_state) % 6u);
uint32_t ready_mask = (1u << channel_count) - 1u;
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
for(unsigned int i = 0u ; i < channel_count ; i++)
{
order[i] = i;
}
for(unsigned int i = channel_count ; i > 1u ; i--)
{
unsigned int swap =
deterministic_random(&random_state) % i;
unsigned int temporary = order[i - 1u];
order[i - 1u] = order[swap];
order[swap] = temporary;
}
gps_nav_epoch_event_result_t result =
push_capture(&assembly, base + (uint64_t)i * 250000u,
100000u + i, order[i], ready_mask);
bool last = i + 1u == sizeof(order) / sizeof(order[0]);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) == last,
"coherent local capture group completed at the wrong event");
}
CHECK(assembly.expected_mask == ready_mask,
"assembly changed its frozen ready mask");
CHECK(assembly.received_mask == ready_mask,
"assembly lost a ready-channel capture");
CHECK((assembly.maximum_capture_counter -
assembly.minimum_capture_counter) <= CAPTURE_WINDOW_TICKS,
"coherent group exceeded its capture span");
}
bool disconnect =
(deterministic_random(&random_state) & 3u) == 0u;
unsigned int dropped_channel = disconnect ?
(deterministic_random(&random_state) % channel_count) :
GPS_NAV_EPOCH_MAX_CANDIDATES;
uint32_t final_mask = ready_mask;
if(disconnect)
{
final_mask &= ~(1u << dropped_channel);
}
static void missing_channel_expires_as_usable_subset(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0x3fu;
const uint64_t base = 900000000ULL;
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
bool complete = false;
for(unsigned int i = 0u ; i < channel_count ; i++)
{
unsigned int channel = order[i];
if(channel == dropped_channel)
{
complete = gps_nav_epoch_assembly_remove_channel(
&assembly, channel);
continue;
}
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 5000u + trial, channel, ready_mask);
if((deterministic_random(&random_state) & 7u) == 0u)
{
CHECK(gps_nav_epoch_assembly_push(
&assembly, 5000u + trial, channel,
ready_mask) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"random duplicate interrupt was not idempotent");
}
complete = result == GPS_NAV_EPOCH_EVENT_COMPLETE;
}
if(disconnect &&
((assembly.expected_mask & (1u << dropped_channel)) != 0u))
{
complete = gps_nav_epoch_assembly_remove_channel(
&assembly, dropped_channel);
}
CHECK(mask_count(final_mask) < 4u ||
(complete && gps_nav_epoch_assembly_complete(&assembly)),
"random interrupt/unlock sequence did not complete");
CHECK(assembly.expected_mask == final_mask,
"random unlock changed the wrong expected channel");
CHECK(assembly.received_mask == final_mask,
"random model lost or invented an interrupt");
for(unsigned int channel = 0u ; channel < 5u ; channel++)
{
CHECK(push_capture(&assembly,
base + (uint64_t)channel * 100000u,
200000u + channel,
channel, ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"partial epoch rejected a coherent channel");
}
CHECK(!gps_nav_epoch_assembly_complete(&assembly),
"missing ready channel completed before holdoff");
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"five-channel partial epoch was not usable");
CHECK(!gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks - 1u),
"epoch expired before its deadline");
CHECK(gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks),
"epoch did not expire at its deadline");
}
static void local_outlier_cannot_poison_epoch(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint32_t ready_mask = 0x1fu;
const uint64_t base = 1000000000ULL;
CHECK(push_capture(&assembly, base, 300000u, 0u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"outlier scenario did not start");
CHECK(push_capture(&assembly, base + 6000000u, 300001u,
1u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_OUTLIER,
"ambiguous local timestamp entered the active group");
CHECK((assembly.received_mask == 0x01u) &&
(assembly.minimum_capture_counter == base) &&
(assembly.maximum_capture_counter == base),
"local outlier mutated the active group");
CHECK(push_capture(&assembly, base - CAPTURE_PERIOD_TICKS,
300002u, 2u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_STALE,
"delayed previous epoch was not rejected");
CHECK(push_capture(&assembly, base + CAPTURE_PERIOD_TICKS,
300003u, 1u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"new local epoch did not replace the incomplete group");
CHECK((assembly.first_capture_counter ==
base + CAPTURE_PERIOD_TICKS) &&
(assembly.received_mask == 0x02u),
"new epoch retained state from the old group");
}
static void duplicate_join_and_disconnect_are_bounded(void)
{
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
const uint64_t base = 1100000000ULL;
CHECK(push_capture(&assembly, base, 400000u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"membership scenario did not start");
CHECK(push_capture(&assembly, base, 400001u, 0u, 0x1fu) ==
GPS_NAV_EPOCH_EVENT_DUPLICATE,
"duplicate capture was not idempotent");
CHECK(push_capture(&assembly, base + 100000u, 400002u,
5u, 0x3fu) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"joining channel expanded the active epoch");
CHECK(!gps_nav_epoch_assembly_remove_channel(&assembly, 4u),
"disconnect completed a one-channel group");
for(unsigned int channel = 1u ; channel < 4u ; channel++)
{
gps_nav_epoch_event_result_t result =
push_capture(&assembly,
base + (uint64_t)channel * 100000u,
400010u + channel, channel, 0x0fu);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == 3u),
"remaining frozen membership completed incorrectly");
}
}
static void multi_epoch_hardware_interrupt_model(void)
static void randomized_capture_liveness(void)
{
gps_nav_epoch_assembly_t assembly;
uint32_t random_state = 0xbb67ae85u;
uint32_t ready_mask = 0x7fu;
unsigned int completed_epochs = 0u;
unsigned int dropped_epochs = 0u;
unsigned int joined_epochs = 0u;
for(unsigned int epoch = 0u ; epoch < 2000u ; epoch++)
uint32_t random_state = 0x6a09e667u;
uint64_t base = 2000000000ULL;
for(unsigned int trial = 0u ; trial < 100000u ; trial++)
{
bool drop_before_interrupt = (epoch == 250u);
bool drop_after_interrupt = (epoch == 900u);
bool join_during_epoch = (epoch == 500u) || (epoch == 1200u);
unsigned int changing_channel =
(epoch == 1200u) ? 6u : 5u;
if(epoch == 400u)
{
ready_mask |= 1u << 2;
}
if(epoch == 1050u)
{
ready_mask |= 1u << 4;
}
if(join_during_epoch)
{
ready_mask &= ~(1u << changing_channel);
}
uint32_t epoch_ready_mask = ready_mask;
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
unsigned int ready_count =
4u + (deterministic_random(&random_state) % 6u);
uint32_t ready_mask = (1u << ready_count) - 1u;
unsigned int delivered_count =
4u + (deterministic_random(&random_state) %
(ready_count - 3u));
unsigned int order[GPS_NAV_EPOCH_MAX_CANDIDATES];
unsigned int order_count = 0u;
for(unsigned int channel = 0u ;
channel < GPS_NAV_EPOCH_MAX_CANDIDATES ; channel++)
for(unsigned int i = 0u ; i < ready_count ; i++)
{
if((epoch_ready_mask & (1u << channel)) != 0u)
{
order[order_count++] = channel;
}
order[i] = i;
}
for(unsigned int i = order_count ; i > 1u ; i--)
for(unsigned int i = ready_count ; i > 1u ; i--)
{
unsigned int swap =
deterministic_random(&random_state) % i;
@@ -326,76 +201,28 @@ static void multi_epoch_hardware_interrupt_model(void)
order[swap] = temporary;
}
gps_nav_epoch_assembly_reset(&assembly);
bool completed = false;
for(unsigned int event_index = 0u ;
event_index < order_count ; event_index++)
for(unsigned int i = 0u ; i < delivered_count ; i++)
{
unsigned int channel = order[event_index];
if(drop_before_interrupt && (channel == 2u))
{
ready_mask &= ~(1u << channel);
(void)gps_nav_epoch_assembly_remove_channel(
&assembly, channel);
dropped_epochs++;
continue;
}
uint64_t offset =
deterministic_random(&random_state) %
(CAPTURE_WINDOW_TICKS + 1u);
gps_nav_epoch_event_result_t result =
gps_nav_epoch_assembly_push(
&assembly, 100000u + epoch, channel,
epoch_ready_mask);
if(result == GPS_NAV_EPOCH_EVENT_COMPLETE)
{
completed = true;
}
if(drop_after_interrupt && (channel == 4u))
{
ready_mask &= ~(1u << channel);
if(gps_nav_epoch_assembly_remove_channel(
&assembly, channel))
{
completed = true;
}
dropped_epochs++;
}
if(join_during_epoch && (event_index == 0u))
{
ready_mask |= 1u << changing_channel;
CHECK(gps_nav_epoch_assembly_push(
&assembly, 100000u + epoch,
changing_channel, ready_mask) ==
GPS_NAV_EPOCH_EVENT_INVALID,
"mid-epoch connection expanded the frozen mask");
joined_epochs++;
}
if((event_index == 0u) && (epoch != 0u))
{
CHECK(gps_nav_epoch_assembly_push(
&assembly, 99999u + epoch,
channel, epoch_ready_mask) ==
GPS_NAV_EPOCH_EVENT_STALE,
"delayed previous-epoch interrupt was accepted");
}
push_capture(&assembly, base + offset,
500000u + i, order[i], ready_mask);
CHECK((result == GPS_NAV_EPOCH_EVENT_ACCEPTED) ||
(result == GPS_NAV_EPOCH_EVENT_COMPLETE),
"random coherent capture was rejected");
}
CHECK(completed &&
gps_nav_epoch_assembly_complete(&assembly),
"hardware stream did not complete a live epoch");
CHECK(assembly.received_mask == assembly.expected_mask,
"hardware stream completed with a missing interrupt");
completed_epochs++;
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"random stream lost four-channel liveness");
if(delivered_count != ready_count)
{
CHECK(gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks),
"partial random epoch did not expire");
}
base += CAPTURE_PERIOD_TICKS;
}
CHECK(completed_epochs == 2000u,
"hardware stream produced more or fewer than one completion per epoch");
CHECK(dropped_epochs == 2u,
"hardware stream did not exercise both unlock positions");
CHECK(joined_epochs == 2u,
"hardware stream did not exercise both reconnect positions");
}
static void all_coherent_across_sample_wrap(void)
@@ -416,19 +243,17 @@ static void all_coherent_across_sample_wrap(void)
static void tow_outlier_does_not_block_good_cluster(void)
{
gps_nav_epoch_key_t candidates[GPS_NAV_EPOCH_MAX_CANDIDATES] = {
{2300, 100.0000, 1000000u},
{2300, 100.0001, 1010000u},
{2300, 99.9999, 1020000u},
{2300, 100.0002, 1030000u},
{2300, 100.0000, 1040000u},
{2300, 100.0001, 1050000u},
{2300, 100.2000, 1060000u}
{380, 469367.4000, 2982422271u},
{380, 469367.4001, 2982428475u},
{381, 28831.6000, 2982490274u},
{380, 469367.3999, 2982550441u},
{380, 469367.4002, 2982661405u}
};
gps_nav_epoch_selection_t selection = select_epoch(candidates, 7u);
CHECK(selection.selected_count == 6u,
"single TOW outlier blocked the six-satellite cluster");
CHECK(selection.selected_mask == 0x3fu,
"TOW outlier was not excluded");
gps_nav_epoch_selection_t selection = select_epoch(candidates, 5u);
CHECK(selection.selected_count == 4u,
"live 45.6-hour TOW outlier blocked the good cluster");
CHECK(selection.selected_mask == 0x1bu,
"live TOW outlier was not excluded");
}
static void newest_sample_outlier_does_not_define_the_cluster(void)
@@ -486,14 +311,11 @@ static void nonfinite_candidate_is_excluded(void)
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();
randomized_interrupt_and_unlock_model();
multi_epoch_hardware_interrupt_model();
local_capture_assembly_completes();
missing_channel_expires_as_usable_subset();
local_outlier_cannot_poison_epoch();
duplicate_join_and_disconnect_are_bounded();
randomized_capture_liveness();
all_coherent_across_sample_wrap();
tow_outlier_does_not_block_good_cluster();
newest_sample_outlier_does_not_define_the_cluster();
soc/software/gps_system/tests/test_gps_nav_epoch_coalescing.c
+95 -138
View File
@@ -4,15 +4,9 @@
#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;
#define PERIOD_TICKS 25000000ULL
#define WINDOW_TICKS 3125000ULL
#define HOLDOFF_TICKS 15000ULL
static int failures;
@@ -23,152 +17,115 @@ static int 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],
static gps_nav_epoch_event_result_t push(
gps_nav_epoch_assembly_t* assembly,
uint64_t counter,
uint64_t local_ticks,
unsigned int channel,
unsigned int* coalesced_pulses)
uint32_t ready_mask)
{
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);
assembly, counter, local_ticks, channel, ready_mask,
PERIOD_TICKS, WINDOW_TICKS, HOLDOFF_TICKS);
}
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)
static void live_fault_recovery(void)
{
for(unsigned int channel = 0u ; channel < CHANNEL_COUNT ; channel++)
/* Exact topology from the captured board state: channel 6 is locked but
* 11.6 seconds stale; channel 3 carries a transient GPS time 45.6 hours
* ahead. Local capture time still places five measurements in one epoch. */
const uint32_t ready_mask =
(1u << 0) | (1u << 2) | (1u << 3) |
(1u << 4) | (1u << 6) | (1u << 7);
const unsigned int channels[] = {3u, 0u, 2u, 4u, 7u};
const uint64_t captures[] = {
810103886055ULL,
810102907239ULL,
810102954617ULL,
810104733473ULL,
810103426569ULL
};
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
for(unsigned int i = 0u ; i < 5u ; i++)
{
CHECK(push(&assembly, captures[i], 100000u + i,
channels[i], ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"live coherent local capture was rejected");
}
CHECK(assembly.received_mask ==
((1u << 0) | (1u << 2) | (1u << 3) |
(1u << 4) | (1u << 7)),
"live capture mask differs from the expected five channels");
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"live five-channel group was not usable");
CHECK(gps_nav_epoch_assembly_expired(
&assembly, assembly.deadline_local_ticks),
"missing channel did not release the live group at holdoff");
/* A physically later local group advances normally regardless of whatever
* week/TOW any channel decoded. */
uint32_t old_mask = assembly.received_mask;
CHECK(push(&assembly, captures[0] + PERIOD_TICKS,
200000u, 3u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"next physical epoch did not advance after the live fault");
CHECK(old_mask != 0u && assembly.received_mask == (1u << 3),
"next epoch mixed measurements from the previous group");
}
static void overwritten_mailbox_remains_bounded(void)
{
const uint32_t ready_mask = 0x3fu;
gps_nav_epoch_assembly_t assembly;
gps_nav_epoch_assembly_reset(&assembly);
uint64_t first_epoch = 900000000000ULL;
/* Channel 4's first mailbox value is overwritten. The other five still
* form a usable bounded snapshot instead of waiting indefinitely. */
for(unsigned int channel = 0u ; channel < 6u ; channel++)
{
if(channel == 4u)
{
continue;
}
CHECK(push(&assembly, first_epoch + channel * 100000u,
300000u + channel, channel, ready_mask) ==
GPS_NAV_EPOCH_EVENT_ACCEPTED,
"partial mailbox epoch rejected");
}
CHECK(gps_nav_epoch_assembly_usable(&assembly, 4u),
"mailbox overwrite blocked the usable subset");
uint32_t previous_mask = assembly.received_mask;
CHECK(push(&assembly, first_epoch + PERIOD_TICKS,
400000u, 0u, ready_mask) ==
GPS_NAV_EPOCH_EVENT_RESTARTED,
"new mailbox epoch did not advance");
CHECK(previous_mask == 0x2fu,
"partial mailbox mask was not preserved for publication");
for(unsigned int channel = 1u ; channel < 6u ; channel++)
{
gps_nav_epoch_event_result_t result =
service_channel(channels, serviced_sequence, assembly,
channel, coalesced_pulses);
push(&assembly,
first_epoch + PERIOD_TICKS + channel * 100000u,
400000u + channel, channel, ready_mask);
CHECK((result == GPS_NAV_EPOCH_EVENT_COMPLETE) ==
(channel == CHANNEL_COUNT - 1u),
"healthy epoch did not complete on its final channel");
(channel == 5u),
"healthy mailbox epoch completed incorrectly");
}
}
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");
live_fault_recovery();
overwritten_mailbox_remains_bounded();
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);
puts("gps_nav_epoch coalescing/fault tests passed");
}
return failures == 0 ? 0 : 1;
}
soc/software/gps_system/tests/test_gps_time_anchor.c
+114
View File
@@ -0,0 +1,114 @@
#include "gps/gps_time_anchor.h"
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#define SAMPLE_RATE_HZ 16368000.0
static int failures;
#define CHECK(condition, message) do { \
if(!(condition)) { \
fprintf(stderr, "FAIL: %s\n", message); \
failures++; \
} \
} while(0)
static void normal_and_skipped_subframes_are_consistent(void)
{
uint32_t first = 1000000000u;
uint32_t six_seconds = (uint32_t)(SAMPLE_RATE_HZ * 6.0);
CHECK(gps_time_anchors_consistent(
380, 469362.0, first, (double)first,
380, 469368.0, first + six_seconds,
(double)(first + six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"normal six-second HOW progression was rejected");
CHECK(gps_time_anchors_consistent(
380, 469362.0, first, (double)first,
380, 469380.0, first + 3u * six_seconds,
(double)(first + 3u * six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"skipped subframes broke HOW continuity");
}
static void week_rollover_is_consistent(void)
{
uint32_t first = 2000000000u;
uint32_t six_seconds = (uint32_t)(SAMPLE_RATE_HZ * 6.0);
CHECK(gps_time_anchors_consistent(
380, 604794.0, first, (double)first,
381, 0.0, first + six_seconds,
(double)(first + six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"valid GPS week rollover was rejected");
CHECK(gps_time_extend_lnav_week(true, 1023, 0u) == 1024,
"LNAV week rollover was not extended monotonically");
CHECK(gps_time_extend_lnav_week(true, 1024, 1023u) == 1023,
"previous modulo week was not extended backward");
CHECK(gps_time_extend_lnav_week(false, 0, 380u) == 380,
"initial LNAV week was altered");
}
static void temporary_week_can_upgrade_on_first_subframe_one(void)
{
uint32_t first = 2500000000u;
uint32_t six_seconds = (uint32_t)(SAMPLE_RATE_HZ * 6.0);
CHECK(gps_time_anchors_consistent(
0, 470940.0, first, (double)first,
0, 470946.0, first + six_seconds,
(double)(first + six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"temporary-week TOW continuity was rejected");
CHECK(gps_time_extend_lnav_week(true, 0, 380u) == 380,
"first SF1 could not upgrade the temporary week");
}
static void captured_future_jump_is_rejected_and_recovers(void)
{
uint32_t first = 3000000000u;
uint32_t six_seconds = (uint32_t)(SAMPLE_RATE_HZ * 6.0);
CHECK(!gps_time_anchors_consistent(
380, 469362.0, first, (double)first,
381, 28830.0, first + six_seconds,
(double)(first + six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"captured 45.6-hour HOW jump was accepted");
/* Two consecutive candidates on the replacement timeline agree, allowing
* the channel-level pending-anchor logic to recover without a reset. */
CHECK(gps_time_anchors_consistent(
380, 469368.0, first + six_seconds,
(double)(first + six_seconds),
380, 469374.0, first + 2u * six_seconds,
(double)(first + 2u * six_seconds),
SAMPLE_RATE_HZ, 120.0, 0.050),
"two good HOW candidates could not establish recovery");
}
static void invalid_inputs_are_rejected(void)
{
CHECK(!gps_time_anchors_consistent(
380, NAN, 1u, 1.0, 380, 6.0, 2u, 2.0,
SAMPLE_RATE_HZ, 120.0, 0.050),
"non-finite HOW was accepted");
CHECK(!gps_time_anchors_consistent(
380, 0.0, 1u, 1.0, 380, 604800.0, 2u, 2.0,
SAMPLE_RATE_HZ, 120.0, 0.050),
"out-of-range HOW was accepted");
}
int main(void)
{
normal_and_skipped_subframes_are_consistent();
week_rollover_is_consistent();
temporary_week_can_upgrade_on_first_subframe_one();
captured_future_jump_is_rejected_and_recovers();
invalid_inputs_are_rejected();
if(failures == 0)
{
puts("gps_time_anchor tests passed");
}
return failures == 0 ? 0 : 1;
}